modbus: Validate response

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]]
+}

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
 }

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)
+			}
+		})
+	}
+}