1# coding: utf8
2
3# This file is part of Scapy
4# Scapy is free software: you can redistribute it and/or modify
5# it under the terms of the GNU General Public License as published by
6# the Free Software Foundation, either version 2 of the License, or
7# any later version.
8#
9# Scapy is distributed in the hope that it will be useful,
10# but WITHOUT ANY WARRANTY; without even the implied warranty of
11# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12# GNU General Public License for more details.
13#
14# You should have received a copy of the GNU General Public License
15# along with Scapy. If not, see <http://www.gnu.org/licenses/>.
16
17# scapy.contrib.description = ModBus Protocol
18# scapy.contrib.status = loads
19
20# Copyright (C) 2017 Arthur Gervais, Ken LE PRADO, Sébastien Mainand,
21# Thomas Aurel
22
23import struct
24
25from scapy.packet import Packet, bind_layers
26from scapy.fields import XByteField, XShortField, StrLenField, ByteEnumField, \
27    BitFieldLenField, ByteField, ConditionalField, EnumField, FieldListField, \
28    ShortField, StrFixedLenField, XShortEnumField
29from scapy.layers.inet import TCP
30from scapy.utils import orb
31from scapy.config import conf
32from scapy.volatile import VolatileValue
33
34
35_modbus_exceptions = {1: "Illegal Function Code",
36                      2: "Illegal Data Address",
37                      3: "Illegal Data Value",
38                      4: "Server Device Failure",
39                      5: "Acknowledge",
40                      6: "Server Device Busy",
41                      8: "Memory Parity Error",
42                      10: "Gateway Path Unavailable",
43                      11: "Gateway Target Device Failed to Respond"}
44
45
46class _ModbusPDUNoPayload(Packet):
47
48    def extract_padding(self, s):
49        return b"", None
50
51
52class ModbusPDU01ReadCoilsRequest(_ModbusPDUNoPayload):
53    name = "Read Coils Request"
54    fields_desc = [XByteField("funcCode", 0x01),
55                   XShortField("startAddr", 0x0000),  # 0x0000 to 0xFFFF
56                   XShortField("quantity", 0x0001)]
57
58
59class ModbusPDU01ReadCoilsResponse(_ModbusPDUNoPayload):
60    name = "Read Coils Response"
61    fields_desc = [XByteField("funcCode", 0x01),
62                   BitFieldLenField("byteCount", None, 8,
63                                    count_of="coilStatus"),
64                   FieldListField("coilStatus", [0x00], ByteField("", 0x00),
65                                  count_from=lambda pkt: pkt.byteCount)]
66
67
68class ModbusPDU01ReadCoilsError(_ModbusPDUNoPayload):
69    name = "Read Coils Exception"
70    fields_desc = [XByteField("funcCode", 0x81),
71                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
72
73
74class ModbusPDU02ReadDiscreteInputsRequest(_ModbusPDUNoPayload):
75    name = "Read Discrete Inputs"
76    fields_desc = [XByteField("funcCode", 0x02),
77                   XShortField("startAddr", 0x0000),
78                   XShortField("quantity", 0x0001)]
79
80
81class ModbusPDU02ReadDiscreteInputsResponse(Packet):
82    """ inputStatus: result is represented as bytes, padded with 0 to have a
83        integer number of bytes. The field does not parse this result and
84        present the bytes directly
85    """
86    name = "Read Discrete Inputs Response"
87    fields_desc = [XByteField("funcCode", 0x02),
88                   BitFieldLenField("byteCount", None, 8,
89                                    count_of="inputStatus"),
90                   FieldListField("inputStatus", [0x00], ByteField("", 0x00),
91                                  count_from=lambda pkt: pkt.byteCount)]
92
93
94class ModbusPDU02ReadDiscreteInputsError(Packet):
95    name = "Read Discrete Inputs Exception"
96    fields_desc = [XByteField("funcCode", 0x82),
97                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
98
99
100class ModbusPDU03ReadHoldingRegistersRequest(_ModbusPDUNoPayload):
101    name = "Read Holding Registers"
102    fields_desc = [XByteField("funcCode", 0x03),
103                   XShortField("startAddr", 0x0000),
104                   XShortField("quantity", 0x0001)]
105
106
107class ModbusPDU03ReadHoldingRegistersResponse(Packet):
108    name = "Read Holding Registers Response"
109    fields_desc = [XByteField("funcCode", 0x03),
110                   BitFieldLenField("byteCount", None, 8,
111                                    count_of="registerVal",
112                                    adjust=lambda pkt, x: x * 2),
113                   FieldListField("registerVal", [0x0000],
114                                  ShortField("", 0x0000),
115                                  count_from=lambda pkt: pkt.byteCount)]
116
117
118class ModbusPDU03ReadHoldingRegistersError(Packet):
119    name = "Read Holding Registers Exception"
120    fields_desc = [XByteField("funcCode", 0x83),
121                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
122
123
124class ModbusPDU04ReadInputRegistersRequest(_ModbusPDUNoPayload):
125    name = "Read Input Registers"
126    fields_desc = [XByteField("funcCode", 0x04),
127                   XShortField("startAddr", 0x0000),
128                   XShortField("quantity", 0x0001)]
129
130
131class ModbusPDU04ReadInputRegistersResponse(Packet):
132    name = "Read Input Registers Response"
133    fields_desc = [XByteField("funcCode", 0x04),
134                   BitFieldLenField("byteCount", None, 8,
135                                    count_of="registerVal",
136                                    adjust=lambda pkt, x: x * 2),
137                   FieldListField("registerVal", [0x0000],
138                                  ShortField("", 0x0000),
139                                  count_from=lambda pkt: pkt.byteCount)]
140
141
142class ModbusPDU04ReadInputRegistersError(Packet):
143    name = "Read Input Registers Exception"
144    fields_desc = [XByteField("funcCode", 0x84),
145                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
146
147
148class ModbusPDU05WriteSingleCoilRequest(Packet):
149    name = "Write Single Coil"
150    fields_desc = [XByteField("funcCode", 0x05),
151                   # from 0x0000 to 0xFFFF
152                   XShortField("outputAddr", 0x0000),
153                   # 0x0000: Off, 0xFF00: On
154                   XShortField("outputValue", 0x0000)]
155
156
157class ModbusPDU05WriteSingleCoilResponse(Packet):
158    # The answer is the same as the request if successful
159    name = "Write Single Coil"
160    fields_desc = [XByteField("funcCode", 0x05),
161                   # from 0x0000 to 0xFFFF
162                   XShortField("outputAddr", 0x0000),
163                   # 0x0000 == Off, 0xFF00 == On
164                   XShortField("outputValue", 0x0000)]
165
166
167class ModbusPDU05WriteSingleCoilError(Packet):
168    name = "Write Single Coil Exception"
169    fields_desc = [XByteField("funcCode", 0x85),
170                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
171
172
173class ModbusPDU06WriteSingleRegisterRequest(_ModbusPDUNoPayload):
174    name = "Write Single Register"
175    fields_desc = [XByteField("funcCode", 0x06),
176                   XShortField("registerAddr", 0x0000),
177                   XShortField("registerValue", 0x0000)]
178
179
180class ModbusPDU06WriteSingleRegisterResponse(Packet):
181    name = "Write Single Register Response"
182    fields_desc = [XByteField("funcCode", 0x06),
183                   XShortField("registerAddr", 0x0000),
184                   XShortField("registerValue", 0x0000)]
185
186
187class ModbusPDU06WriteSingleRegisterError(Packet):
188    name = "Write Single Register Exception"
189    fields_desc = [XByteField("funcCode", 0x86),
190                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
191
192
193class ModbusPDU07ReadExceptionStatusRequest(_ModbusPDUNoPayload):
194    name = "Read Exception Status"
195    fields_desc = [XByteField("funcCode", 0x07)]
196
197
198class ModbusPDU07ReadExceptionStatusResponse(Packet):
199    name = "Read Exception Status Response"
200    fields_desc = [XByteField("funcCode", 0x07),
201                   XByteField("startAddr", 0x00)]
202
203
204class ModbusPDU07ReadExceptionStatusError(Packet):
205    name = "Read Exception Status Exception"
206    fields_desc = [XByteField("funcCode", 0x87),
207                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
208
209
210_diagnostics_sub_function = {
211    0x0000: "Return Query Data",
212    0x0001: "Restart Communications Option",
213    0x0002: "Return Diagnostic Register",
214    0x0003: "Change ASCII Input Delimiter",
215    0x0004: "Force Listen Only Mode",
216    0x000A: "Clear Counters and Diagnostic Register",
217    0x000B: "Return Bus Message Count",
218    0x000C: "Return Bus Communication Error Count",
219    0x000D: "Return Bus Exception Error Count",
220    0x000E: "Return Slave Message Count",
221    0x000F: "Return Slave No Response Count",
222    0x0010: "Return Slave NAK Count",
223    0x0011: "Return Slave Busy Count",
224    0x0012: "Return Bus Character Overrun Count",
225    0x0014: "Clear Overrun Counter and Flag"
226}
227
228
229class ModbusPDU08DiagnosticsRequest(_ModbusPDUNoPayload):
230    name = "Diagnostics"
231    fields_desc = [XByteField("funcCode", 0x08),
232                   XShortEnumField("subFunc", 0x0000,
233                                   _diagnostics_sub_function),
234                   FieldListField("data", [0x0000], XShortField("", 0x0000))]
235
236
237class ModbusPDU08DiagnosticsResponse(_ModbusPDUNoPayload):
238    name = "Diagnostics Response"
239    fields_desc = [XByteField("funcCode", 0x08),
240                   XShortEnumField("subFunc", 0x0000,
241                                   _diagnostics_sub_function),
242                   FieldListField("data", [0x0000], XShortField("", 0x0000))]
243
244
245class ModbusPDU08DiagnosticsError(_ModbusPDUNoPayload):
246    name = "Diagnostics Exception"
247    fields_desc = [XByteField("funcCode", 0x88),
248                   ByteEnumField("exceptionCode", 1, _modbus_exceptions)]
249
250
251class ModbusPDU0BGetCommEventCounterRequest(_ModbusPDUNoPayload):
252    name = "Get Comm Event Counter"
253    fields_desc = [XByteField("funcCode", 0x0B)]
254
255
256class ModbusPDU0BGetCommEventCounterResponse(_ModbusPDUNoPayload):
257    name = "Get Comm Event Counter Response"
258    fields_desc = [XByteField("funcCode", 0x0B),
259                   XShortField("status", 0x0000),
260                   XShortField("eventCount", 0xFFFF)]
261
262
263class ModbusPDU0BGetCommEventCounterError(_ModbusPDUNoPayload):
264    name = "Get Comm Event Counter Exception"
265    fields_desc = [XByteField("funcCode", 0x8B),
266                   ByteEnumField("exceptionCode", 1, _modbus_exceptions)]
267
268
269class ModbusPDU0CGetCommEventLogRequest(_ModbusPDUNoPayload):
270    name = "Get Comm Event Log"
271    fields_desc = [XByteField("funcCode", 0x0C)]
272
273
274class ModbusPDU0CGetCommEventLogResponse(_ModbusPDUNoPayload):
275    name = "Get Comm Event Log Response"
276    fields_desc = [XByteField("funcCode", 0x0C),
277                   ByteField("byteCount", 8),
278                   XShortField("status", 0x0000),
279                   XShortField("eventCount", 0x0108),
280                   XShortField("messageCount", 0x0121),
281                   FieldListField("event", [0x20, 0x00], XByteField("", 0x00))]
282
283
284class ModbusPDU0CGetCommEventLogError(_ModbusPDUNoPayload):
285    name = "Get Comm Event Log Exception"
286    fields_desc = [XByteField("funcCode", 0x8C),
287                   XByteField("exceptionCode", 1)]
288
289
290class ModbusPDU0FWriteMultipleCoilsRequest(Packet):
291    name = "Write Multiple Coils"
292    fields_desc = [XByteField("funcCode", 0x0F),
293                   XShortField("startAddr", 0x0000),
294                   XShortField("quantityOutput", 0x0001),
295                   BitFieldLenField("byteCount", None, 8,
296                                    count_of="outputsValue"),
297                   FieldListField("outputsValue", [0x00], XByteField("", 0x00),
298                                  count_from=lambda pkt: pkt.byteCount)]
299
300
301class ModbusPDU0FWriteMultipleCoilsResponse(Packet):
302    name = "Write Multiple Coils Response"
303    fields_desc = [XByteField("funcCode", 0x0F),
304                   XShortField("startAddr", 0x0000),
305                   XShortField("quantityOutput", 0x0001)]
306
307
308class ModbusPDU0FWriteMultipleCoilsError(Packet):
309    name = "Write Multiple Coils Exception"
310    fields_desc = [XByteField("funcCode", 0x8F),
311                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
312
313
314class ModbusPDU10WriteMultipleRegistersRequest(Packet):
315    name = "Write Multiple Registers"
316    fields_desc = [XByteField("funcCode", 0x10),
317                   XShortField("startAddr", 0x0000),
318                   BitFieldLenField("quantityRegisters", None, 16,
319                                    count_of="outputsValue"),
320                   BitFieldLenField("byteCount", None, 8,
321                                    count_of="outputsValue",
322                                    adjust=lambda pkt, x: x * 2),
323                   FieldListField("outputsValue", [0x0000],
324                                  XShortField("", 0x0000),
325                                  count_from=lambda pkt: pkt.byteCount)]
326
327
328class ModbusPDU10WriteMultipleRegistersResponse(Packet):
329    name = "Write Multiple Registers Response"
330    fields_desc = [XByteField("funcCode", 0x10),
331                   XShortField("startAddr", 0x0000),
332                   XShortField("quantityRegisters", 0x0001)]
333
334
335class ModbusPDU10WriteMultipleRegistersError(Packet):
336    name = "Write Multiple Registers Exception"
337    fields_desc = [XByteField("funcCode", 0x90),
338                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
339
340
341class ModbusPDU11ReportSlaveIdRequest(_ModbusPDUNoPayload):
342    name = "Report Slave Id"
343    fields_desc = [XByteField("funcCode", 0x11)]
344
345
346class ModbusPDU11ReportSlaveIdResponse(Packet):
347    name = "Report Slave Id Response"
348    fields_desc = [
349        XByteField("funcCode", 0x11),
350        BitFieldLenField("byteCount", None, 8, length_of="slaveId"),
351        ConditionalField(StrLenField("slaveId", "",
352                                     length_from=lambda pkt: pkt.byteCount),
353                         lambda pkt: pkt.byteCount > 0),
354        ConditionalField(XByteField("runIdicatorStatus", 0x00),
355                         lambda pkt: pkt.byteCount > 0),
356    ]
357
358
359class ModbusPDU11ReportSlaveIdError(Packet):
360    name = "Report Slave Id Exception"
361    fields_desc = [XByteField("funcCode", 0x91),
362                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
363
364
365class ModbusReadFileSubRequest(Packet):
366    name = "Sub-request of Read File Record"
367    fields_desc = [ByteField("refType", 0x06),
368                   ShortField("fileNumber", 0x0001),
369                   ShortField("recordNumber", 0x0000),
370                   ShortField("recordLength", 0x0001)]
371
372    def guess_payload_class(self, payload):
373        return ModbusReadFileSubRequest
374
375
376class ModbusPDU14ReadFileRecordRequest(Packet):
377    name = "Read File Record"
378    fields_desc = [XByteField("funcCode", 0x14),
379                   ByteField("byteCount", None)]
380
381    def guess_payload_class(self, payload):
382        if self.byteCount > 0:
383            return ModbusReadFileSubRequest
384        else:
385            return Packet.guess_payload_class(self, payload)
386
387    def post_build(self, p, pay):
388        if self.byteCount is None:
389            tmp_len = len(pay)
390            p = p[:1] + struct.pack("!B", tmp_len) + p[3:]
391        return p + pay
392
393
394class ModbusReadFileSubResponse(Packet):
395    name = "Sub-response"
396    fields_desc = [
397        BitFieldLenField("respLength", None, 8, count_of="recData",
398                         adjust=lambda pkt, p: p * 2 + 1),
399        ByteField("refType", 0x06),
400        FieldListField("recData", [0x0000], XShortField("", 0x0000),
401                       count_from=lambda pkt: (pkt.respLength - 1) // 2),
402    ]
403
404    def guess_payload_class(self, payload):
405        return ModbusReadFileSubResponse
406
407
408class ModbusPDU14ReadFileRecordResponse(Packet):
409    name = "Read File Record Response"
410    fields_desc = [XByteField("funcCode", 0x14),
411                   ByteField("dataLength", None)]
412
413    def post_build(self, p, pay):
414        if self.dataLength is None:
415            tmp_len = len(pay)
416            p = p[:1] + struct.pack("!B", tmp_len) + p[3:]
417        return p + pay
418
419    def guess_payload_class(self, payload):
420        if self.dataLength > 0:
421            return ModbusReadFileSubResponse
422        else:
423            return Packet.guess_payload_class(self, payload)
424
425
426class ModbusPDU14ReadFileRecordError(Packet):
427    name = "Read File Record Exception"
428    fields_desc = [XByteField("funcCode", 0x94),
429                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
430
431
432# 0x15 : Write File Record
433class ModbusWriteFileSubRequest(Packet):
434    name = "Sub request of Write File Record"
435    fields_desc = [
436        ByteField("refType", 0x06),
437        ShortField("fileNumber", 0x0001),
438        ShortField("recordNumber", 0x0000),
439        BitFieldLenField("recordLength", None, 16,
440                         length_of="recordData",
441                         adjust=lambda pkt, p: p // 2),
442        FieldListField("recordData", [0x0000],
443                       ShortField("", 0x0000),
444                       length_from=lambda pkt: pkt.recordLength * 2),
445    ]
446
447    def guess_payload_class(self, payload):
448        if payload:
449            return ModbusWriteFileSubRequest
450
451
452class ModbusPDU15WriteFileRecordRequest(Packet):
453    name = "Write File Record"
454    fields_desc = [XByteField("funcCode", 0x15),
455                   ByteField("dataLength", None)]
456
457    def post_build(self, p, pay):
458        if self.dataLength is None:
459            tmp_len = len(pay)
460            p = p[:1] + struct.pack("!B", tmp_len) + p[3:]
461            return p + pay
462
463    def guess_payload_class(self, payload):
464        if self.dataLength > 0:
465            return ModbusWriteFileSubRequest
466        else:
467            return Packet.guess_payload_class(self, payload)
468
469
470class ModbusWriteFileSubResponse(ModbusWriteFileSubRequest):
471    name = "Sub response of Write File Record"
472
473    def guess_payload_class(self, payload):
474        if payload:
475            return ModbusWriteFileSubResponse
476
477
478class ModbusPDU15WriteFileRecordResponse(ModbusPDU15WriteFileRecordRequest):
479    name = "Write File Record Response"
480
481    def guess_payload_class(self, payload):
482        if self.dataLength > 0:
483            return ModbusWriteFileSubResponse
484        else:
485            return Packet.guess_payload_class(self, payload)
486
487
488class ModbusPDU15WriteFileRecordError(Packet):
489    name = "Write File Record Exception"
490    fields_desc = [XByteField("funcCode", 0x95),
491                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
492
493
494class ModbusPDU16MaskWriteRegisterRequest(Packet):
495    # and/or to 0xFFFF/0x0000 so that nothing is changed in memory
496    name = "Mask Write Register"
497    fields_desc = [XByteField("funcCode", 0x16),
498                   XShortField("refAddr", 0x0000),
499                   XShortField("andMask", 0xffff),
500                   XShortField("orMask", 0x0000)]
501
502
503class ModbusPDU16MaskWriteRegisterResponse(Packet):
504    name = "Mask Write Register Response"
505    fields_desc = [XByteField("funcCode", 0x16),
506                   XShortField("refAddr", 0x0000),
507                   XShortField("andMask", 0xffff),
508                   XShortField("orMask", 0x0000)]
509
510
511class ModbusPDU16MaskWriteRegisterError(Packet):
512    name = "Mask Write Register Exception"
513    fields_desc = [XByteField("funcCode", 0x96),
514                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
515
516
517class ModbusPDU17ReadWriteMultipleRegistersRequest(Packet):
518    name = "Read Write Multiple Registers"
519    fields_desc = [XByteField("funcCode", 0x17),
520                   XShortField("readStartingAddr", 0x0000),
521                   XShortField("readQuantityRegisters", 0x0001),
522                   XShortField("writeStartingAddr", 0x0000),
523                   BitFieldLenField("writeQuantityRegisters", None, 16,
524                                    count_of="writeRegistersValue"),
525                   BitFieldLenField("byteCount", None, 8,
526                                    count_of="writeRegistersValue",
527                                    adjust=lambda pkt, x: x * 2),
528                   FieldListField("writeRegistersValue", [0x0000],
529                                  XShortField("", 0x0000),
530                                  count_from=lambda pkt: pkt.byteCount)]
531
532
533class ModbusPDU17ReadWriteMultipleRegistersResponse(Packet):
534    name = "Read Write Multiple Registers Response"
535    fields_desc = [XByteField("funcCode", 0x17),
536                   BitFieldLenField("byteCount", None, 8,
537                                    count_of="registerVal",
538                                    adjust=lambda pkt, x: x * 2),
539                   FieldListField("registerVal", [0x0000],
540                                  ShortField("", 0x0000),
541                                  count_from=lambda pkt: pkt.byteCount)]
542
543
544class ModbusPDU17ReadWriteMultipleRegistersError(Packet):
545    name = "Read Write Multiple Exception"
546    fields_desc = [XByteField("funcCode", 0x97),
547                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
548
549
550class ModbusPDU18ReadFIFOQueueRequest(Packet):
551    name = "Read FIFO Queue"
552    fields_desc = [XByteField("funcCode", 0x18),
553                   XShortField("FIFOPointerAddr", 0x0000)]
554
555
556class ModbusPDU18ReadFIFOQueueResponse(Packet):
557    name = "Read FIFO Queue Response"
558    fields_desc = [XByteField("funcCode", 0x18),
559                   # TODO: ByteCount must includes size of FIFOCount
560                   BitFieldLenField("byteCount", None, 16, count_of="FIFOVal",
561                                    adjust=lambda pkt, p: p * 2 + 2),
562                   BitFieldLenField("FIFOCount", None, 16, count_of="FIFOVal"),
563                   FieldListField("FIFOVal", [], ShortField("", 0x0000),
564                                  count_from=lambda pkt: pkt.byteCount)]
565
566
567class ModbusPDU18ReadFIFOQueueError(Packet):
568    name = "Read FIFO Queue Exception"
569    fields_desc = [XByteField("funcCode", 0x98),
570                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
571
572
573# TODO: not implemented, out of the main specification
574# class ModbusPDU2B0DCANOpenGeneralReferenceRequest(Packet):
575#     name = "CANopen General Reference Request"
576#     fields_desc = []
577#
578#
579# class ModbusPDU2B0DCANOpenGeneralReferenceResponse(Packet):
580#     name = "CANopen General Reference Response"
581#     fields_desc = []
582#
583#
584# class ModbusPDU2B0DCANOpenGeneralReferenceError(Packet):
585#     name = "CANopen General Reference Error"
586#     fields_desc = []
587
588
589# 0x2B/0x0E - Read Device Identification values
590_read_device_id_codes = {1: "Basic",
591                         2: "Regular",
592                         3: "Extended",
593                         4: "Specific"}
594# 0x00->0x02: mandatory
595# 0x03->0x06: optional
596# 0x07->0x7F: Reserved (optional)
597# 0x80->0xFF: product dependent private objects (optional)
598_read_device_id_object_id = {0x00: "VendorName",
599                             0x01: "ProductCode",
600                             0x02: "MajorMinorRevision",
601                             0x03: "VendorUrl",
602                             0x04: "ProductName",
603                             0x05: "ModelName",
604                             0x06: "UserApplicationName"}
605_read_device_id_conformity_lvl = {
606    0x01: "Basic Identification (stream only)",
607    0x02: "Regular Identification (stream only)",
608    0x03: "Extended Identification (stream only)",
609    0x81: "Basic Identification (stream and individual access)",
610    0x82: "Regular Identification (stream and individual access)",
611    0x83: "Extended Identification (stream and individual access)",
612}
613_read_device_id_more_follow = {0x00: "No",
614                               0x01: "Yes"}
615
616
617class ModbusPDU2B0EReadDeviceIdentificationRequest(Packet):
618    name = "Read Device Identification"
619    fields_desc = [XByteField("funcCode", 0x2B),
620                   XByteField("MEIType", 0x0E),
621                   ByteEnumField("readCode", 1, _read_device_id_codes),
622                   ByteEnumField("objectId", 0x00, _read_device_id_object_id)]
623
624
625class ModbusPDU2B0EReadDeviceIdentificationResponse(Packet):
626    name = "Read Device Identification"
627    fields_desc = [XByteField("funcCode", 0x2B),
628                   XByteField("MEIType", 0x0E),
629                   ByteEnumField("readCode", 4, _read_device_id_codes),
630                   ByteEnumField("conformityLevel", 0x01,
631                                 _read_device_id_conformity_lvl),
632                   ByteEnumField("more", 0x00, _read_device_id_more_follow),
633                   ByteEnumField("nextObjId", 0x00, _read_device_id_object_id),
634                   ByteField("objCount", 0x00)]
635
636    def guess_payload_class(self, payload):
637        if self.objCount > 0:
638            return ModbusObjectId
639        else:
640            return Packet.guess_payload_class(self, payload)
641
642
643class ModbusPDU2B0EReadDeviceIdentificationError(Packet):
644    name = "Read Exception Status Exception"
645    fields_desc = [XByteField("funcCode", 0xAB),
646                   ByteEnumField("exceptCode", 1, _modbus_exceptions)]
647
648
649_reserved_funccode_request = {
650    0x09: '0x09 Unknown Reserved Request',
651    0x0A: '0x0a Unknown Reserved Request',
652    0x0D: '0x0d Unknown Reserved Request',
653    0x0E: '0x0e Unknown Reserved Request',
654    0x29: '0x29 Unknown Reserved Request',
655    0x2A: '0x2a Unknown Reserved Request',
656    0x5A: 'Specific Schneider Electric Request',
657    0x5B: '0x5b Unknown Reserved Request',
658    0x7D: '0x7d Unknown Reserved Request',
659    0x7E: '0x7e Unknown Reserved Request',
660    0x7F: '0x7f Unknown Reserved Request',
661}
662
663_reserved_funccode_response = {
664    0x09: '0x09 Unknown Reserved Response',
665    0x0A: '0x0a Unknown Reserved Response',
666    0x0D: '0x0d Unknown Reserved Response',
667    0x0E: '0x0e Unknown Reserved Response',
668    0x29: '0x29 Unknown Reserved Response',
669    0x2A: '0x2a Unknown Reserved Response',
670    0x5A: 'Specific Schneider Electric Response',
671    0x5B: '0x5b Unknown Reserved Response',
672    0x7D: '0x7d Unknown Reserved Response',
673    0x7E: '0x7e Unknown Reserved Response',
674    0x7F: '0x7f Unknown Reserved Response',
675}
676
677_reserved_funccode_error = {
678    0x89: '0x89 Unknown Reserved Error',
679    0x8A: '0x8a Unknown Reserved Error',
680    0x8D: '0x8d Unknown Reserved Error',
681    0x8E: '0x8e Unknown Reserved Error',
682    0xA9: '0x88 Unknown Reserved Error',
683    0xAA: '0x88 Unknown Reserved Error',
684    0xDA: 'Specific Schneider Electric Error',
685    0xDB: '0xdb Unknown Reserved Error',
686    0xDC: '0xdc Unknown Reserved Error',
687    0xFD: '0xfd Unknown Reserved Error',
688    0xFE: '0xfe Unknown Reserved Error',
689    0xFF: '0xff Unknown Reserved Error',
690}
691
692
693class ModbusPDUReservedFunctionCodeRequest(_ModbusPDUNoPayload):
694    name = "Reserved Function Code Request"
695    fields_desc = [
696        ByteEnumField("funcCode", 0x00, _reserved_funccode_request),
697        StrFixedLenField('payload', '', 255), ]
698
699    def mysummary(self):
700        return self.sprintf("Modbus Reserved Request %funcCode%")
701
702
703class ModbusPDUReservedFunctionCodeResponse(_ModbusPDUNoPayload):
704    name = "Reserved Function Code Response"
705    fields_desc = [
706        ByteEnumField("funcCode", 0x00, _reserved_funccode_response),
707        StrFixedLenField('payload', '', 255), ]
708
709    def mysummary(self):
710        return self.sprintf("Modbus Reserved Response %funcCode%")
711
712
713class ModbusPDUReservedFunctionCodeError(_ModbusPDUNoPayload):
714    name = "Reserved Function Code Error"
715    fields_desc = [
716        ByteEnumField("funcCode", 0x00, _reserved_funccode_error),
717        StrFixedLenField('payload', '', 255), ]
718
719    def mysummary(self):
720        return self.sprintf("Modbus Reserved Error %funcCode%")
721
722
723_userdefined_funccode_request = {
724}
725_userdefined_funccode_response = {
726}
727_userdefined_funccode_error = {
728}
729
730
731class ModbusByteEnumField(EnumField):
732    __slots__ = "defEnum"
733
734    def __init__(self, name, default, enum, defEnum):
735        EnumField.__init__(self, name, default, enum, "B")
736        self.defEnum = defEnum
737
738    def i2repr_one(self, pkt, x):
739        if self not in conf.noenum and not isinstance(x, VolatileValue) \
740                and x in self.i2s:
741            return self.i2s[x]
742        if self.defEnum:
743            return self.defEnum
744        return repr(x)
745
746
747class ModbusPDUUserDefinedFunctionCodeRequest(_ModbusPDUNoPayload):
748    name = "User-Defined Function Code Request"
749    fields_desc = [
750        ModbusByteEnumField(
751            "funcCode", 0x00, _userdefined_funccode_request,
752            "Unknown user-defined request function Code"),
753        StrFixedLenField('payload', '', 255), ]
754
755    def mysummary(self):
756        return self.sprintf("Modbus User-Defined Request %funcCode%")
757
758
759class ModbusPDUUserDefinedFunctionCodeResponse(_ModbusPDUNoPayload):
760    name = "User-Defined Function Code Response"
761    fields_desc = [
762        ModbusByteEnumField(
763            "funcCode", 0x00, _userdefined_funccode_response,
764            "Unknown user-defined response function Code"),
765        StrFixedLenField('payload', '', 255), ]
766
767    def mysummary(self):
768        return self.sprintf("Modbus User-Defined Response %funcCode%")
769
770
771class ModbusPDUUserDefinedFunctionCodeError(_ModbusPDUNoPayload):
772    name = "User-Defined Function Code Error"
773    fields_desc = [
774        ModbusByteEnumField(
775            "funcCode", 0x00, _userdefined_funccode_error,
776            "Unknown user-defined error function Code"),
777        StrFixedLenField('payload', '', 255), ]
778
779    def mysummary(self):
780        return self.sprintf("Modbus User-Defined Error %funcCode%")
781
782
783class ModbusObjectId(Packet):
784    name = "Object"
785    fields_desc = [ByteEnumField("id", 0x00, _read_device_id_object_id),
786                   BitFieldLenField("length", None, 8, length_of="value"),
787                   StrLenField("value", "",
788                               length_from=lambda pkt: pkt.length)]
789
790    def guess_payload_class(self, payload):
791        return ModbusObjectId
792
793
794_modbus_request_classes = {
795    0x01: ModbusPDU01ReadCoilsRequest,
796    0x02: ModbusPDU02ReadDiscreteInputsRequest,
797    0x03: ModbusPDU03ReadHoldingRegistersRequest,
798    0x04: ModbusPDU04ReadInputRegistersRequest,
799    0x05: ModbusPDU05WriteSingleCoilRequest,
800    0x06: ModbusPDU06WriteSingleRegisterRequest,
801    0x07: ModbusPDU07ReadExceptionStatusRequest,
802    0x08: ModbusPDU08DiagnosticsRequest,
803    0x0B: ModbusPDU0BGetCommEventCounterRequest,
804    0x0C: ModbusPDU0CGetCommEventLogRequest,
805    0x0F: ModbusPDU0FWriteMultipleCoilsRequest,
806    0x10: ModbusPDU10WriteMultipleRegistersRequest,
807    0x11: ModbusPDU11ReportSlaveIdRequest,
808    0x14: ModbusPDU14ReadFileRecordRequest,
809    0x15: ModbusPDU15WriteFileRecordRequest,
810    0x16: ModbusPDU16MaskWriteRegisterRequest,
811    0x17: ModbusPDU17ReadWriteMultipleRegistersRequest,
812    0x18: ModbusPDU18ReadFIFOQueueRequest,
813}
814_modbus_error_classes = {
815    0x81: ModbusPDU01ReadCoilsError,
816    0x82: ModbusPDU02ReadDiscreteInputsError,
817    0x83: ModbusPDU03ReadHoldingRegistersError,
818    0x84: ModbusPDU04ReadInputRegistersError,
819    0x85: ModbusPDU05WriteSingleCoilError,
820    0x86: ModbusPDU06WriteSingleRegisterError,
821    0x87: ModbusPDU07ReadExceptionStatusError,
822    0x88: ModbusPDU08DiagnosticsError,
823    0x8B: ModbusPDU0BGetCommEventCounterError,
824    0x0C: ModbusPDU0CGetCommEventLogError,
825    0x8F: ModbusPDU0FWriteMultipleCoilsError,
826    0x90: ModbusPDU10WriteMultipleRegistersError,
827    0x91: ModbusPDU11ReportSlaveIdError,
828    0x94: ModbusPDU14ReadFileRecordError,
829    0x95: ModbusPDU15WriteFileRecordError,
830    0x96: ModbusPDU16MaskWriteRegisterError,
831    0x97: ModbusPDU17ReadWriteMultipleRegistersError,
832    0x98: ModbusPDU18ReadFIFOQueueError,
833    0xAB: ModbusPDU2B0EReadDeviceIdentificationError,
834}
835_modbus_response_classes = {
836    0x01: ModbusPDU01ReadCoilsResponse,
837    0x02: ModbusPDU02ReadDiscreteInputsResponse,
838    0x03: ModbusPDU03ReadHoldingRegistersResponse,
839    0x04: ModbusPDU04ReadInputRegistersResponse,
840    0x05: ModbusPDU05WriteSingleCoilResponse,
841    0x06: ModbusPDU06WriteSingleRegisterResponse,
842    0x07: ModbusPDU07ReadExceptionStatusResponse,
843    0x88: ModbusPDU08DiagnosticsResponse,
844    0x8B: ModbusPDU0BGetCommEventCounterRequest,
845    0x0C: ModbusPDU0CGetCommEventLogResponse,
846    0x0F: ModbusPDU0FWriteMultipleCoilsResponse,
847    0x10: ModbusPDU10WriteMultipleRegistersResponse,
848    0x11: ModbusPDU11ReportSlaveIdResponse,
849    0x14: ModbusPDU14ReadFileRecordResponse,
850    0x15: ModbusPDU15WriteFileRecordResponse,
851    0x16: ModbusPDU16MaskWriteRegisterResponse,
852    0x17: ModbusPDU17ReadWriteMultipleRegistersResponse,
853    0x18: ModbusPDU18ReadFIFOQueueResponse,
854}
855_mei_types_request = {
856    0x0E: ModbusPDU2B0EReadDeviceIdentificationRequest,
857    # 0x0D: ModbusPDU2B0DCANOpenGeneralReferenceRequest,
858}
859_mei_types_response = {
860    0x0E: ModbusPDU2B0EReadDeviceIdentificationResponse,
861    # 0x0D: ModbusPDU2B0DCANOpenGeneralReferenceResponse,
862}
863
864
865class ModbusADURequest(Packet):
866    name = "ModbusADU"
867    fields_desc = [
868        # needs to be unique
869        XShortField("transId", 0x0000),
870        # needs to be zero (Modbus)
871        XShortField("protoId", 0x0000),
872        # is calculated with payload
873        ShortField("len", None),
874        # 0xFF (recommended as non-significant value) or 0x00
875        XByteField("unitId", 0xff),
876    ]
877
878    def guess_payload_class(self, payload):
879        function_code = orb(payload[0])
880
881        if function_code == 0x2B:
882            sub_code = orb(payload[1])
883            try:
884                return _mei_types_request[sub_code]
885            except KeyError:
886                pass
887        try:
888            return _modbus_request_classes[function_code]
889        except KeyError:
890            pass
891        if function_code in _reserved_funccode_request:
892            return ModbusPDUReservedFunctionCodeRequest
893        return ModbusPDUUserDefinedFunctionCodeRequest
894
895    def post_build(self, p, pay):
896        if self.len is None:
897            tmp_len = len(pay) + 1  # +len(p)
898            p = p[:4] + struct.pack("!H", tmp_len) + p[6:]
899        return p + pay
900
901
902class ModbusADUResponse(Packet):
903    name = "ModbusADU"
904    fields_desc = [
905        # needs to be unique
906        XShortField("transId", 0x0000),
907        # needs to be zero (Modbus)
908        XShortField("protoId", 0x0000),
909        # is calculated with payload
910        ShortField("len", None),
911        # 0xFF or 0x00 should be used for Modbus over TCP/IP
912        XByteField("unitId", 0xff),
913    ]
914
915    def guess_payload_class(self, payload):
916        function_code = orb(payload[0])
917
918        if function_code == 0x2B:
919            sub_code = orb(payload[1])
920            try:
921                return _mei_types_response[sub_code]
922            except KeyError:
923                pass
924        try:
925            return _modbus_response_classes[function_code]
926        except KeyError:
927            pass
928        try:
929            return _modbus_error_classes[function_code]
930        except KeyError:
931            pass
932        if function_code in _reserved_funccode_response:
933            return ModbusPDUReservedFunctionCodeResponse
934        elif function_code in _reserved_funccode_error:
935            return ModbusPDUReservedFunctionCodeError
936        if function_code < 0x80:
937            return ModbusPDUUserDefinedFunctionCodeResponse
938        return ModbusPDUUserDefinedFunctionCodeError
939
940    def post_build(self, p, pay):
941        if self.len is None:
942            tmp_len = len(pay) + 1  # +len(p)
943            p = p[:4] + struct.pack("!H", tmp_len) + p[6:]
944        return p + pay
945
946
947bind_layers(TCP, ModbusADURequest, dport=502)
948bind_layers(TCP, ModbusADUResponse, sport=502)
949