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