Stefan/evcc-io
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Modbus Proxy: fix coils (#5201)
Some checks failed
Release / call-build-workflow (push) Has been cancelled
Details
Release / Publish Docker :release (push) Has been cancelled
Details
Release / Github & APT (push) Has been cancelled
Details
Release / Hassio Addon (push) Has been cancelled
Details

Browse source
This commit is contained in:
andig 2022-11-19 10:52:32 +01:00 • committed by GitHub
parent 99264e4cf9
commit 771da60e12
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 141 additions and 54 deletions
Download patch file Download diff file Expand all files Collapse all files

87
server/modbus/handler.go
View file

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

2
server/modbus/proxy.go
View file

@ -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,
}

106
server/modbus/proxy_test.go
View file

@ -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
}