diff --git a/provider/modbus.go b/provider/modbus.go index ccd00b870..fe37a703f 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() - if err != nil { - return 0, fmt.Errorf("read failed: %w", err) - } - - return m.scale * m.op.Decode(bytes), nil -} - var _ FloatProvider = (*Modbus)(nil) -// FloatGetter executes configured modbus read operation and implements func() (float64, error) +// FloatGetter implements func() (float64, error) func (m *Modbus) FloatGetter() (func() (f float64, err error), error) { - return m.floatGetter, nil + op, err := m.reg.Operation() + if err != nil { + return nil, err + } + + decode, err := m.reg.DecodeFunc() + if err != nil { + return nil, err + } + + return func() (float64, error) { + bytes, err := m.readBytes(op) + if err != nil { + return 0, fmt.Errorf("read failed: %w", err) + } + + 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 + return func() (bool, error) { + res, err := g() + return res != 0, err + }, err +} + +func (m *Modbus) writeFunc() (func(float64) error, error) { + op, err := m.reg.Operation() + if err != nil { + return nil, err + } + + encode, err := m.reg.EncodeFunc() + if err != nil { + return nil, err + } + + return func(val float64) error { + val *= m.scale + + 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: + return fmt.Errorf("invalid func code: %d", op.FuncCode) + } }, nil } 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) - } - - return err -} - -// FloatSetter executes configured modbus write operation and implements SetFloatProvider +// FloatSetter 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) - } - - 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) - } - - var err error - switch m.op.FuncCode { - case gridx.FuncCodeWriteMultipleRegisters: - err = m.writeMultipleRegisters(uval) - - default: - err = fmt.Errorf("invalid write function code: %d", m.op.FuncCode) - } - - return err - }, nil + return m.writeFunc() } var _ SetIntProvider = (*Modbus)(nil) -// IntSetter executes configured modbus write operation and implements SetIntProvider +// IntSetter implements SetIntProvider func (m *Modbus) IntSetter(_ string) (func(int64) error, error) { + fun, err := m.writeFunc() + if err != nil { + return nil, err + } + 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)) - - case gridx.FuncCodeWriteMultipleRegisters: - err = m.writeMultipleRegisters(uint64(ival)) - - 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)) - - default: - err = fmt.Errorf("invalid write function code: %d", m.op.FuncCode) - } - - return err + 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 f48dd2777..000000000 --- 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 4f7b95c30..f857f846d 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,6 +55,157 @@ func (r Register) Length() (uint16, error) { } } +func (r Register) FuncCode() (uint8, error) { + switch strings.ToLower(r.Type) { + case "holding": + return modbus.FuncCodeReadHoldingRegisters, nil + case "input": + return modbus.FuncCodeReadInputRegisters, nil + case "coil": + return modbus.FuncCodeReadCoils, nil + case "writesingle", "writeholding": + return modbus.FuncCodeWriteSingleRegister, nil + case "writemultiple", "writeholdings": + return modbus.FuncCodeWriteMultipleRegisters, nil + case "writecoil": + 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 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 + } + + // 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 nil, fmt.Errorf("invalid register length: %d", length) + } + + 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: + 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() @@ -63,98 +213,15 @@ func (r Register) Operation() (RegisterOperation, error) { return RegisterOperation{}, err } + fc, err := r.FuncCode() + if err != nil { + return RegisterOperation{}, err + } + op := RegisterOperation{ - Addr: r.Address, - Length: len, - } - - switch strings.ToLower(r.Type) { - case "holding": - op.FuncCode = modbus.FuncCodeReadHoldingRegisters - case "input": - op.FuncCode = modbus.FuncCodeReadInputRegisters - case "coil": - op.FuncCode = modbus.FuncCodeReadCoils - case "writesingle", "writeholding": - op.FuncCode = modbus.FuncCodeWriteSingleRegister - case "writemultiple", "writeholdings": - op.FuncCode = modbus.FuncCodeWriteMultipleRegisters - case "writecoil": - op.FuncCode = modbus.FuncCodeWriteSingleCoil - default: - return RegisterOperation{}, fmt.Errorf("invalid register type: %s", r.Type) - } - - 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 - - default: - return RegisterOperation{}, fmt.Errorf("invalid register decoding: %s", r.Decode) - } - } 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)) - } - - default: - op.Encode = func(v uint64) uint64 { - return v - } - } + 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) -} diff --git a/util/modbus/register_test.go b/util/modbus/register_test.go index bf9eaef67..f13ae9aee 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) + } +}