Last active
May 19, 2020 09:47
-
-
Save muhlemmer/67770bdacf853739d76ca59f0925652a to your computer and use it in GitHub Desktop.
MODBUS ADU and PDU decoder; attachment to https://medium.com/@timmhlmann/decoding-modbus-with-golangs-binary-package-4129dd444b09
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
package main | |
import ( | |
"bytes" | |
"encoding/binary" | |
"errors" | |
"fmt" | |
"io" | |
"log" | |
"sync" | |
) | |
// Remember: Big-endian values! | |
var responseADU = []byte{ | |
// MBAP HEADER: | |
0x00, 0xFF, // Tx ID #255, typically an incremental value | |
0x00, 0x00, // Protocol ID, always 0 | |
0x00, 0x0B, // Length of the following data (8+3=11) | |
0x00, // Unit identifier, 0 unless a Gateway is used | |
// PROTOCOL DATA UNIT: | |
0x03, // Function Code 03: Read Holding Registers | |
0x08, // Byte count of the following data (4*2=8) | |
0x00, 0x0A, // First register; value 10 | |
0x0A, 0x00, // Second register; value 2560 | |
0xFF, 0xFF, // Third register; value 65535 or -1 when signed. | |
0x00, 0x01, // Fourth register; value 1 | |
} | |
func printBuffer(b *bytes.Buffer) { | |
fmt.Printf("Buffer :: Length: %d; Values: %# 02X\n", b.Len(), b.Bytes()) | |
} | |
const ( | |
mbapHeaderSize = 7 | |
) | |
func main() { | |
conn := bytes.NewBuffer(responseADU) | |
printBuffer(conn) | |
fmt.Println("\n==========\n ") | |
ReadHoldingRegistersClumsy(conn, binary.BigEndian) | |
fmt.Println("\n==========\n ") | |
conn = bytes.NewBuffer(responseADU) | |
dest := make([]int16, 4) | |
if err := ReadHoldingRegisters(conn, binary.BigEndian, dest); err != nil { | |
log.Fatal(err) | |
} | |
fmt.Printf("[]int16 :: %v\n", dest) | |
fmt.Println("\n==========\n ") | |
conn = bytes.NewBuffer(responseADU) | |
var n int64 | |
if err := ReadHoldingRegisters(conn, binary.BigEndian, &n); err != nil { | |
log.Fatal(err) | |
} | |
fmt.Printf("int64 :: %v\n", n) | |
fmt.Println("\n==========\n ") | |
conn = bytes.NewBuffer(responseADU) | |
var fl float64 | |
if err := ReadHoldingRegisters(conn, binary.BigEndian, &fl); err != nil { | |
log.Fatal(err) | |
} | |
fmt.Printf("float64 :: %v\n", fl) | |
} | |
type mbap struct { | |
TxID uint16 | |
ProtoID uint16 | |
Length uint16 | |
UnitID uint8 | |
} | |
// ReadClumsy a response PDU from the connection. | |
// strips the MBAP header and returns only the PDU | |
func ReadClumsy(conn io.Reader, decoder binary.ByteOrder) ([]byte, error) { | |
bs := make([]byte, mbapHeaderSize) | |
if n, err := conn.Read(bs); err != nil { | |
return nil, err | |
} else if n < mbapHeaderSize { | |
return nil, errors.New("Insufficient header data") | |
} | |
header := mbap{ | |
TxID: decoder.Uint16(bs[:2]), | |
ProtoID: decoder.Uint16(bs[2:4]), | |
Length: decoder.Uint16(bs[4:6]), | |
UnitID: uint8(bs[6]), | |
} | |
// Contents of header at this stage | |
fmt.Printf("Header :: %+v\n", header) | |
// State of the Read buffer | |
printBuffer(conn.(*bytes.Buffer)) | |
// Substract the UnitID field | |
pl := header.Length - 1 | |
bs = make([]byte, pl) | |
// Read the remaining portion | |
if n, err := conn.Read(bs); err != nil { | |
return nil, err | |
} else if n < mbapHeaderSize { | |
return nil, errors.New("Insufficient PDU data") | |
} | |
// Read buffer is now empty | |
printBuffer(conn.(*bytes.Buffer)) | |
// All the values are now in the byte slice | |
fmt.Printf("Resp PDU :: %# 02X\n", bs) | |
return bs, nil | |
} | |
// ReadHoldingRegistersClumsy obtains and decodes a PDU with a function 03 reply format. | |
func ReadHoldingRegistersClumsy(conn io.ReadWriter, decoder binary.ByteOrder) ([]uint16, error) { | |
pdu, err := ReadClumsy(conn, decoder) | |
if err != nil { | |
return nil, err | |
} | |
// First byte is function code | |
if pdu[0] != 0x03 { | |
return nil, fmt.Errorf("Unexpected response Function Code: %# 03x", pdu[0]) | |
} | |
// Second byte is Byte count of the remaining data | |
values := make([]uint16, pdu[1]/2) | |
for i := 0; i < len(values); i++ { | |
// Subslice start at offset 2, for function code and byte count | |
// up to index 4 gives 2 bytes. | |
// Add i*2 to advance 2 bytes for every uint16 iteration. | |
values[i] = decoder.Uint16(pdu[2+i*2 : 4+i*2]) | |
} | |
fmt.Printf("Values :: %d\n", values) | |
return values, nil | |
} | |
// Read a response PDU from the connection and Copy it into dest. | |
func Read(conn io.Reader, order binary.ByteOrder, dest *bytes.Buffer) error { | |
var header mbap | |
if err := binary.Read(conn, order, &header); err != nil { | |
return err | |
} | |
// Contents of header at this stage | |
fmt.Printf("Header :: %+v\n", header) | |
// State of the Read buffer | |
printBuffer(conn.(*bytes.Buffer)) | |
// Substract the UnitID field and Copy the remaining portion | |
if _, err := io.CopyN(dest, conn, int64(header.Length-1)); err != nil { | |
return err | |
} | |
fmt.Printf("Dest PDU :: %# 02X\n", dest.Bytes()) | |
return nil | |
} | |
var bufPool = sync.Pool{ | |
New: func() interface{} { return new(bytes.Buffer) }, | |
} | |
// ReadHoldingRegisters obtains and decodes a PDU with a function 03 reply format. | |
// The result is scanned into dest, which should be a slice of fixed-width types. | |
// See binary.Read() documentation. | |
func ReadHoldingRegisters(conn io.ReadWriter, order binary.ByteOrder, dest interface{}) error { | |
buf := bufPool.Get().(*bytes.Buffer) | |
defer func() { | |
buf.Reset() | |
bufPool.Put(buf) | |
}() | |
if err := Read(conn, order, buf); err != nil { | |
return err | |
} | |
// Discard the Function Code and Byte Count, we don't need them. | |
buf.Next(2) | |
if err := binary.Read(buf, order, dest); err != nil { | |
return fmt.Errorf("Decode PDU: %w", err) | |
} | |
return nil | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment