git.giacomolli.uk - onehouse/blob - driver/m31/tcp_client.go

   1 package m31
   2
   3 import (
   4         "bytes"
   5         "context"
   6         "encoding/binary"
   7         "errors"
   8         "fmt"
   9         "io"
  10         "net"
  11         "os"
  12         "time"
  13 )
  14
  15 const (
  16         connectTimeout = 2 * time.Second
  17         writeTimeout   = time.Second
  18         readTimeout    = 2 * time.Second
  19 )
  20
  21 type tcpClient struct {
  22         conn        net.Conn
  23         unit        byte
  24         transaction uint16
  25 }
  26
  27 // NewTCPClient supplies Modbus TCP functions 01, 02, 03, 05, 15, and 16 without
  28 // opening a connection. Connect has a two-second timeout. Writes have a
  29 // one-second timeout, and reads have a two-second timeout. Earlier caller
  30 // deadlines apply to each operation. The caller owns Close.
  31 func NewTCPClient() Client {
  32         return &tcpClient{}
  33 }
  34
  35 func (c *tcpClient) Connect(ctx context.Context, address string, unit byte) error {
  36         if c.conn != nil {
  37                 return fmt.Errorf("modbus connection is already open")
  38         }
  39
  40         if unit == 0 || unit > 247 {
  41                 return fmt.Errorf("modbus unit must be from one through 247")
  42         }
  43
  44         ctx, cancel := context.WithTimeout(ctx, connectTimeout)
  45         defer cancel()
  46
  47         conn, err := (&net.Dialer{}).DialContext(ctx, "tcp", address)
  48         if err != nil {
  49                 return fmt.Errorf("connect Modbus transport: %w", err)
  50         }
  51
  52         c.conn, c.unit = conn, unit
  53
  54         return nil
  55 }
  56
  57 func (c *tcpClient) Close() error {
  58         if c.conn == nil {
  59                 return nil
  60         }
  61
  62         err := c.conn.Close()
  63         c.conn = nil
  64         if err != nil {
  65                 return fmt.Errorf("close Modbus transport: %w", err)
  66         }
  67
  68         return nil
  69 }
  70
  71 func (c *tcpClient) ReadInputs(ctx context.Context, count uint16) ([]bool, error) {
  72         return c.readBits(ctx, 2, count)
  73 }
  74
  75 func (c *tcpClient) ReadCoils(ctx context.Context, count uint16) ([]bool, error) {
  76         return c.readBits(ctx, 1, count)
  77 }
  78
  79 func (c *tcpClient) readBits(ctx context.Context, function byte, count uint16) ([]bool, error) {
  80         if count == 0 || count > 2000 {
  81                 return nil, fmt.Errorf("bit read count must be from one through 2000")
  82         }
  83
  84         data, err := c.exchange(ctx, function, words(0, count))
  85         if err != nil {
  86                 return nil, fmt.Errorf("read Modbus bits: %w", err)
  87         }
  88
  89         size := (int(count) + 7) / 8
  90         if len(data) != size+1 || int(data[0]) != size {
  91                 return nil, fmt.Errorf("modbus bit response has an invalid length")
  92         }
  93
  94         values := make([]bool, count)
  95
  96         for index := range values {
  97                 values[index] = data[1+index/8]&(1<<uint(index%8)) != 0
  98         }
  99
 100         return values, nil
 101 }
 102
 103 func (c *tcpClient) ReadRegisters(ctx context.Context, address uint16, count uint16) ([]uint16, error) {
 104         if count == 0 || count > 125 || uint32(address)+uint32(count) > 65536 {
 105                 return nil, fmt.Errorf("invalid register read range")
 106         }
 107
 108         data, err := c.exchange(ctx, 3, words(address, count))
 109         if err != nil {
 110                 return nil, fmt.Errorf("read Modbus registers: %w", err)
 111         }
 112
 113         if len(data) != int(count)*2+1 || int(data[0]) != int(count)*2 {
 114                 return nil, fmt.Errorf("modbus register response has an invalid length")
 115         }
 116
 117         values := make([]uint16, count)
 118
 119         for index := range values {
 120                 values[index] = binary.BigEndian.Uint16(data[1+2*index:])
 121         }
 122
 123         return values, nil
 124 }
 125
 126 func (c *tcpClient) WriteCoil(ctx context.Context, address uint16, value bool) error {
 127         var encoded uint16
 128
 129         if value {
 130                 encoded = 0xff00
 131         }
 132
 133         return c.write(ctx, 5, words(address, encoded), words(address, encoded))
 134 }
 135
 136 func (c *tcpClient) WriteCoils(ctx context.Context, address uint16, values []bool) error {
 137         if len(values) == 0 || len(values) > 1968 || uint32(address)+uint32(len(values)) > 65536 {
 138                 return fmt.Errorf("invalid coil write range")
 139         }
 140
 141         data := append(words(address, uint16(len(values))), byte((len(values)+7)/8))
 142         data = append(data, make([]byte, (len(values)+7)/8)...)
 143
 144         for index, value := range values {
 145                 if value {
 146                         data[5+index/8] |= 1 << uint(index%8)
 147                 }
 148         }
 149
 150         return c.write(ctx, 15, data, words(address, uint16(len(values))))
 151 }
 152
 153 func (c *tcpClient) WriteRegister(ctx context.Context, address uint16, value uint16) error {
 154         data := append(words(address, 1), 2)
 155         data = append(data, words(value)...)
 156
 157         return c.write(ctx, 16, data, words(address, 1))
 158 }
 159
 160 func (c *tcpClient) write(ctx context.Context, function byte, request []byte, expected []byte) error {
 161         data, err := c.exchange(ctx, function, request)
 162         if err != nil {
 163                 return fmt.Errorf("write Modbus value: %w", err)
 164         }
 165
 166         if !bytes.Equal(data, expected) {
 167                 return fmt.Errorf("modbus write acknowledgement differs from request")
 168         }
 169
 170         return nil
 171 }
 172
 173 func (c *tcpClient) exchange(ctx context.Context, function byte, data []byte) (response []byte, err error) {
 174         if ctx.Err() != nil {
 175                 return nil, ctx.Err()
 176         }
 177
 178         if c.conn == nil {
 179                 return nil, fmt.Errorf("modbus transport is closed")
 180         }
 181
 182         c.transaction++
 183         request := append(words(c.transaction, 0, uint16(len(data)+2)), c.unit, function)
 184         request = append(request, data...)
 185         err = c.conn.SetWriteDeadline(operationDeadline(ctx, writeTimeout))
 186         if err != nil {
 187                 return nil, fmt.Errorf("set Modbus write deadline: %w", err)
 188         }
 189
 190         err = c.conn.SetReadDeadline(operationDeadline(ctx, readTimeout))
 191         if err != nil {
 192                 return nil, fmt.Errorf("set Modbus read deadline: %w", err)
 193         }
 194
 195         interrupted := make(chan struct{})
 196
 197         var interruptErr error
 198
 199         stop := context.AfterFunc(ctx, func() {
 200                 interruptErr = c.conn.SetDeadline(time.Now())
 201
 202                 close(interrupted)
 203         })
 204         defer func() {
 205                 if !stop() {
 206                         <-interrupted
 207
 208                         if err == nil || deadlineOnly(err) {
 209                                 err = ctx.Err()
 210                         }
 211
 212                         err = errors.Join(err, interruptErr, ctx.Err())
 213                 }
 214         }()
 215
 216         count, err := c.conn.Write(request)
 217         if err != nil {
 218                 return nil, fmt.Errorf("write Modbus request: %w", err)
 219         }
 220
 221         if count != len(request) {
 222                 return nil, fmt.Errorf("write Modbus request: %w", io.ErrShortWrite)
 223         }
 224
 225         var header [7]byte
 226
 227         _, err = io.ReadFull(c.conn, header[:])
 228         if err != nil {
 229                 return nil, fmt.Errorf("read Modbus header: %w", err)
 230         }
 231
 232         length := binary.BigEndian.Uint16(header[4:6])
 233         if binary.BigEndian.Uint16(header[:2]) != c.transaction || binary.BigEndian.Uint16(header[2:4]) != 0 || header[6] != c.unit || length < 2 || length > 254 {
 234                 return nil, fmt.Errorf("invalid Modbus response header")
 235         }
 236
 237         body := make([]byte, length-1)
 238         _, err = io.ReadFull(c.conn, body)
 239         if err != nil {
 240                 return nil, fmt.Errorf("read Modbus body: %w", err)
 241         }
 242
 243         if body[0] == function|0x80 && len(body) == 2 {
 244                 return nil, fmt.Errorf("modbus exception %d", body[1])
 245         }
 246
 247         if body[0] != function {
 248                 return nil, fmt.Errorf("invalid Modbus response function")
 249         }
 250
 251         return body[1:], nil
 252 }
 253
 254 func words(values ...uint16) []byte {
 255         data := make([]byte, 2*len(values))
 256
 257         for index, value := range values {
 258                 binary.BigEndian.PutUint16(data[2*index:], value)
 259         }
 260
 261         return data
 262 }
 263
 264 func operationDeadline(ctx context.Context, timeout time.Duration) time.Time {
 265         deadline := time.Now().Add(timeout)
 266         caller, exists := ctx.Deadline()
 267         if exists && caller.Before(deadline) {
 268                 return caller
 269         }
 270
 271         return deadline
 272 }
 273
 274 func deadlineOnly(err error) bool {
 275         joined, ok := err.(interface{ Unwrap() []error })
 276         if ok {
 277                 found := false
 278
 279                 for _, cause := range joined.Unwrap() {
 280                         if cause != nil {
 281                                 found = true
 282                                 if !deadlineOnly(cause) {
 283                                         return false
 284                                 }
 285                         }
 286                 }
 287
 288                 return found
 289         }
 290
 291         wrapped, ok := err.(interface{ Unwrap() error })
 292         if ok {
 293                 return deadlineOnly(wrapped.Unwrap())
 294         }
 295
 296         return errors.Is(err, os.ErrDeadlineExceeded)
 297 }