Repository: evcc-io/evcc
Pull request #12595 (2024-03-02 09:43:57)

Modbus: refactor write encoding
Fix https://github.com/evcc-io/evcc/issues/12444

andig: @MarcusWichelmann könntest Du es mal ausprobieren? Ist leider komplizierter geworden, als ich mir das vorgestellt hatte.

Patch:
diff --git a/provider/modbus.go b/provider/modbus.go
index ccd00b87..fe37a703 100644
--- a/provider/modbus.go
+++ b/provider/modbus.go
@@ -2,7 +2,6 @@ package provider
 
 import (
 	"bytes"
-	"encoding/binary"
 	"fmt"
 	"math"
 	"strings"
@@ -17,7 +16,7 @@ import (
 type Modbus struct {
 	log   *util.Logger
 	conn  *modbus.Connection
-	op    modbus.RegisterOperation
+	reg   modbus.Register
 	scale float64
 }
 
@@ -69,55 +68,58 @@ func NewModbusFromConfig(other map[string]interface{}) (Provider, error) {
 		return nil, err
 	}
 
-	op, err := cc.Register.Operation()
-	if err != nil {
-		return nil, err
-	}
-
 	mb := &Modbus{
 		log:   log,
 		conn:  conn,
-		op:    op,
+		reg:   cc.Register,
 		scale: cc.Scale,
 	}
 	return mb, nil
 }
 
-func (m *Modbus) bytesGetter() ([]byte, error) {
-	switch m.op.FuncCode {
+func (m *Modbus) readBytes(op modbus.RegisterOperation) ([]byte, error) {
+	switch op.FuncCode {
 	case gridx.FuncCodeReadHoldingRegisters:
-		return m.conn.ReadHoldingRegisters(m.op.Addr, m.op.Length)
+		return m.conn.ReadHoldingRegisters(op.Addr, op.Length)
 
 	case gridx.FuncCodeReadInputRegisters:
-		return m.conn.ReadInputRegisters(m.op.Addr, m.op.Length)
+		return m.conn.ReadInputRegisters(op.Addr, op.Length)
 
 	case gridx.FuncCodeReadCoils:
-		return m.conn.ReadCoils(m.op.Addr, m.op.Length)
+		return m.conn.ReadCoils(op.Addr, op.Length)
 
 	default:
-		return nil, fmt.Errorf("invalid read function code: %d", m.op.FuncCode)
+		return nil, fmt.Errorf("invalid read function code: %d", op.FuncCode)
 	}
 }
 
-func (m *Modbus) floatGetter() (f float64, err error) {
-	bytes, err := m.bytesGetter()
+var _ FloatProvider = (*Modbus)(nil)
+
+// FloatGetter implements func() (float64, error)
+func (m *Modbus) FloatGetter() (func() (f float64, err error), error) {
+	op, err := m.reg.Operation()
 	if err != nil {
-		return 0, fmt.Errorf("read failed: %w", err)
+		return nil, err
 	}
 
-	return m.scale * m.op.Decode(bytes), nil
-}
+	decode, err := m.reg.DecodeFunc()
+	if err != nil {
+		return nil, err
+	}
 
-var _ FloatProvider = (*Modbus)(nil)
+	return func() (float64, error) {
+		bytes, err := m.readBytes(op)
+		if err != nil {
+			return 0, fmt.Errorf("read failed: %w", err)
+		}
 
-// FloatGetter executes configured modbus read operation and implements func() (float64, error)
-func (m *Modbus) FloatGetter() (func() (f float64, err error), error) {
-	return m.floatGetter, nil
+		return m.scale * decode(bytes), nil
+	}, nil
 }
 
 var _ IntProvider = (*Modbus)(nil)
 
-// IntGetter executes configured modbus read operation and implements IntProvider
+// IntGetter implements IntProvider
 func (m *Modbus) IntGetter() (func() (int64, error), error) {
 	g, err := m.FloatGetter()
 
@@ -129,10 +131,15 @@ func (m *Modbus) IntGetter() (func() (int64, error), error) {
 
 var _ StringProvider = (*Modbus)(nil)
 
-// StringGetter executes configured modbus read operation and implements IntProvider
+// StringGetter implements StringProvider
 func (m *Modbus) StringGetter() (func() (string, error), error) {
+	op, err := m.reg.Operation()
+	if err != nil {
+		return nil, err
+	}
+
 	return func() (string, error) {
-		b, err := m.bytesGetter()
+		b, err := m.readBytes(op)
 		if err != nil {
 			return "", err
 		}
@@ -143,113 +150,72 @@ func (m *Modbus) StringGetter() (func() (string, error), error) {
 
 var _ BoolProvider = (*Modbus)(nil)
 
-// BoolGetter executes configured modbus read operation and implements IntProvider
+// BoolGetter implements BoolProvider
 func (m *Modbus) BoolGetter() (func() (bool, error), error) {
-	return func() (bool, error) {
-		bytes, err := m.bytesGetter()
-		if err != nil {
-			return false, err
-		}
+	g, err := m.FloatGetter()
 
-		u, err := UintFromBytes(bytes)
-		return u > 0, err
-	}, nil
+	return func() (bool, error) {
+		res, err := g()
+		return res != 0, err
+	}, err
 }
 
-var _ SetFloatProvider = (*Modbus)(nil)
-
-func (m *Modbus) writeMultipleRegisters(val uint64) error {
-	val = m.op.Encode(val)
-
-	var err error
-	switch m.op.Length {
-	case 1:
-		var b [2]byte
-		binary.BigEndian.PutUint16(b[:], uint16(val))
-		_, err = m.conn.WriteMultipleRegisters(m.op.Addr, 1, b[:])
-
-	case 2:
-		var b [4]byte
-		binary.BigEndian.PutUint32(b[:], uint32(val))
-		_, err = m.conn.WriteMultipleRegisters(m.op.Addr, 2, b[:])
-
-	case 4:
-		var b [8]byte
-		binary.BigEndian.PutUint64(b[:], val)
-		_, err = m.conn.WriteMultipleRegisters(m.op.Addr, 4, b[:])
-
-	default:
-		err = fmt.Errorf("invalid write length: %d", m.op.Length)
+func (m *Modbus) writeFunc() (func(float64) error, error) {
+	op, err := m.reg.Operation()
+	if err != nil {
+		return nil, err
 	}
 
-	return err
-}
-
-// FloatSetter executes configured modbus write operation and implements SetFloatProvider
-func (m *Modbus) FloatSetter(_ string) (func(float64) error, error) {
-	// need multiple registers for float
-	if m.op.FuncCode != gridx.FuncCodeWriteMultipleRegisters {
-		return nil, fmt.Errorf("invalid write function code: %d", m.op.FuncCode)
+	encode, err := m.reg.EncodeFunc()
+	if err != nil {
+		return nil, err
 	}
 
 	return func(val float64) error {
-		val = m.scale * val
-
-		var uval uint64
-		switch m.op.Length {
-		case 2:
-			uval = uint64(math.Float32bits(float32(val)))
-		case 4:
-			uval = math.Float64bits(val)
-		}
+		val *= m.scale
 
-		var err error
-		switch m.op.FuncCode {
-		case gridx.FuncCodeWriteMultipleRegisters:
-			err = m.writeMultipleRegisters(uval)
+		switch {
+		case op.FuncCode == gridx.FuncCodeWriteSingleRegister:
+			_, err = m.conn.WriteSingleRegister(op.Addr, uint16(val))
+			return err
+
+		case op.FuncCode == gridx.FuncCodeWriteMultipleRegisters:
+			b, err := encode(val)
+			if err == nil {
+				_, err = m.conn.WriteMultipleRegisters(op.Addr, op.Length, b)
+			}
+			return err
 
 		default:
-			err = fmt.Errorf("invalid write function code: %d", m.op.FuncCode)
+			return fmt.Errorf("invalid func code: %d", op.FuncCode)
 		}
-
-		return err
 	}, nil
 }
 
-var _ SetIntProvider = (*Modbus)(nil)
-
-// IntSetter executes configured modbus write operation and implements SetIntProvider
-func (m *Modbus) IntSetter(_ string) (func(int64) error, error) {
-	return func(val int64) error {
-		ival := int64(m.scale * float64(val))
-
-		var err error
-		switch m.op.FuncCode {
-		case gridx.FuncCodeWriteSingleRegister:
-			_, err = m.conn.WriteSingleRegister(m.op.Addr, uint16(ival))
+var _ SetFloatProvider = (*Modbus)(nil)
 
-		case gridx.FuncCodeWriteMultipleRegisters:
-			err = m.writeMultipleRegisters(uint64(ival))
+// FloatSetter implements SetFloatProvider
+func (m *Modbus) FloatSetter(_ string) (func(float64) error, error) {
+	return m.writeFunc()
+}
 
-		case gridx.FuncCodeWriteSingleCoil:
-			if ival != 0 {
-				// Modbus protocol requires 0xFF00 for ON
-				// and 0x0000 for OFF
-				ival = 0xFF00
-			}
-			_, err = m.conn.WriteSingleCoil(m.op.Addr, uint16(ival))
+var _ SetIntProvider = (*Modbus)(nil)
 
-		default:
-			err = fmt.Errorf("invalid write function code: %d", m.op.FuncCode)
-		}
+// IntSetter implements SetIntProvider
+func (m *Modbus) IntSetter(_ string) (func(int64) error, error) {
+	fun, err := m.writeFunc()
+	if err != nil {
+		return nil, err
+	}
 
-		return err
+	return func(val int64) error {
+		return fun(float64(val))
 	}, nil
 }
 
 var _ SetBoolProvider = (*Modbus)(nil)
 
-// BoolSetter executes configured modbus write operation and implements SetBoolProvider
+// BoolSetter implements SetBoolProvider
 func (m *Modbus) BoolSetter(param string) (func(bool) error, error) {
 	set, err := m.IntSetter(param)
 
diff --git a/provider/modbus_helper.go b/provider/modbus_helper.go
deleted file mode 100644
index f48dd277..00000000
--- a/provider/modbus_helper.go
+++ /dev/null
@@ -1,24 +0,0 @@
-package provider
-
-import (
-	"encoding/binary"
-	"fmt"
-)
-
-// UintFromBytes converts byte slice to bigendian uint value
-func UintFromBytes(bytes []byte) (u uint64, err error) {
-	switch l := len(bytes); l {
-	case 1:
-		u = uint64(bytes[0])
-	case 2:
-		u = uint64(binary.BigEndian.Uint16(bytes))
-	case 4:
-		u = uint64(binary.BigEndian.Uint32(bytes))
-	case 8:
-		u = binary.BigEndian.Uint64(bytes)
-	default:
-		err = fmt.Errorf("unexpected length: %d", l)
-	}
-
-	return u, err
-}
diff --git a/util/modbus/register.go b/util/modbus/register.go
index 4f7b95c3..f857f846 100644
--- a/util/modbus/register.go
+++ b/util/modbus/register.go
@@ -5,7 +5,6 @@ import (
 	"errors"
 	"fmt"
 	"math"
-	"slices"
 	"strings"
 
 	"github.com/grid-x/modbus"
@@ -56,105 +55,173 @@ func (r Register) Length() (uint16, error) {
 	}
 }
 
-// Operation creates a modbus operation from a register definition
-func (r Register) Operation() (RegisterOperation, error) {
-	len, err := r.Length()
-	if err != nil {
-		return RegisterOperation{}, err
-	}
-
-	op := RegisterOperation{
-		Addr:   r.Address,
-		Length: len,
-	}
-
+func (r Register) FuncCode() (uint8, error) {
 	switch strings.ToLower(r.Type) {
 	case "holding":
-		op.FuncCode = modbus.FuncCodeReadHoldingRegisters
+		return modbus.FuncCodeReadHoldingRegisters, nil
 	case "input":
-		op.FuncCode = modbus.FuncCodeReadInputRegisters
+		return modbus.FuncCodeReadInputRegisters, nil
 	case "coil":
-		op.FuncCode = modbus.FuncCodeReadCoils
+		return modbus.FuncCodeReadCoils, nil
 	case "writesingle", "writeholding":
-		op.FuncCode = modbus.FuncCodeWriteSingleRegister
+		return modbus.FuncCodeWriteSingleRegister, nil
 	case "writemultiple", "writeholdings":
-		op.FuncCode = modbus.FuncCodeWriteMultipleRegisters
+		return modbus.FuncCodeWriteMultipleRegisters, nil
 	case "writecoil":
-		op.FuncCode = modbus.FuncCodeWriteSingleCoil
+		return modbus.FuncCodeWriteSingleCoil, nil
+	default:
+		return 0, fmt.Errorf("invalid register type: %s", r.Type)
+	}
+}
+
+func (r Register) DecodeFunc() (func([]byte) float64, error) {
+	switch strings.ToLower(r.encoding()) {
+	// 8 bit (coil)
+	case "bool8":
+		return decodeBool8, nil
+
+	// 16 bit
+	case "int16":
+		return asFloat64(encoding.Int16), nil
+	case "int16nan":
+		return decodeNaN16(asFloat64(encoding.Int16), 1<<15, 1<<15-1), nil
+	case "uint16":
+		return asFloat64(encoding.Uint16), nil
+	case "uint16nan":
+		return decodeNaN16(asFloat64(encoding.Uint16), 1<<16-1), nil
+	case "bool16":
+		mask, err := decodeMask(r.BitMask)
+		if err != nil {
+			return nil, err
+		}
+		return decodeBool16(mask), nil
+
+	// 32 bit
+	case "int32":
+		return asFloat64(encoding.Int32), nil
+	case "int32nan":
+		return decodeNaN32(asFloat64(encoding.Int32), 1<<31, 1<<31-1), nil
+	case "int32s":
+		return asFloat64(encoding.Int32LswFirst), nil
+	case "uint32":
+		return asFloat64(encoding.Uint32), nil
+	case "uint32s":
+		return asFloat64(encoding.Uint32LswFirst), nil
+	case "uint32nan":
+		return decodeNaN32(asFloat64(encoding.Uint32), 1<<32-1), nil
+	case "float32", "ieee754":
+		return asFloat64(encoding.Float32), nil
+	case "float32s", "ieee754s":
+		return asFloat64(encoding.Float32LswFirst), nil
+
+	// 64 bit
+	case "uint64":
+		return asFloat64(encoding.Uint64), nil
+	case "uint64nan":
+		return decodeNaN64(asFloat64(encoding.Uint64), 1<<64-1), nil
+	case "float64":
+		return encoding.Float64, nil
+
 	default:
-		return RegisterOperation{}, fmt.Errorf("invalid register type: %s", r.Type)
+		return nil, fmt.Errorf("invalid register decoding: %s", r.encoding())
+	}
+}
+
+func (r Register) encodeToBytes(fun func(float64) uint64) (func(float64) ([]byte, error), error) {
+	length, err := r.Length()
+	if err != nil {
+		return nil, err
 	}
 
-	if op.IsRead() {
-		switch strings.ToLower(r.encoding()) {
-		// 8 bit (coil)
-		case "bool8":
-			op.Decode = decodeBool8
-
-		// 16 bit
-		case "int16":
-			op.Decode = asFloat64(encoding.Int16)
-		case "int16nan":
-			op.Decode = decodeNaN16(asFloat64(encoding.Int16), 1<<15, 1<<15-1)
-		case "uint16":
-			op.Decode = asFloat64(encoding.Uint16)
-		case "uint16nan":
-			op.Decode = decodeNaN16(asFloat64(encoding.Uint16), 1<<16-1)
-		case "bool16":
-			mask, err := decodeMask(r.BitMask)
-			if err != nil {
-				return op, err
-			}
-			op.Decode = decodeBool16(mask)
-
-		// 32 bit
-		case "int32":
-			op.Decode = asFloat64(encoding.Int32)
-		case "int32nan":
-			op.Decode = decodeNaN32(asFloat64(encoding.Int32), 1<<31, 1<<31-1)
-		case "int32s":
-			op.Decode = asFloat64(encoding.Int32LswFirst)
-		case "uint32":
-			op.Decode = asFloat64(encoding.Uint32)
-		case "uint32s":
-			op.Decode = asFloat64(encoding.Uint32LswFirst)
-		case "uint32nan":
-			op.Decode = decodeNaN32(asFloat64(encoding.Uint32), 1<<32-1)
-		case "float32", "ieee754":
-			op.Decode = asFloat64(encoding.Float32)
-		case "float32s", "ieee754s":
-			op.Decode = asFloat64(encoding.Float32LswFirst)
-
-		// 64 bit
-		case "uint64":
-			op.Decode = asFloat64(encoding.Uint64)
-		case "uint64nan":
-			op.Decode = decodeNaN64(asFloat64(encoding.Uint64), 1<<64-1)
-		case "float64":
-			op.Decode = encoding.Float64
+	// swapped
+	if strings.HasSuffix(strings.ToLower(r.encoding()), "s") {
+		if length != 2 {
+			return nil, fmt.Errorf("invalid swapped encoding register length: %d", length)
+		}
+
+		inner := fun
+		fun = func(f float64) uint64 {
+			v := inner(f)
+			return v&0xFFFF<<16 | v&0xFFFF0000>>16
+		}
+	}
 
+	return func(f float64) ([]byte, error) {
+		v := fun(f)
+		b := make([]byte, 2*length)
+
+		switch length {
+		case 1:
+			binary.BigEndian.PutUint16(b[:], uint16(v))
+		case 2:
+			binary.BigEndian.PutUint32(b[:], uint32(v))
+		case 4:
+			binary.BigEndian.PutUint64(b[:], v)
 		default:
-			return RegisterOperation{}, fmt.Errorf("invalid register decoding: %s", r.Decode)
+			return nil, fmt.Errorf("invalid register length: %d", length)
 		}
-	} else {
-		switch strings.ToLower(r.encoding()) {
-		case "int32s", "uint32s":
-			op.Encode = func(v uint64) uint64 {
-				return v&0xFFFF<<16 | v&0xFFFF0000>>16
-			}
-
-		case "float32s", "ieee754s":
-			op.Encode = func(v uint64) uint64 {
-				b := make([]byte, 4)
-				encoding.PutFloat32LswFirst(b, float32(v))
-				return uint64(binary.BigEndian.Uint32(b))
-			}
+
+		return b, nil
+	}, nil
+}
+
+func (r Register) EncodeFunc() (func(float64) ([]byte, error), error) {
+	enc := strings.ToLower(r.encoding())
+
+	switch {
+	case strings.HasPrefix(enc, "int") || strings.HasPrefix(enc, "uint"):
+		return r.encodeToBytes(func(v float64) uint64 {
+			return uint64(v)
+		})
+
+	case strings.HasPrefix(enc, "float") || strings.HasPrefix(enc, "ieee754"):
+		length, err := r.Length()
+		if err != nil {
+			return nil, err
+		}
+
+		switch length {
+		case 2:
+			return r.encodeToBytes(func(v float64) uint64 {
+				return uint64(math.Float32bits(float32(v)))
+			})
+
+		case 4:
+			return r.encodeToBytes(func(v float64) uint64 {
+				return math.Float64bits(v)
+			})
 
 		default:
-			op.Encode = func(v uint64) uint64 {
-				return v
-			}
+			return nil, fmt.Errorf("invalid register length: %d", length)
 		}
+
+	default:
+		return nil, fmt.Errorf("invalid register encoding: %s", r.encoding())
+	}
+}
+
+type RegisterOperation struct {
+	FuncCode uint8
+	Addr     uint16
+	Length   uint16
+}
+
+// Operation creates a modbus operation from a register definition
+func (r Register) Operation() (RegisterOperation, error) {
+	len, err := r.Length()
+	if err != nil {
+		return RegisterOperation{}, err
+	}
+
+	fc, err := r.FuncCode()
+	if err != nil {
+		return RegisterOperation{}, err
+	}
+
+	op := RegisterOperation{
+		Addr:     r.Address,
+		Length:   len,
+		FuncCode: fc,
 	}
 
 	return op, nil
@@ -170,19 +237,3 @@ func asFloat64[T constraints.Signed | constraints.Unsigned | constraints.Float](
 		return res
 	}
 }
-
-type RegisterOperation struct {
-	FuncCode uint8
-	Addr     uint16
-	Length   uint16
-	Encode   func(uint64) uint64
-	Decode   func([]byte) float64
-}
-
-func (op RegisterOperation) IsRead() bool {
-	return !slices.Contains([]uint8{
-		modbus.FuncCodeWriteSingleRegister,
-		modbus.FuncCodeWriteMultipleRegisters,
-		modbus.FuncCodeWriteSingleCoil,
-	}, op.FuncCode)
-}


Test patch:
diff --git a/util/modbus/register_test.go b/util/modbus/register_test.go
index bf9eaef6..f13ae9ae 100644
--- a/util/modbus/register_test.go
+++ b/util/modbus/register_test.go
@@ -1,6 +1,8 @@
 package modbus
 
 import (
+	"encoding/binary"
+	"math"
 	"testing"
 
 	"github.com/stretchr/testify/require"
@@ -23,3 +25,41 @@ func TestLength(t *testing.T) {
 		require.Equal(t, tc.want, res, tc)
 	}
 }
+
+func TestEncoding(t *testing.T) {
+	v32 := math.Float32bits(float32(0x12345678))
+
+	var b32, b32s [4]byte
+	binary.BigEndian.PutUint32(b32[:], v32)
+	binary.BigEndian.PutUint32(b32s[:], v32>>16|v32&0xFFFF<<16)
+
+	var b64 [8]byte
+	binary.BigEndian.PutUint64(b64[:], math.Float64bits(float64(0x12345678)))
+
+	tc := []struct {
+		r   Register
+		in  float64
+		out []byte
+	}{
+		{Register{Encoding: "int16"}, 0x1234, []byte{0x12, 0x34}},
+		{Register{Encoding: "uint16"}, 0x1234, []byte{0x12, 0x34}},
+		{Register{Encoding: "int32"}, 0x12345678, []byte{0x12, 0x34, 0x56, 0x78}},
+		{Register{Encoding: "uint32"}, 0x12345678, []byte{0x12, 0x34, 0x56, 0x78}},
+		{Register{Encoding: "int32s"}, 0x12345678, []byte{0x56, 0x78, 0x12, 0x34}},
+		{Register{Encoding: "uint32s"}, 0x12345678, []byte{0x56, 0x78, 0x12, 0x34}},
+		{Register{Encoding: "float32"}, 0x12345678, b32[:]},
+		{Register{Encoding: "ieee754"}, 0x12345678, b32[:]},
+		{Register{Encoding: "float32s"}, 0x12345678, b32s[:]},
+		{Register{Encoding: "ieee754s"}, 0x12345678, b32s[:]},
+		{Register{Encoding: "float64"}, 0x12345678, b64[:]},
+	}
+
+	for _, tc := range tc {
+		fun, err := tc.r.EncodeFunc()
+		require.NoError(t, err, tc)
+
+		res, err := fun(tc.in)
+		require.NoError(t, err, tc)
+		require.Equal(t, tc.out, res, tc)
+	}
+}