From 6951f53738eaf788b2bdd472bc92183df0bc9bfc Mon Sep 17 00:00:00 2001
From: Kris Watts <kristopher.watts@sinkholesoftware.com>
Date: Thu, 2 Nov 2017 01:01:15 -0600
Subject: [PATCH] adding in modbus decoder, still need to add in the tests

---
 layers/layertypes.go |  1 +
 layers/modbus.go     | 70 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+)
 create mode 100644 layers/modbus.go

diff --git a/layers/layertypes.go b/layers/layertypes.go
index faffb91ad..73a48c145 100644
--- a/layers/layertypes.go
+++ b/layers/layertypes.go
@@ -125,6 +125,7 @@ var (
 	LayerTypeSTP                         = gopacket.RegisterLayerType(121, gopacket.LayerTypeMetadata{Name: "STP", Decoder: gopacket.DecodeFunc(decodeSTP)})
 	LayerTypeBFD                         = gopacket.RegisterLayerType(122, gopacket.LayerTypeMetadata{Name: "BFD", Decoder: gopacket.DecodeFunc(decodeBFD)})
 	LayerTypeOSPF                        = gopacket.RegisterLayerType(123, gopacket.LayerTypeMetadata{Name: "OSPF", Decoder: gopacket.DecodeFunc(decodeOSPF)})
+	LayerTypeModbus                      = gopacket.RegisterLayerType(124, gopacket.LayerTypeMetadata{Name: "Modbus", Decoder: gopacket.DecodeFunc(decodeModbus)})
 )
 
 var (
diff --git a/layers/modbus.go b/layers/modbus.go
new file mode 100644
index 000000000..6eeb67926
--- /dev/null
+++ b/layers/modbus.go
@@ -0,0 +1,70 @@
+// Copyright 2017 Google, Inc. All rights reserved.
+//
+// Use of this source code is governed by a BSD-style license
+// that can be found in the LICENSE file in the root of the source
+// tree.
+
+package layers
+
+import (
+	"encoding/binary"
+	"errors"
+
+	"github.com/google/gopacket"
+)
+
+const (
+	MBAPHeaderLen int = 7
+	MinModbusPacketLen int = MBAPHeaderLen+1
+)
+
+var (
+	ErrDataTooSmall = errors.New("Data too small for Modbus")
+)
+
+type MBAP struct {
+	TransactionID uint16
+	ProtocolID    uint16
+	Length        uint16
+	UnitID        uint8
+}
+
+type Modbus struct {
+	BaseLayer
+	MBAP
+	FunctionCode uint8
+	Payload      []byte
+}
+
+func (m *Modbus) DecodeFromBytes(data []byte, df gopacket.DecodeFeedback) error {
+	if len(data) < MinModbusPacketLen {
+		return ErrDataTooSmall
+	}
+	m.TransactionID = binary.BigEndian.Uint16(data[0:2])
+	m.ProtocolID = binary.BigEndian.Uint16(data[2:4])
+	m.Length = binary.BigEndian.Uint16(data[4:6])
+	m.UnitID = data[6]
+	m.FunctionCode = data[7]
+	m.Payload = data[7:]
+	return nil
+}
+
+func (m *Modbus) LayerType() gopacket.LayerType {
+	return LayerTypeModbus
+}
+
+func (m *Modbus) NextLayerType() gopacket.LayerType {
+	return gopacket.LayerTypeZero
+}
+
+func (m *Modbus) CanDecode() gopacket.LayerClass {
+	return LayerTypeModbus
+}
+
+func decodeModbus(data []byte, p gopacket.PacketBuilder) error {
+	if len(data) < MinModbusPacketLen {
+		return ErrDataTooSmall
+	}
+	modbus := &Modbus{}
+	return decodingLayerDecoder(modbus, data, p)
+}