#1754 Add Modbus Support (#1823)

Author: yahyayozo

Packet++/CMakeLists.txt

@@ -30,6 +30,7 @@ add_library(
   src/Layer.cpp
   src/LdapLayer.cpp
   src/LLCLayer.cpp
+  src/ModbusLayer.cpp
   src/MplsLayer.cpp
   src/NdpLayer.cpp
   src/NflogLayer.cpp

Packet++/header/ModbusLayer.h

@@ -0,0 +1,164 @@
+#pragma once
+
+#include "Layer.h"
+
+/// @file
+/// This file contains classes for parsing, creating and editing Modbus packets.
+
+/// @namespace pcpp
+/// @brief The main namespace for the PcapPlusPlus lib
+namespace pcpp
+{
+
+#pragma pack(push, 1)
+	/// @struct modbus_header
+	/// MODBUS Application Protocol header
+	struct modbus_header
+	{
+		/// For synchronization between messages of server and client
+		uint16_t transactionId;
+		/// 0 for Modbus/TCP
+		uint16_t protocolId;
+		/// Number of remaining bytes in this frame starting from the unit id
+		uint16_t length;
+		/// Unit identifier
+		uint8_t unitId;
+		/// Function code
+		uint8_t functionCode;
+	};
+#pragma pack(pop)
+	static_assert(sizeof(modbus_header) == 8, "modbus_header size is not 8 bytes");
+
+	/// @class ModbusLayer
+	/// Represents the MODBUS Application Protocol layer
+	class ModbusLayer : public Layer
+	{
+	public:
+		/// @brief Enum class representing Modbus function codes.
+		/// This enumeration defines the standard Modbus function codes used in request and response PDUs.
+		/// Each value corresponds to a specific operation defined by the Modbus protocol.
+		enum class ModbusFunctionCode : uint8_t
+		{
+			/// Read coil status (0x01)
+			ReadCoils = 1,
+
+			/// Read discrete input status (0x02)
+			ReadDiscreteInputs = 2,
+
+			/// Read holding registers (0x03)
+			ReadHoldingRegisters = 3,
+
+			/// Read input registers (0x04)
+			ReadInputRegisters = 4,
+
+			/// Write a single coil (0x05)
+			WriteSingleCoil = 5,
+
+			/// Write a single holding register (0x06)
+			WriteSingleHoldingRegister = 6,
+
+			/// Write multiple coils (0x0F)
+			WriteMultipleCoils = 15,
+
+			/// Write multiple holding registers (0x10)
+			WriteMultipleHoldingRegisters = 16,
+
+			/// Report slave ID (0x11)
+			ReadSlaveId = 17,
+
+			/// Unknown or unsupported function code (0xFF)
+			UnknownFunction = 0xFF
+		};
+
+		/// @struct ModbusReadInputRegisters
+		/// Represents a Modbus request to read input registers.
+		struct ModbusReadInputRegisters
+		{
+			uint16_t startingAddress;  ///< Starting address of the input registers to read
+			uint16_t quantity;         ///< Number of input registers to read
+		};
+
+		/// A constructor that creates the layer from an existing packet raw data
+		/// @param[in] data A pointer to the raw data
+		/// @param[in] dataLen Size of the data in bytes
+		/// @param[in] prevLayer A pointer to the previous layer
+		/// @param[in] packet A pointer to the Packet instance where layer will be stored in
+		ModbusLayer(uint8_t* data, size_t dataLen, Layer* prevLayer, Packet* packet)
+		    : Layer(data, dataLen, prevLayer, packet, Modbus)
+		{}
+
+		/// A constructor that creates the layer from user inputs
+		/// @param[in] transactionId Transaction ID
+		/// @param[in] unitId Unit ID
+		ModbusLayer(uint16_t transactionId, uint8_t unitId);
+
+		/// @brief  Check if a port is a valid MODBUS port
+		/// @param port Port number to check
+		/// @note MODBUS uses port 502, so this function checks if the port is equal to 502
+		/// @return true if the port is valid, false otherwise
+		static bool isModbusPort(uint16_t port)
+		{
+			return port == 502;
+		}
+
+		/// @return MODBUS message type
+		uint16_t getTransactionId() const;
+
+		/// @return MODBUS protocol id
+		uint16_t getProtocolId() const;
+
+		/// @return MODBUS remaining bytes in frame starting from the unit id
+		/// @note This is the length of the MODBUS payload + unit_id, not the entire packet
+		uint16_t getLength() const;
+
+		/// @return MODBUS unit id
+		uint8_t getUnitId() const;
+
+		/// @return MODBUS function code
+		ModbusFunctionCode getFunctionCode() const;
+
+		/// @brief set the MODBUS transaction id
+		/// @param transactionId transaction id
+		void setTransactionId(uint16_t transactionId);
+
+		/// @brief set the MODBUS header unit id
+		/// @param unitId unit id
+		void setUnitId(uint8_t unitId);
+
+		/// @brief set the MODBUS header function code
+		/// @param functionCode function code
+		void setFunctionCode(ModbusFunctionCode functionCode);
+
+		// Overridden methods
+
+		/// Does nothing for this layer (ModbusLayer is always last)
+		void parseNextLayer() override
+		{}
+
+		/// @brief Get the length of the MODBUS header
+		/// @return Length of the MODBUS header in bytes
+		size_t getHeaderLen() const override
+		{
+			return sizeof(modbus_header);
+		}
+
+		/// Does nothing for this layer
+		void computeCalculateFields() override
+		{}
+
+		/// @return A string representation of the layer most important data (should look like the layer description in
+		/// Wireshark)
+		std::string toString() const override;
+
+		/// @return The OSI Model layer this protocol belongs to
+		OsiModelLayer getOsiModelLayer() const override
+		{
+			return OsiModelApplicationLayer;
+		}
+
+	private:
+		/// @return A pointer to the MODBUS header
+		modbus_header* getModbusHeader() const;
+	};
+
+}  // namespace pcpp

Packet++/header/ProtocolType.h

@@ -248,6 +248,9 @@ namespace pcpp
 	/// File Transfer Protocol (FTP) Data channel
 	const ProtocolType FTPData = 60;
 
+	/// Modbus protocol
+	const ProtocolType Modbus = 61;
+
 	/// FTP protocol family (FTPControl and FtpData protocols)
 	const ProtocolTypeFamily FTP = 0x3c29;
 

Packet++/src/ModbusLayer.cpp

@@ -0,0 +1,102 @@
+#include "ModbusLayer.h"
+#include "EndianPortable.h"
+#include <iostream>
+#include <iomanip>
+#include <cstring>
+#include "Logger.h"
+
+namespace pcpp
+{
+	ModbusLayer::ModbusLayer(uint16_t transactionId, uint8_t unitId)
+	{
+		const int16_t pduSize = sizeof(ModbusReadInputRegisters);  // Currently only supporting Read Input Registers
+		const size_t headerLen = sizeof(modbus_header);
+
+		m_DataLen = headerLen + pduSize;
+		m_Data = new uint8_t[m_DataLen]{};
+		memset(m_Data, 0, m_DataLen);
+
+		// Initialize the header fields to default values
+		modbus_header* header = getModbusHeader();
+		header->transactionId = htobe16(transactionId);
+		header->protocolId = 0;                 // 0 for Modbus/TCP
+		header->length = htobe16(pduSize + 2);  // Length includes unitId and functionCode
+		header->unitId = unitId;
+		header->functionCode = static_cast<uint8_t>(ModbusFunctionCode::ReadInputRegisters);
+	}
+
+	modbus_header* ModbusLayer::getModbusHeader() const
+	{
+		return (modbus_header*)m_Data;
+	}
+
+	uint16_t ModbusLayer::getTransactionId() const
+	{
+		return be16toh(getModbusHeader()->transactionId);
+	}
+
+	uint16_t ModbusLayer::getProtocolId() const
+	{
+		return be16toh(getModbusHeader()->protocolId);
+	}
+
+	uint16_t ModbusLayer::getLength() const
+	{
+		return be16toh(getModbusHeader()->length);
+	}
+
+	uint8_t ModbusLayer::getUnitId() const
+	{
+		return getModbusHeader()->unitId;
+	}
+
+	ModbusLayer::ModbusFunctionCode ModbusLayer::getFunctionCode() const
+	{
+		switch (getModbusHeader()->functionCode)
+		{
+		case 1:
+			return ModbusFunctionCode::ReadCoils;
+		case 2:
+			return ModbusFunctionCode::ReadDiscreteInputs;
+		case 3:
+			return ModbusFunctionCode::ReadHoldingRegisters;
+		case 4:
+			return ModbusFunctionCode::ReadInputRegisters;
+		case 5:
+			return ModbusFunctionCode::WriteSingleCoil;
+		case 6:
+			return ModbusFunctionCode::WriteSingleHoldingRegister;
+		case 15:
+			return ModbusFunctionCode::WriteMultipleCoils;
+		case 16:
+			return ModbusFunctionCode::WriteMultipleHoldingRegisters;
+		case 17:
+			return ModbusFunctionCode::ReadSlaveId;
+		default:
+			return ModbusFunctionCode::UnknownFunction;
+		}
+	}
+
+	void ModbusLayer::setTransactionId(uint16_t transactionId)
+	{
+		getModbusHeader()->transactionId = htobe16(transactionId);
+	}
+
+	void ModbusLayer::setUnitId(uint8_t unitId)
+	{
+		getModbusHeader()->unitId = unitId;
+	}
+
+	void ModbusLayer::setFunctionCode(ModbusLayer::ModbusFunctionCode functionCode)
+	{
+		getModbusHeader()->functionCode = static_cast<uint8_t>(functionCode);
+	}
+
+	std::string ModbusLayer::toString() const
+	{
+		return "Modbus Layer, Transaction ID: " + std::to_string(getTransactionId()) +
+		       ", Protocol ID: " + std::to_string(getProtocolId()) + ", Length: " + std::to_string(getLength()) +
+		       ", Unit ID: " + std::to_string(getUnitId()) +
+		       ", Function Code: " + std::to_string(static_cast<uint8_t>(getFunctionCode()));
+	}
+}  // namespace pcpp

Packet++/src/TcpLayer.cpp

@@ -19,6 +19,7 @@
 #include "SmtpLayer.h"
 #include "LdapLayer.h"
 #include "GtpLayer.h"
+#include "ModbusLayer.h"
 #include "PacketUtils.h"
 #include "Logger.h"
 #include "DeprecationUtils.h"
@@ -464,6 +465,10 @@ namespace pcpp
 		{
 			constructNextLayer<GtpV2Layer>(payload, payloadLen, m_Packet);
 		}
+		else if (ModbusLayer::isModbusPort(portDst))
+		{
+			constructNextLayer<ModbusLayer>(payload, payloadLen, m_Packet);
+		}
 		else
 		{
 			constructNextLayer<PayloadLayer>(payload, payloadLen, m_Packet);

README.md

@@ -277,16 +277,17 @@ PcapPlusPlus currently supports parsing, editing and creation of packets of the
 42. FTP
 43. HTTP headers (request & response)
 44. LDAP
-45. NTP (v3, v4)
-46. PEM decoder and encoder
-47. Radius
-48. S7 Communication (S7comm)
-49. SMTP
-50. SOME/IP
-51. SSH - parsing only (no editing capabilities)
-52. Telnet - parsing only (no editing capabilities)
-53. X509 certificates - parsing only (no editing capabilities)
-54. Generic payload
+45. Modbus
+46. NTP (v3, v4)
+47. PEM decoder and encoder
+48. Radius
+49. S7 Communication (S7comm)
+50. SMTP
+51. SOME/IP
+52. SSH - parsing only (no editing capabilities)
+53. Telnet - parsing only (no editing capabilities)
+54. X509 certificates - parsing only (no editing capabilities)
+55. Generic payload
 
 ## DPDK And PF_RING Support
 

Tests/Packet++Test/CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(
   Tests/IPv6Tests.cpp
   Tests/LdapTests.cpp
   Tests/LLCTests.cpp
+  Tests/ModbusTests.cpp
   Tests/NflogTests.cpp
   Tests/NtpTests.cpp
   Tests/PacketTests.cpp

Tests/Packet++Test/PacketExamples/ModbusRequest.dat

@@ -0,0 +1 @@
+000c29af7ffe109add4e060d080045000040c6e64000400658880a0101ea0a0a0555c8d301f6e072efbac405c33a8018ffffd5d900000101080a37c3fee900ba2a0f001100000006ff0402580064

Tests/Packet++Test/PacketExamples/modbusFrames.pcap

@@ -318,6 +318,10 @@ PTF_TEST_CASE(CiscoHdlcParsingTest);
 PTF_TEST_CASE(CiscoHdlcLayerCreationTest);
 PTF_TEST_CASE(CiscoHdlcLayerEditTest);
 
+// Implemented in ModbusTests.cpp
+PTF_TEST_CASE(ModbusLayerCreationTest);
+PTF_TEST_CASE(ModbusLayerParsingTest);
+
 // Implemented in X509Tests.cpp
 PTF_TEST_CASE(X509ParsingTest);
 PTF_TEST_CASE(X509VariantsParsingTest);