Skip to content

Commit

Permalink
Modbus Proxy: fix coils (#5201)
Browse files Browse the repository at this point in the history
  • Loading branch information
@andig
andig authored Nov 19, 2022
1 parent 99264e4 commit 771da60
Show file tree
Hide file tree
Showing 3 changed files with 141 additions and 54 deletions.
87 changes: 50 additions & 37 deletions server/modbus/handler.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ package modbus
import (
"encoding/binary"
"errors"
"math/bits"

"github.com/andig/mbserver"
"github.com/evcc-io/evcc/util"
Expand Down Expand Up @@ -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)
}
}

Expand All @@ -74,15 +53,48 @@ 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
if errors.As(err, &modbusError) {
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) {
Expand All @@ -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)
}
Expand All @@ -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 changes: 1 addition & 1 deletion server/modbus/proxy.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
}

Expand Down
106 changes: 90 additions & 16 deletions server/modbus/proxy_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++ {
Expand All @@ -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)
Expand All @@ -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
}

0 comments on commit 771da60

Please sign in to comment.