Repository: evcc-io/evcc
Pull request #5201 (2022-11-18 23:32:48)

Modbus Proxy: fix handling of coils


Patch:
diff --git a/cmd/config.go b/cmd/config.go
index 97f2c1b9..4ab69e80 100644
--- a/cmd/config.go
+++ b/cmd/config.go
@@ -269,6 +269,7 @@ func (cp *ConfigProvider) configureVehicles(conf config) error {
 
 			v, err := vehicle.NewFromConfig(cc.Type, cc.Other)
 			if err != nil {
+				log.ERROR.Printf("creating vehicle %s failed: %v", cc.Name, err)
 				// wrap any created errors to prevent fatals
 				v, _ = wrapper.New(v, err)
 			}
diff --git a/server/modbus/handler.go b/server/modbus/handler.go
index 6c2ac1ad..5492ac10 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 7a4593b1..39b0676b 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,
 	}
 


Test patch:
diff --git a/server/modbus/proxy_test.go b/server/modbus/proxy_test.go
index c325cf82..83951dcd 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
 }