src/modbus/modbus_serial_rtu.c

   1 /*
   2  * This file is part of the libsigrok project.
   3  *
   4  * Copyright (C) 2015 Aurelien Jacobs <aurel@gnuage.org>
   5  *
   6  * This program is free software: you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation, either version 3 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include <glib.h>
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include "libsigrok.h"
  24 #include "libsigrok-internal.h"
  25
  26 #define LOG_PREFIX "modbus_serial"
  27
  28 #define BUFFER_SIZE 1024
  29
  30 struct modbus_serial_rtu {
  31         struct sr_serial_dev_inst *serial;
  32         uint8_t slave_addr;
  33         uint16_t crc;
  34 };
  35
  36 static int modbus_serial_rtu_dev_inst_new(void *priv, const char *resource,
  37                 char **params, const char *serialcomm, int modbusaddr)
  38 {
  39         struct modbus_serial_rtu *modbus = priv;
  40
  41         (void)params;
  42
  43         modbus->serial = sr_serial_dev_inst_new(resource, serialcomm);
  44         modbus->slave_addr = modbusaddr;
  45
  46         return SR_OK;
  47 }
  48
  49 static int modbus_serial_rtu_open(void *priv)
  50 {
  51         struct modbus_serial_rtu *modbus = priv;
  52         struct sr_serial_dev_inst *serial = modbus->serial;
  53
  54         if (serial_open(serial, SERIAL_RDWR) != SR_OK)
  55                 return SR_ERR;
  56
  57         if (serial_flush(serial) != SR_OK)
  58                 return SR_ERR;
  59
  60         return SR_OK;
  61 }
  62
  63 static int modbus_serial_rtu_source_add(struct sr_session *session, void *priv,
  64                 int events, int timeout, sr_receive_data_callback cb, void *cb_data)
  65 {
  66         struct modbus_serial_rtu *modbus = priv;
  67         struct sr_serial_dev_inst *serial = modbus->serial;
  68
  69         return serial_source_add(session, serial, events, timeout, cb, cb_data);
  70 }
  71
  72 static int modbus_serial_rtu_source_remove(struct sr_session *session,
  73                 void *priv)
  74 {
  75         struct modbus_serial_rtu *modbus = priv;
  76         struct sr_serial_dev_inst *serial = modbus->serial;
  77
  78         return serial_source_remove(session, serial);
  79 }
  80
  81 static uint16_t modbus_serial_rtu_crc(uint16_t crc,
  82                 const uint8_t *buffer, int len)
  83 {
  84         int i;
  85
  86         if (!buffer || len < 0)
  87                 return crc;
  88
  89         while (len--) {
  90                 crc ^= *buffer++;
  91                 for (i = 0; i < 8; i++) {
  92                         int carry = crc & 1;
  93                         crc >>= 1;
  94                         if (carry)
  95                                 crc ^= 0xA001;
  96                 }
  97         }
  98
  99         return crc;
 100 }
 101
 102 static int modbus_serial_rtu_send(void *priv,
 103                 const uint8_t *buffer, int buffer_size)
 104 {
 105         int result;
 106         struct modbus_serial_rtu *modbus = priv;
 107         struct sr_serial_dev_inst *serial = modbus->serial;
 108         uint8_t slave_addr = modbus->slave_addr;
 109         uint16_t crc;
 110
 111         result = serial_write_nonblocking(serial, &slave_addr, sizeof(slave_addr));
 112         if (result < 0)
 113                 return result;
 114
 115         result = serial_write_nonblocking(serial, buffer, buffer_size);
 116         if (result < 0)
 117                 return result;
 118
 119         crc = modbus_serial_rtu_crc(0xFFFF, &slave_addr, sizeof(slave_addr));
 120         crc = modbus_serial_rtu_crc(crc, buffer, buffer_size);
 121
 122         result = serial_write_nonblocking(serial, &crc, sizeof(crc));
 123         if (result < 0)
 124                 return result;
 125
 126         return SR_OK;
 127 }
 128
 129 static int modbus_serial_rtu_read_begin(void *priv, uint8_t *function_code)
 130 {
 131         struct modbus_serial_rtu *modbus = priv;
 132         uint8_t slave_addr;
 133         int ret;
 134
 135         ret = serial_read_blocking(modbus->serial, &slave_addr, 1, 100);
 136         if (ret != 1 || slave_addr != modbus->slave_addr)
 137                 return ret;
 138
 139         ret = serial_read_blocking(modbus->serial, function_code, 1, 100);
 140         if (ret != 1)
 141                 return ret;
 142
 143         modbus->crc = modbus_serial_rtu_crc(0xFFFF, &slave_addr, sizeof(slave_addr));
 144         modbus->crc = modbus_serial_rtu_crc(modbus->crc, function_code, 1);
 145
 146         return SR_OK;
 147 }
 148
 149 static int modbus_serial_rtu_read_data(void *priv, uint8_t *buf, int maxlen)
 150 {
 151         struct modbus_serial_rtu *modbus = priv;
 152         int ret;
 153
 154         ret = serial_read_nonblocking(modbus->serial, buf, maxlen);
 155         if (ret < 0)
 156                 return ret;
 157         modbus->crc = modbus_serial_rtu_crc(modbus->crc, buf, ret);
 158         return ret;
 159 }
 160
 161 static int modbus_serial_rtu_read_end(void *priv)
 162 {
 163         struct modbus_serial_rtu *modbus = priv;
 164         uint16_t crc;
 165         int ret;
 166
 167         ret = serial_read_blocking(modbus->serial, &crc, sizeof(crc), 100);
 168         if (ret != 2)
 169                 return ret;
 170
 171         if (crc != modbus->crc) {
 172                 sr_err("CRC error (0x%04X vs 0x%04X).", crc, modbus->crc);
 173                 return SR_ERR_DATA;
 174         }
 175
 176         return SR_OK;
 177 }
 178
 179 static int modbus_serial_rtu_close(void *priv)
 180 {
 181         struct modbus_serial_rtu *modbus = priv;
 182
 183         return serial_close(modbus->serial);
 184 }
 185
 186 static void modbus_serial_rtu_free(void *priv)
 187 {
 188         struct modbus_serial_rtu *modbus = priv;
 189
 190         sr_serial_dev_inst_free(modbus->serial);
 191 }
 192
 193 SR_PRIV const struct sr_modbus_dev_inst modbus_serial_rtu_dev = {
 194         .name          = "serial_rtu",
 195         .prefix        = "",
 196         .priv_size     = sizeof(struct modbus_serial_rtu),
 197         .scan          = NULL,
 198         .dev_inst_new  = modbus_serial_rtu_dev_inst_new,
 199         .open          = modbus_serial_rtu_open,
 200         .source_add    = modbus_serial_rtu_source_add,
 201         .source_remove = modbus_serial_rtu_source_remove,
 202         .send          = modbus_serial_rtu_send,
 203         .read_begin    = modbus_serial_rtu_read_begin,
 204         .read_data     = modbus_serial_rtu_read_data,
 205         .read_end      = modbus_serial_rtu_read_end,
 206         .close         = modbus_serial_rtu_close,
 207         .free          = modbus_serial_rtu_free,
 208 };