#1754 Add Modbus Support

Packet++/CMakeLists.txt

@@ -29,6 +29,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,119 @@
+#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:
+		/// 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
+		/// @param[in] functionCode Function code
+		ModbusLayer(uint16_t transactionId, uint8_t unitId, uint8_t functionCode);
+
+		/// @brief  Check if a port is a valid MODBUS port
+		/// @param port
+		/// @return true if the port is valid, false otherwise
+		static bool isModbusPort(uint16_t port)
+		{
+			return port == 502;
+		}
+
+		/// @return A pointer to the MODBUS header
+		modbus_header* getModbusHeader() const;
+
+		/// @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
+		uint8_t 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(uint8_t 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);
+		}
+
+		/// Each layer can compute field values automatically using this method. This is an abstract method
+		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;
+		}
+	};
+
+}  // namespace pcpp

Packet++/header/ProtocolType.h

@@ -220,6 +220,9 @@ namespace pcpp
 	/// Cisco HDLC protocol
 	const ProtocolType CiscoHDLC = 58;
 
+	/// Modbus protocol
+	const ProtocolType Modbus = 59;
+
 	/// An enum representing OSI model layers
 	enum OsiModelLayer
 	{

Packet++/src/ModbusLayer.cpp

@@ -0,0 +1,77 @@
+#include "ModbusLayer.h"
+#include "EndianPortable.h"
+#include <iostream>
+#include <iomanip>
+#include <cstring>
+
+namespace pcpp
+{
+	ModbusLayer::ModbusLayer(uint16_t transactionId, uint8_t unitId, uint8_t functionCode)
+	{
+		const size_t headerLen = sizeof(modbus_header);
+		m_DataLen = headerLen;
+		m_Data = new uint8_t[headerLen];
+		memset(m_Data, 0, headerLen);
+
+		// 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(2);  // minimum length of the MODBUS payload + unit_id
+		header->unitId = unitId;
+		header->functionCode = functionCode;
+	}
+
+	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;
+	}
+
+	uint8_t ModbusLayer::getFunctionCode() const
+	{
+		return getModbusHeader()->functionCode;
+	}
+
+	void ModbusLayer::setTransactionId(uint16_t transactionId)
+	{
+		getModbusHeader()->transactionId = htobe16(transactionId);
+	}
+
+	void ModbusLayer::setUnitId(uint8_t unitId)
+	{
+		getModbusHeader()->unitId = unitId;
+	}
+
+	void ModbusLayer::setFunctionCode(uint8_t functionCode)
+	{
+		getModbusHeader()->functionCode = 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(getFunctionCode());
+	}
+
+}  // namespace pcpp

Packet++/src/TcpLayer.cpp

@@ -18,6 +18,7 @@
 #include "SmtpLayer.h"
 #include "LdapLayer.h"
 #include "GtpLayer.h"
+#include "ModbusLayer.h"
 #include "PacketUtils.h"
 #include "Logger.h"
 #include "DeprecationUtils.h"
@@ -456,6 +457,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);

Tests/Packet++Test/CMakeLists.txt

@@ -21,6 +21,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 @@
+0002b3ce705100207800620d08004500003085a14000800660eb0a0000390a0000030a1201f66197f1e370f1ad6f5018fa9c18f500000000000000020a11

Tests/Packet++Test/TestDefinition.h

@@ -282,6 +282,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);

Tests/Packet++Test/Tests/ModbusTests.cpp

@@ -0,0 +1,53 @@
+#include "../TestDefinition.h"
+#include "../Utils/TestUtils.h"
+#include "Packet.h"
+#include "ModbusLayer.h"
+#include "EndianPortable.h"
+#include "SystemUtils.h"
+
+PTF_TEST_CASE(ModbusLayerCreationTest)
+{
+	pcpp::ModbusLayer modbusLayer(12345, 1, 3);
+
+	PTF_ASSERT_EQUAL(modbusLayer.getTransactionId(), 12345);
+	PTF_ASSERT_EQUAL(modbusLayer.getProtocolId(), 0);
+	PTF_ASSERT_EQUAL(modbusLayer.getLength(), 2);  // minimum length of the MODBUS payload + unit_id
+	PTF_ASSERT_EQUAL(modbusLayer.getUnitId(), 1);
+	PTF_ASSERT_EQUAL(modbusLayer.getFunctionCode(), 3);
+	PTF_ASSERT_EQUAL(modbusLayer.getHeaderLen(), sizeof(pcpp::modbus_header));
+
+	PTF_ASSERT_EQUAL(modbusLayer.getOsiModelLayer(), pcpp::OsiModelApplicationLayer);
+
+	modbusLayer.setTransactionId(54321);
+	PTF_ASSERT_EQUAL(modbusLayer.getTransactionId(), 54321);
+	modbusLayer.setUnitId(2);
+	PTF_ASSERT_EQUAL(modbusLayer.getUnitId(), 2);
+	modbusLayer.setFunctionCode(6);
+	PTF_ASSERT_EQUAL(modbusLayer.getFunctionCode(), 6);
+
+	// just to pass the codecov
+	modbusLayer.computeCalculateFields();
+
+}  // ModbusLayerCreationTest
+
+PTF_TEST_CASE(ModbusLayerParsingTest)
+{
+	timeval time;
+	gettimeofday(&time, nullptr);
+
+	READ_FILE_AND_CREATE_PACKET(1, "PacketExamples/ModbusRequest.dat");
+
+	pcpp::Packet packet(&rawPacket1);
+	PTF_ASSERT_TRUE(packet.isPacketOfType(pcpp::Modbus));
+
+	pcpp::ModbusLayer* modbusLayer = packet.getLayerOfType<pcpp::ModbusLayer>();
+	PTF_ASSERT_NOT_NULL(modbusLayer);
+	PTF_ASSERT_EQUAL(modbusLayer->getTransactionId(), 0);
+	PTF_ASSERT_EQUAL(modbusLayer->getProtocolId(), 0);
+	PTF_ASSERT_EQUAL(modbusLayer->getLength(), 2);
+	PTF_ASSERT_EQUAL(modbusLayer->getUnitId(), 10);
+	PTF_ASSERT_EQUAL(modbusLayer->getFunctionCode(), 17);
+
+	PTF_ASSERT_EQUAL(modbusLayer->toString(),
+	                 "Modbus Layer, Transaction ID: 12345, Protocol ID: 0, Length: 2, Unit ID: 1, Function Code: 3");
+}  // ModbusLayerParsingTest

Tests/Packet++Test/main.cpp

@@ -349,6 +349,9 @@ int main(int argc, char* argv[])
 	PTF_RUN_TEST(CiscoHdlcLayerCreationTest, "chdlc");
 	PTF_RUN_TEST(CiscoHdlcLayerEditTest, "chdlc");
 
+	PTF_RUN_TEST(ModbusLayerCreationTest, "modbus");
+	PTF_RUN_TEST(ModbusLayerParsingTest, "modbus");
+
 	PTF_RUN_TEST(X509ParsingTest, "x509");
 	PTF_RUN_TEST(X509VariantsParsingTest, "x509");
 	PTF_RUN_TEST(X509InvalidDataTest, "x509");