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