From b3b6f28401c7e475df7c01ac5b14f07246a14d3b Mon Sep 17 00:00:00 2001 From: Rob Watson Date: Sat, 16 Jul 2022 22:39:26 +0200 Subject: [PATCH] modbus: Validate response --- command/failurecode_string.go | 33 +++++++++++++++ command/modbus.go | 76 +++++++++++++++++++++++++++++++++-- command/modbus_test.go | 63 +++++++++++++++++++++++++++++ 3 files changed, 168 insertions(+), 4 deletions(-) create mode 100644 command/failurecode_string.go create mode 100644 command/modbus_test.go diff --git a/command/failurecode_string.go b/command/failurecode_string.go new file mode 100644 index 0000000..e2e3633 --- /dev/null +++ b/command/failurecode_string.go @@ -0,0 +1,33 @@ +// Code generated by "stringer -type=FailureCode"; DO NOT EDIT. + +package command + +import "strconv" + +func _() { + // An "invalid array index" compiler error signifies that the constant values have changed. + // Re-run the stringer command to generate them again. + var x [1]struct{} + _ = x[FailureCodeIllegalFunction-1] + _ = x[FailureCodeIllegalDataAddress-2] + _ = x[FailureCodeIllegalDataValue-3] + _ = x[FailureCodeSlaveDeviceFailure-4] + _ = x[FailureCodeAcknowledge-5] + _ = x[FailureCodeSlaveDeviceBusy-6] + _ = x[FailureCodeNegativeAcknowledgement-7] + _ = x[FailureCodeMemoryParityError-8] + _ = x[FailureCodeGatewayPathUnavailable-9] + _ = x[FailureCodeGatewayTargetDeviceFailedToRespond-10] +} + +const _FailureCode_name = "FailureCodeIllegalFunctionFailureCodeIllegalDataAddressFailureCodeIllegalDataValueFailureCodeSlaveDeviceFailureFailureCodeAcknowledgeFailureCodeSlaveDeviceBusyFailureCodeNegativeAcknowledgementFailureCodeMemoryParityErrorFailureCodeGatewayPathUnavailableFailureCodeGatewayTargetDeviceFailedToRespond" + +var _FailureCode_index = [...]uint16{0, 26, 55, 82, 111, 133, 159, 193, 221, 254, 299} + +func (i FailureCode) String() string { + i -= 1 + if i >= FailureCode(len(_FailureCode_index)-1) { + return "FailureCode(" + strconv.FormatInt(int64(i+1), 10) + ")" + } + return _FailureCode_name[_FailureCode_index[i]:_FailureCode_index[i+1]] +} diff --git a/command/modbus.go b/command/modbus.go index d6a4515..11b79eb 100644 --- a/command/modbus.go +++ b/command/modbus.go @@ -1,5 +1,28 @@ package command +//go:generate stringer -type=FailureCode + +import ( + "encoding/binary" + "errors" + "fmt" +) + +type FailureCode byte + +const ( + FailureCodeIllegalFunction FailureCode = iota + 1 + FailureCodeIllegalDataAddress + FailureCodeIllegalDataValue + FailureCodeSlaveDeviceFailure + FailureCodeAcknowledge + FailureCodeSlaveDeviceBusy + FailureCodeNegativeAcknowledgement + FailureCodeMemoryParityError + FailureCodeGatewayPathUnavailable + FailureCodeGatewayTargetDeviceFailedToRespond +) + var modbusCrcTable []uint16 func init() { @@ -19,7 +42,10 @@ func init() { } type ModbusCommand struct { - payload []byte + payload []byte + commandType ModbusCommandType + offset uint16 + value uint16 } const modbusComAddr byte = 0xf7 @@ -27,7 +53,8 @@ const modbusComAddr byte = 0xf7 type ModbusCommandType byte const ( - ModbusCommandTypeRead ModbusCommandType = 0x03 + ModbusCommandTypeRead ModbusCommandType = 0x03 + // TODO: implement write commands. ModbusCommandTypeWrite ModbusCommandType = 0x06 ModbusCommandTypeWriteMulti ModbusCommandType = 0x10 ) @@ -45,7 +72,12 @@ func NewModbus(commandType ModbusCommandType, offset uint16, value uint16) *Modb p = append(p, byte(sum&0xff)) p = append(p, byte((sum>>8)&0xff)) - return &ModbusCommand{payload: p} + return &ModbusCommand{ + payload: p, + commandType: commandType, + offset: offset, + value: value, + } } func modbusChecksum(b []byte) uint16 { @@ -58,6 +90,42 @@ func modbusChecksum(b []byte) uint16 { func (cmd ModbusCommand) String() string { return string(cmd.payload) } +// ValidateResponse validates the entire response and if valid returns the +// response body. func (cmd ModbusCommand) ValidateResponse(p []byte) ([]byte, error) { - return p[5 : len(p)-2], nil + if len(p) < 4 { + return nil, errors.New("invalid response: response too short") + } + + var expectedLen int + cmdType := ModbusCommandType(p[3]) + + switch cmdType { + case ModbusCommandTypeRead: + if uint16(p[4]) != cmd.value*2 { + return nil, fmt.Errorf("short response: expected %d, got %d", p[4], cmd.value*2) + } + expectedLen = int(p[4]) + 7 + if len(p) < expectedLen { + return nil, fmt.Errorf("invalid read length: expected %d, got %d", expectedLen, len(p)) + } + case ModbusCommandTypeWrite, ModbusCommandTypeWriteMulti: + panic("unimplemented") + default: + expectedLen = len(p) + } + + offset := expectedLen - 2 + wantSum := modbusChecksum(p[2:offset]) + gotSum := binary.LittleEndian.Uint16(p[offset:]) + if wantSum != gotSum { + return nil, fmt.Errorf("invalid CRC-16: want `%X`, got `%X`", wantSum, gotSum) + } + + if p[3] != byte(cmd.commandType) { + failureCode := FailureCode(p[4]) + return nil, fmt.Errorf("command failed with code: %d, error: %s", failureCode, failureCode.String()) + } + + return p[5:offset], nil } diff --git a/command/modbus_test.go b/command/modbus_test.go new file mode 100644 index 0000000..0dda97c --- /dev/null +++ b/command/modbus_test.go @@ -0,0 +1,63 @@ +package command_test + +import ( + "testing" + + "git.netflux.io/rob/solar-toolkit/command" + "github.com/stretchr/testify/require" +) + +func TestModbusValidateResponse(t *testing.T) { + validResponse := []byte{170, 85, 247, 3, 250, 22, 7, 15, 17, 46, 24, 11, 243, 0, 83, 0, 0, 9, 243, 9, 53, 0, 41, 0, 0, 3, 198, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 2, 9, 114, 0, 144, 19, 136, 0, 0, 13, 182, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 0, 1, 0, 0, 13, 182, 0, 0, 11, 153, 127, 255, 255, 255, 255, 255, 255, 255, 9, 112, 0, 5, 19, 136, 0, 1, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 29, 128, 0, 0, 0, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 29, 0, 2, 2, 169, 127, 255, 2, 20, 1, 0, 14, 181, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 33, 0, 1, 255, 255, 0, 0, 0, 0, 0, 0, 34, 145, 0, 0, 1, 100, 0, 0, 34, 139, 0, 0, 1, 52, 1, 96, 0, 0, 0, 0, 0, 0, 0, 0, 13, 73, 0, 126, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 2, 8, 64, 71, 0, 3, 0, 0, 255, 255, 147, 214} + validResponseWithExtraBytes := []byte{170, 85, 247, 3, 250, 22, 7, 15, 17, 46, 24, 11, 243, 0, 83, 0, 0, 9, 243, 9, 53, 0, 41, 0, 0, 3, 198, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 2, 9, 114, 0, 144, 19, 136, 0, 0, 13, 182, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 0, 1, 0, 0, 13, 182, 0, 0, 11, 153, 127, 255, 255, 255, 255, 255, 255, 255, 9, 112, 0, 5, 19, 136, 0, 1, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 29, 128, 0, 0, 0, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 29, 0, 2, 2, 169, 127, 255, 2, 20, 1, 0, 14, 181, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 33, 0, 1, 255, 255, 0, 0, 0, 0, 0, 0, 34, 145, 0, 0, 1, 100, 0, 0, 34, 139, 0, 0, 1, 52, 1, 96, 0, 0, 0, 0, 0, 0, 0, 0, 13, 73, 0, 126, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 2, 8, 64, 71, 0, 3, 0, 0, 255, 255, 147, 214, 1, 2, 3} + invalidReadLengthResponse := []byte{170, 85, 247, 3, 250, 22, 7, 15, 17, 46, 24, 243, 0, 83, 0, 0, 9, 243, 9, 53, 0, 41, 0, 0, 3, 198, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 2, 9, 114, 0, 144, 19, 136, 0, 0, 13, 182, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 0, 1, 0, 0, 13, 182, 0, 0, 11, 153, 127, 255, 255, 255, 255, 255, 255, 255, 9, 112, 0, 5, 19, 136, 0, 1, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 29, 128, 0, 0, 0, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 29, 0, 2, 2, 169, 127, 255, 2, 20, 1, 0, 14, 181, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 33, 0, 1, 255, 255, 0, 0, 0, 0, 0, 0, 34, 145, 0, 0, 1, 100, 0, 0, 34, 139, 0, 0, 1, 52, 1, 96, 0, 0, 0, 0, 0, 0, 0, 0, 13, 73, 0, 126, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 2, 8, 64, 71, 0, 3, 0, 0, 255, 255, 147, 213} + invalidChecksumResponse := []byte{170, 85, 247, 3, 250, 22, 7, 15, 17, 46, 24, 11, 243, 0, 83, 0, 0, 9, 243, 9, 53, 0, 41, 0, 0, 3, 198, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 2, 9, 114, 0, 144, 19, 136, 0, 0, 13, 182, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 0, 1, 0, 0, 13, 182, 0, 0, 11, 153, 127, 255, 255, 255, 255, 255, 255, 255, 9, 112, 0, 5, 19, 136, 0, 1, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 2, 29, 128, 0, 0, 0, 128, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 29, 0, 2, 2, 169, 127, 255, 2, 20, 1, 0, 14, 181, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 33, 0, 1, 255, 255, 0, 0, 0, 0, 0, 0, 34, 145, 0, 0, 1, 100, 0, 0, 34, 139, 0, 0, 1, 52, 1, 96, 0, 0, 0, 0, 0, 0, 0, 0, 13, 73, 0, 126, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 2, 8, 64, 71, 0, 3, 0, 0, 255, 255, 147, 213} + failureCodeResponse := []byte{170, 85, 0, 0, 1, 176} + + testCases := []struct { + name string + resp []byte + wantErr string + }{ + { + name: "empty response", + wantErr: "invalid response: response too short", + }, + { + name: "invalid read length", + resp: invalidReadLengthResponse, + wantErr: "invalid read length: expected 257, got 256", + }, + { + name: "invalid checksum", + resp: invalidChecksumResponse, + wantErr: "invalid CRC-16: want `D693`, got `D593`", + }, + { + name: "failure code rcvd", + resp: failureCodeResponse, + wantErr: "command failed with code: 1, error: FailureCodeIllegalFunction", + }, + { + name: "valid response with extra bytes", + resp: validResponseWithExtraBytes, + }, + { + name: "valid response", + resp: validResponse, + }, + } + + for _, tc := range testCases { + t.Run(tc.name, func(t *testing.T) { + cmd := command.NewModbus(command.ModbusCommandTypeRead, 0x891c, 0x007d) + _, err := cmd.ValidateResponse(tc.resp) + + if tc.wantErr == "" { + require.NoError(t, err) + } else { + require.EqualError(t, err, tc.wantErr) + } + }) + } +}