diff --git a/server/modbus/handler.go b/server/modbus/handler.go index 6c2ac1ad9..5492ac10c 100644 --- a/server/modbus/handler.go +++ b/server/modbus/handler.go @@ -3,6 +3,7 @@ package modbus import ( "encoding/binary" "errors" + "math/bits" "github.com/andig/mbserver" "github.com/evcc-io/evcc/util" @@ -33,33 +34,11 @@ func asBytes(u []uint16) []byte { return b } -func bytesAsBool(b []byte) []bool { - var res []bool - for _, c := range bytesAsUint16(b) { - if c != 0 { - res = append(res, true) - continue - } - res = append(res, false) - } - return res -} - -func boolAsBytes(b []bool) []byte { - res := make([]byte, 2*len(b)) - for i, bb := range b { - if bb { - binary.BigEndian.PutUint16(res[2*i:], 0xFF00) - } - } - return res -} - func (h *handler) logResult(op string, b []byte, err error) { if err == nil { - h.log.TRACE.Printf(op+" response: %0x", b) + h.log.TRACE.Printf(op+": %0x", b) } else { - h.log.TRACE.Printf(op+" response: %v", err) + h.log.TRACE.Printf(op+": %v", err) } } @@ -74,7 +53,27 @@ func (h *handler) exceptionToUint16AndError(op string, b []byte, err error) ([]u return bytesAsUint16(b), err } -func (h *handler) exceptionToBoolAndError(op string, b []byte, err error) ([]bool, error) { +func coilsToBytes(b []bool) []byte { + l := len(b) / 8 + if len(b)%8 != 0 { + l++ + } + + res := make([]byte, l) + + for i, bb := range b { + if bb { + byteNum := i / 8 + bit := i % 8 + + res[byteNum] |= bits.RotateLeft8(1, bit) + } + } + + return res +} + +func (h *handler) coilsToResult(op string, qty uint16, b []byte, err error) ([]bool, error) { h.logResult(op, b, err) var modbusError *gridx.Error @@ -82,7 +81,20 @@ func (h *handler) exceptionToBoolAndError(op string, b []byte, err error) ([]boo err = mbserver.MapExceptionCodeToError(modbusError.ExceptionCode) } - return bytesAsBool(b), err + var res []bool + +LOOP: + for _, bb := range b { + for bit := 0; bit < 8; bit++ { + if len(res) >= int(qty) { + break LOOP + } + + res = append(res, bits.RotateLeft8(bb, -bit)&1 != 0) + } + } + + return res, err } func (h *handler) HandleCoils(req *mbserver.CoilsRequest) ([]bool, error) { @@ -92,28 +104,29 @@ func (h *handler) HandleCoils(req *mbserver.CoilsRequest) ([]bool, error) { } if req.Quantity == 1 { - h.log.TRACE.Printf("write coil: id: %d addr: %d val: %t", req.UnitId, req.Addr, req.Args[0]) + h.log.TRACE.Printf("write coil: id %d addr %d val %t", req.UnitId, req.Addr, req.Args[0]) var u uint16 if req.Args[0] { u = 0xFF00 } b, err := h.conn.WriteSingleCoilWithSlave(req.UnitId, req.Addr, u) - return h.exceptionToBoolAndError("write coil", b, err) + return h.coilsToResult("write coil", req.Quantity, b, err) } - h.log.TRACE.Printf("write multiple coils: id: %d addr: %d qty: %d val: %v", req.UnitId, req.Addr, req.Quantity, req.Args) - b, err := h.conn.WriteMultipleCoilsWithSlave(req.UnitId, req.Addr, req.Quantity, boolAsBytes(req.Args)) - return h.exceptionToBoolAndError("write multiple coils", b, err) + h.log.TRACE.Printf("write multiple coils: id %d addr %d qty %d val %v", req.UnitId, req.Addr, req.Quantity, req.Args) + args := coilsToBytes(req.Args) + b, err := h.conn.WriteMultipleCoilsWithSlave(req.UnitId, req.Addr, req.Quantity, args) + return h.coilsToResult("write multiple coils", req.Quantity, b, err) } - h.log.TRACE.Printf("read coil: id: %d addr: %d qty: %d", req.UnitId, req.Addr, req.Quantity) + h.log.TRACE.Printf("read coil: id %d addr %d qty %d", req.UnitId, req.Addr, req.Quantity) b, err := h.conn.ReadCoilsWithSlave(req.UnitId, req.Addr, req.Quantity) - return h.exceptionToBoolAndError("read coil", b, err) + return h.coilsToResult("read coil", req.Quantity, b, err) } func (h *handler) HandleInputRegisters(req *mbserver.InputRegistersRequest) (res []uint16, err error) { - h.log.TRACE.Printf("read input: id: %d addr: %d qty: %d", req.UnitId, req.Addr, req.Quantity) + h.log.TRACE.Printf("read input: id %d addr %d qty %d", req.UnitId, req.Addr, req.Quantity) b, err := h.conn.ReadInputRegistersWithSlave(req.UnitId, req.Addr, req.Quantity) return h.exceptionToUint16AndError("read input", b, err) } @@ -125,17 +138,17 @@ func (h *handler) HandleHoldingRegisters(req *mbserver.HoldingRegistersRequest) } if req.Quantity == 1 { - h.log.TRACE.Printf("write holding: id: %d addr: %d val: %0x", req.UnitId, req.Addr, req.Args[0]) + h.log.TRACE.Printf("write holding: id %d addr %d val %0x", req.UnitId, req.Addr, req.Args[0]) b, err := h.conn.WriteSingleRegisterWithSlave(req.UnitId, req.Addr, req.Args[0]) return h.exceptionToUint16AndError("write holding", b, err) } - h.log.TRACE.Printf("write multiple holding: id: %d addr: %d qty: %d val: %0x", req.UnitId, req.Addr, req.Quantity, asBytes(req.Args)) + h.log.TRACE.Printf("write multiple holding: id %d addr %d qty %d val %0x", req.UnitId, req.Addr, req.Quantity, asBytes(req.Args)) b, err := h.conn.WriteMultipleRegistersWithSlave(req.UnitId, req.Addr, req.Quantity, asBytes(req.Args)) return h.exceptionToUint16AndError("write multiple holding", b, err) } - h.log.TRACE.Printf("read holding: id: %d addr: %d qty: %d", req.UnitId, req.Addr, req.Quantity) + h.log.TRACE.Printf("read holding: id %d addr %d qty %d", req.UnitId, req.Addr, req.Quantity) b, err := h.conn.ReadHoldingRegistersWithSlave(req.UnitId, req.Addr, req.Quantity) return h.exceptionToUint16AndError("read holding", b, err) } diff --git a/server/modbus/proxy.go b/server/modbus/proxy.go index 7a4593b1f..39b0676b0 100644 --- a/server/modbus/proxy.go +++ b/server/modbus/proxy.go @@ -24,7 +24,7 @@ func StartProxy(port int, config modbus.Settings, readOnly bool) error { h := &handler{ log: util.NewLogger(fmt.Sprintf("proxy-%d", port)), readOnly: readOnly, - RequestHandler: new(mbserver.DummyHandler), + RequestHandler: new(mbserver.DummyHandler), // supplies HandleDiscreteInputs conn: conn, } diff --git a/server/modbus/proxy_test.go b/server/modbus/proxy_test.go index c325cf82d..83951dcdf 100644 --- a/server/modbus/proxy_test.go +++ b/server/modbus/proxy_test.go @@ -9,30 +9,27 @@ import ( "time" "github.com/andig/mbserver" + "github.com/evcc-io/evcc/util" "github.com/evcc-io/evcc/util/modbus" "github.com/stretchr/testify/assert" ) -func TestProxyRead(t *testing.T) { - l, err := net.Listen("tcp", ":0") +func TestConcurrentRead(t *testing.T) { + l, err := net.Listen("tcp", "localhost:0") assert.NoError(t, err) defer l.Close() - t.Log(l.Addr().String()) - - conn, err := modbus.NewConnection(l.Addr().String(), "", "", 0, modbus.Tcp, 1) - assert.NoError(t, err) - - h := &echoHandler{ + srv, _ := mbserver.New(&echoHandler{ id: 0, RequestHandler: new(mbserver.DummyHandler), - conn: conn, - } - - srv, _ := mbserver.New(h) + }) assert.NoError(t, srv.Start(l)) defer func() { _ = srv.Stop() }() + // client + conn, err := modbus.NewConnection(l.Addr().String(), "", "", 0, modbus.Tcp, 1) + assert.NoError(t, err) + var wg sync.WaitGroup for i := 1; i <= 10; i++ { @@ -41,12 +38,15 @@ func TestProxyRead(t *testing.T) { go func(id int) { for i := 0; i < 50; i++ { addr := uint16(rand.Int31n(200) + 1) + qty := uint16(rand.Int31n(32) + 1) - b, err := conn.ReadInputRegistersWithSlave(uint8(id), addr, 1) + b, err := conn.ReadInputRegistersWithSlave(uint8(id), addr, qty) assert.NoError(t, err) if err == nil { - assert.Equal(t, addr^uint16(id), binary.BigEndian.Uint16(b)) + for u := uint16(0); u < qty; u++ { + assert.Equal(t, addr^uint16(id)^u, binary.BigEndian.Uint16(b[2*u:])) + } } time.Sleep(time.Duration(rand.Int31n(1000)) * time.Microsecond) @@ -59,12 +59,86 @@ func TestProxyRead(t *testing.T) { wg.Wait() } +func TestReadCoils(t *testing.T) { + // downstream server + l, err := net.Listen("tcp", "localhost:0") + assert.NoError(t, err) + defer l.Close() + + srv, _ := mbserver.New(&echoHandler{ + id: 0, + RequestHandler: new(mbserver.DummyHandler), + }) + assert.NoError(t, srv.Start(l)) + defer func() { _ = srv.Stop() }() + + // proxy server + pl, err := net.Listen("tcp", "localhost:0") + assert.NoError(t, err) + defer pl.Close() + + downstreamConn, err := modbus.NewConnection(l.Addr().String(), "", "", 0, modbus.Tcp, 1) + assert.NoError(t, err) + + proxy, _ := mbserver.New(&handler{ + log: util.NewLogger("foo"), + RequestHandler: new(mbserver.DummyHandler), + conn: downstreamConn, + }) + assert.NoError(t, proxy.Start(pl)) + defer func() { _ = proxy.Stop() }() + + // test client + { + conn, err := modbus.NewConnection(pl.Addr().String(), "", "", 0, modbus.Tcp, 1) + assert.NoError(t, err) + + { // read + b, err := conn.ReadCoilsWithSlave(1, 1, 1) + assert.NoError(t, err) + assert.Equal(t, []byte{0x01}, b) + + b, err = conn.ReadCoilsWithSlave(1, 1, 2) + assert.NoError(t, err) + assert.Equal(t, []byte{0x03}, b) + + b, err = conn.ReadCoilsWithSlave(1, 1, 9) + assert.NoError(t, err) + assert.Equal(t, []byte{0xFF, 0x01}, b) + } + { // write + b, err := conn.WriteSingleCoilWithSlave(1, 1, 0xFF00) + assert.NoError(t, err) + assert.Equal(t, []byte{0xFF, 0x00}, b) + + b, err = conn.WriteMultipleCoilsWithSlave(1, 1, 9, []byte{0xFF, 0x01}) + assert.NoError(t, err) + assert.Equal(t, []byte{0x00, 0x09}, b) + } + } +} + type echoHandler struct { id int mbserver.RequestHandler - conn *modbus.Connection } func (h *echoHandler) HandleInputRegisters(req *mbserver.InputRegistersRequest) (res []uint16, err error) { - return []uint16{req.Addr ^ uint16(req.UnitId)}, err + for u := uint16(0); u < req.Quantity; u++ { + res = append(res, req.Addr^uint16(req.UnitId)^u) + } + + return res, err +} + +func (h *echoHandler) HandleCoils(req *mbserver.CoilsRequest) (res []bool, err error) { + if req.IsWrite { + return nil, nil + } + + for u := uint16(0); u < req.Quantity; u++ { + res = append(res, true) + } + + return res, err }