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 <config.h>
  21 #include <glib.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <libsigrok/libsigrok.h>
  25 #include "libsigrok-internal.h"
  26
  27 #define LOG_PREFIX "modbus_serial"
  28
  29 #define BUFFER_SIZE 1024
  30
  31 struct modbus_serial_rtu {
  32         struct sr_serial_dev_inst *serial;
  33         uint8_t slave_addr;
  34         uint16_t crc;
  35 };
  36
  37 static int modbus_serial_rtu_dev_inst_new(void *priv, const char *resource,
  38                 char **params, const char *serialcomm, int modbusaddr)
  39 {
  40         struct modbus_serial_rtu *modbus = priv;
  41
  42         (void)params;
  43
  44         modbus->serial = sr_serial_dev_inst_new(resource, serialcomm);
  45         modbus->slave_addr = modbusaddr;
  46
  47         return SR_OK;
  48 }
  49
  50 static int modbus_serial_rtu_open(void *priv)
  51 {
  52         struct modbus_serial_rtu *modbus = priv;
  53         struct sr_serial_dev_inst *serial = modbus->serial;
  54
  55         if (serial_open(serial, SERIAL_RDWR) != SR_OK)
  56                 return SR_ERR;
  57
  58         if (serial_flush(serial) != SR_OK)
  59                 return SR_ERR;
  60
  61         return SR_OK;
  62 }
  63
  64 static int modbus_serial_rtu_source_add(struct sr_session *session, void *priv,
  65                 int events, int timeout, sr_receive_data_callback cb, void *cb_data)
  66 {
  67         struct modbus_serial_rtu *modbus = priv;
  68         struct sr_serial_dev_inst *serial = modbus->serial;
  69
  70         return serial_source_add(session, serial, events, timeout, cb, cb_data);
  71 }
  72
  73 static int modbus_serial_rtu_source_remove(struct sr_session *session,
  74                 void *priv)
  75 {
  76         struct modbus_serial_rtu *modbus = priv;
  77         struct sr_serial_dev_inst *serial = modbus->serial;
  78
  79         return serial_source_remove(session, serial);
  80 }
  81
  82 static uint16_t modbus_serial_rtu_crc(uint16_t crc,
  83                 const uint8_t *buffer, int len)
  84 {
  85         int i;
  86
  87         if (!buffer || len < 0)
  88                 return crc;
  89
  90         while (len--) {
  91                 crc ^= *buffer++;
  92                 for (i = 0; i < 8; i++) {
  93                         int carry = crc & 1;
  94                         crc >>= 1;
  95                         if (carry)
  96                                 crc ^= 0xA001;
  97                 }
  98         }
  99
 100         return crc;
 101 }
 102
 103 static int modbus_serial_rtu_send(void *priv,
 104                 const uint8_t *buffer, int buffer_size)
 105 {
 106         int result;
 107         struct modbus_serial_rtu *modbus = priv;
 108         struct sr_serial_dev_inst *serial = modbus->serial;
 109         uint8_t slave_addr = modbus->slave_addr;
 110         uint16_t crc;
 111
 112         result = serial_write_blocking(serial, &slave_addr, sizeof(slave_addr), 0);
 113         if (result < 0)
 114                 return SR_ERR;
 115
 116         result = serial_write_blocking(serial, buffer, buffer_size, 0);
 117         if (result < 0)
 118                 return SR_ERR;
 119
 120         crc = modbus_serial_rtu_crc(0xFFFF, &slave_addr, sizeof(slave_addr));
 121         crc = modbus_serial_rtu_crc(crc, buffer, buffer_size);
 122
 123         result = serial_write_blocking(serial, &crc, sizeof(crc), 0);
 124         if (result < 0)
 125                 return SR_ERR;
 126
 127         return SR_OK;
 128 }
 129
 130 static int modbus_serial_rtu_read_begin(void *priv, uint8_t *function_code)
 131 {
 132         struct modbus_serial_rtu *modbus = priv;
 133         uint8_t slave_addr;
 134         int ret;
 135
 136         ret = serial_read_blocking(modbus->serial, &slave_addr, 1, 500);
 137         if (ret != 1 || slave_addr != modbus->slave_addr)
 138                 return SR_ERR;
 139
 140         ret = serial_read_blocking(modbus->serial, function_code, 1, 100);
 141         if (ret != 1)
 142                 return SR_ERR;
 143
 144         modbus->crc = modbus_serial_rtu_crc(0xFFFF, &slave_addr, sizeof(slave_addr));
 145         modbus->crc = modbus_serial_rtu_crc(modbus->crc, function_code, 1);
 146
 147         return SR_OK;
 148 }
 149
 150 static int modbus_serial_rtu_read_data(void *priv, uint8_t *buf, int maxlen)
 151 {
 152         struct modbus_serial_rtu *modbus = priv;
 153         int ret;
 154
 155         ret = serial_read_nonblocking(modbus->serial, buf, maxlen);
 156         if (ret < 0)
 157                 return ret;
 158         modbus->crc = modbus_serial_rtu_crc(modbus->crc, buf, ret);
 159         return ret;
 160 }
 161
 162 static int modbus_serial_rtu_read_end(void *priv)
 163 {
 164         struct modbus_serial_rtu *modbus = priv;
 165         uint16_t crc;
 166         int ret;
 167
 168         ret = serial_read_blocking(modbus->serial, &crc, sizeof(crc), 100);
 169         if (ret != 2)
 170                 return SR_ERR;
 171
 172         if (crc != modbus->crc) {
 173                 sr_err("CRC error (0x%04X vs 0x%04X).", crc, modbus->crc);
 174                 return SR_ERR_DATA;
 175         }
 176
 177         return SR_OK;
 178 }
 179
 180 static int modbus_serial_rtu_close(void *priv)
 181 {
 182         struct modbus_serial_rtu *modbus = priv;
 183
 184         return serial_close(modbus->serial);
 185 }
 186
 187 static void modbus_serial_rtu_free(void *priv)
 188 {
 189         struct modbus_serial_rtu *modbus = priv;
 190
 191         sr_serial_dev_inst_free(modbus->serial);
 192 }
 193
 194 SR_PRIV const struct sr_modbus_dev_inst modbus_serial_rtu_dev = {
 195         .name          = "serial_rtu",
 196         .prefix        = "",
 197         .priv_size     = sizeof(struct modbus_serial_rtu),
 198         .scan          = NULL,
 199         .dev_inst_new  = modbus_serial_rtu_dev_inst_new,
 200         .open          = modbus_serial_rtu_open,
 201         .source_add    = modbus_serial_rtu_source_add,
 202         .source_remove = modbus_serial_rtu_source_remove,
 203         .send          = modbus_serial_rtu_send,
 204         .read_begin    = modbus_serial_rtu_read_begin,
 205         .read_data     = modbus_serial_rtu_read_data,
 206         .read_end      = modbus_serial_rtu_read_end,
 207         .close         = modbus_serial_rtu_close,
 208         .free          = modbus_serial_rtu_free,
 209 };