src/modbus/modbus.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 "libsigrok.h"
  21 #include "libsigrok-internal.h"
  22
  23 #include <glib.h>
  24 #include <string.h>
  25
  26 #define LOG_PREFIX "modbus"
  27
  28 static const struct sr_modbus_dev_inst *modbus_devs[] = {
  29 };
  30
  31 static struct sr_dev_inst *sr_modbus_scan_resource(const char *resource,
  32                 const char *serialcomm, int modbusaddr,
  33                 struct sr_dev_inst *(*probe_device)(struct sr_modbus_dev_inst *modbus))
  34 {
  35         struct sr_modbus_dev_inst *modbus;
  36         struct sr_dev_inst *sdi;
  37
  38         if (!(modbus = modbus_dev_inst_new(resource, serialcomm, modbusaddr)))
  39                 return NULL;
  40
  41         if (sr_modbus_open(modbus) != SR_OK) {
  42                 sr_info("Couldn't open MODBUS device.");
  43                 sr_modbus_free(modbus);
  44                 return NULL;
  45         };
  46
  47         if ((sdi = probe_device(modbus)))
  48                 return sdi;
  49
  50         sr_modbus_close(modbus);
  51         sr_modbus_free(modbus);
  52         return NULL;
  53 }
  54
  55 /**
  56  *  Scan for MODBUS devices which match a probing function.
  57  *
  58  *  @param drvc the driver context doing the scan.
  59  *  @param options the scan options to find devies.
  60  *  @param probe_device the callback function that will be called for each
  61  *                      found devices to validate wheter this device matches
  62  *                      what we are scanning for.
  63  *
  64  *  @return a list of the devices found or NULL if no device found.
  65  */
  66 SR_PRIV GSList *sr_modbus_scan(struct drv_context *drvc, GSList *options,
  67                 struct sr_dev_inst *(*probe_device)(struct sr_modbus_dev_inst *modbus))
  68 {
  69         GSList *resources, *l, *devices;
  70         struct sr_dev_inst *sdi;
  71         const char *resource = NULL;
  72         const char *serialcomm = NULL;
  73         int modbusaddr = 1;
  74         gchar **res;
  75         unsigned i;
  76
  77         for (l = options; l; l = l->next) {
  78                 struct sr_config *src = l->data;
  79                 switch (src->key) {
  80                 case SR_CONF_CONN:
  81                         resource = g_variant_get_string(src->data, NULL);
  82                         break;
  83                 case SR_CONF_SERIALCOMM:
  84                         serialcomm = g_variant_get_string(src->data, NULL);
  85                         break;
  86                 case SR_CONF_MODBUSADDR:
  87                         modbusaddr = g_variant_get_uint64(src->data);
  88                         break;
  89                 }
  90         }
  91
  92         devices = NULL;
  93         for (i = 0; i < ARRAY_SIZE(modbus_devs); i++) {
  94                 if ((resource && strcmp(resource, modbus_devs[i]->prefix))
  95                     || !modbus_devs[i]->scan)
  96                         continue;
  97                 resources = modbus_devs[i]->scan(modbusaddr);
  98                 for (l = resources; l; l = l->next) {
  99                         res = g_strsplit(l->data, ":", 2);
 100                         if (res[0] && (sdi = sr_modbus_scan_resource(res[0],
 101                                                          serialcomm ? serialcomm : res[1],
 102                                                          modbusaddr, probe_device))) {
 103                                 devices = g_slist_append(devices, sdi);
 104                                 sdi->connection_id = g_strdup(l->data);
 105                         }
 106                         g_strfreev(res);
 107                 }
 108                 g_slist_free_full(resources, g_free);
 109         }
 110
 111         if (!devices && resource) {
 112                 sdi = sr_modbus_scan_resource(resource, serialcomm, modbusaddr,
 113                                               probe_device);
 114                 if (sdi)
 115                         devices = g_slist_append(NULL, sdi);
 116         }
 117
 118         /* Tack a copy of the newly found devices onto the driver list. */
 119         if (devices)
 120                 drvc->instances = g_slist_concat(drvc->instances, g_slist_copy(devices));
 121
 122         return devices;
 123 }
 124
 125 /**
 126  *  Allocate and initialize struct for a MODBUS device instance.
 127  *
 128  *  @param resource the resource description string.
 129  *  @param serialcomm additionnal parameters for serial port resources.
 130  *
 131  *  @return the allocated sr_modbus_dev_inst structure or NULL on failure.
 132  */
 133 SR_PRIV struct sr_modbus_dev_inst *modbus_dev_inst_new(const char *resource,
 134                 const char *serialcomm, int modbusaddr)
 135 {
 136         struct sr_modbus_dev_inst *modbus = NULL;
 137         const struct sr_modbus_dev_inst *modbus_dev;
 138         gchar **params;
 139         unsigned i;
 140
 141         for (i = 0; i < ARRAY_SIZE(modbus_devs); i++) {
 142                 modbus_dev = modbus_devs[i];
 143                 if (!strncmp(resource, modbus_dev->prefix, strlen(modbus_dev->prefix))) {
 144                         sr_dbg("Opening %s device %s.", modbus_dev->name, resource);
 145                         modbus = g_malloc(sizeof(*modbus));
 146                         *modbus = *modbus_dev;
 147                         modbus->priv = g_malloc0(modbus->priv_size);
 148                         modbus->read_timeout_ms = 1000;
 149                         params = g_strsplit(resource, "/", 0);
 150                         if (modbus->dev_inst_new(modbus->priv, resource,
 151                                                  params, serialcomm, modbusaddr) != SR_OK) {
 152                                 sr_modbus_free(modbus);
 153                                 modbus = NULL;
 154                         }
 155                         g_strfreev(params);
 156                         break;
 157                 }
 158         }
 159
 160         return modbus;
 161 }
 162
 163 /**
 164  * Open MODBUS device.
 165  *
 166  * @param modbus Previously initialized MODBUS device structure.
 167  *
 168  * @return SR_OK on success, SR_ERR on failure.
 169  */
 170 SR_PRIV int sr_modbus_open(struct sr_modbus_dev_inst *modbus)
 171 {
 172         return modbus->open(modbus->priv);
 173 }
 174
 175 /**
 176  * Add an event source for an MODBUS device.
 177  *
 178  * @param session The session to add the event source to.
 179  * @param modbus Previously initialized MODBUS device structure.
 180  * @param events Events to check for.
 181  * @param timeout Max time to wait before the callback is called, ignored if 0.
 182  * @param cb Callback function to add. Must not be NULL.
 183  * @param cb_data Data for the callback function. Can be NULL.
 184  *
 185  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
 186  *         SR_ERR_MALLOC upon memory allocation errors.
 187  */
 188 SR_PRIV int sr_modbus_source_add(struct sr_session *session,
 189                 struct sr_modbus_dev_inst *modbus, int events, int timeout,
 190                 sr_receive_data_callback cb, void *cb_data)
 191 {
 192         return modbus->source_add(session, modbus->priv, events, timeout, cb, cb_data);
 193 }
 194
 195 /**
 196  * Remove event source for an MODBUS device.
 197  *
 198  * @param session The session to remove the event source from.
 199  * @param modbus Previously initialized MODBUS device structure.
 200  *
 201  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
 202  *         SR_ERR_MALLOC upon memory allocation errors, SR_ERR_BUG upon
 203  *         internal errors.
 204  */
 205 SR_PRIV int sr_modbus_source_remove(struct sr_session *session,
 206                 struct sr_modbus_dev_inst *modbus)
 207 {
 208         return modbus->source_remove(session, modbus->priv);
 209 }
 210
 211 /**
 212  * Send a MODBUS command.
 213  *
 214  * @param modbus Previously initialized MODBUS device structure.
 215  * @param request buffer containing the MODBUS command to send.
 216  * @param request_size the size of the request buffer.
 217  *
 218  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
 219  *         SR_ERR on failure.
 220  */
 221 SR_PRIV int sr_modbus_request(struct sr_modbus_dev_inst *modbus,
 222                 uint8_t *request, int request_size)
 223 {
 224         if (!request || request_size < 1)
 225                 return SR_ERR_ARG;
 226
 227         return modbus->send(modbus->priv, request, request_size);
 228 }
 229
 230 /**
 231  * Receive a MODBUS reply.
 232  *
 233  * @param modbus Previously initialized MODBUS device structure.
 234  * @param reply buffer to store the received MODBUS reply.
 235  * @param reply_size the size of the reply buffer.
 236  *
 237  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
 238  *         SR_ERR on failure.
 239  */
 240 SR_PRIV int sr_modbus_reply(struct sr_modbus_dev_inst *modbus,
 241                 uint8_t *reply, int reply_size)
 242 {
 243         int len, ret;
 244         gint64 laststart;
 245         unsigned int elapsed_ms;
 246
 247         if (!reply || reply_size < 2)
 248                 return SR_ERR_ARG;
 249
 250         laststart = g_get_monotonic_time();
 251
 252         ret = modbus->read_begin(modbus->priv, reply);
 253         if (ret != SR_OK)
 254                 return ret;
 255         if (*reply & 0x80)
 256                 reply_size = 2;
 257
 258         reply++;
 259         reply_size--;
 260
 261         while (reply_size > 0) {
 262                 len = modbus->read_data(modbus->priv, reply, reply_size);
 263                 if (len < 0) {
 264                         sr_err("Incompletely read MODBUS response.");
 265                         return SR_ERR;
 266                 } else if (len > 0) {
 267                         laststart = g_get_monotonic_time();
 268                 }
 269                 reply += len;
 270                 reply_size -= len;
 271                 elapsed_ms = (g_get_monotonic_time() - laststart) / 1000;
 272                 if (elapsed_ms >= modbus->read_timeout_ms) {
 273                         sr_err("Timed out waiting for MODBUS response.");
 274                         return SR_ERR;
 275                 }
 276         }
 277
 278         ret = modbus->read_end(modbus->priv);
 279         if (ret != SR_OK)
 280                 return ret;
 281
 282         return SR_OK;
 283 }
 284
 285 /**
 286  * Send a MODBUS command and receive the corresponding reply.
 287  *
 288  * @param modbus Previously initialized MODBUS device structure.
 289  * @param request buffer containing the MODBUS command to send.
 290  * @param request_size the size of the request buffer.
 291  * @param reply buffer to store the received MODBUS reply.
 292  * @param reply_size the size of the reply buffer.
 293  *
 294  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
 295  *         SR_ERR on failure.
 296  */
 297 SR_PRIV int sr_modbus_request_reply(struct sr_modbus_dev_inst *modbus,
 298                 uint8_t *request, int request_size, uint8_t *reply, int reply_size)
 299 {
 300         int ret;
 301         ret = sr_modbus_request(modbus, request, request_size);
 302         if (ret != SR_OK)
 303                 return ret;
 304         return sr_modbus_reply(modbus, reply, reply_size);
 305 }
 306
 307 enum {
 308         MODBUS_READ_COILS               = 0x01,
 309         MODBUS_READ_HOLDING_REGISTERS   = 0x03,
 310         MODBUS_WRITE_COIL               = 0x05,
 311         MODBUS_WRITE_MULTIPLE_REGISTERS = 0x10,
 312 };
 313
 314 static int sr_modbus_error_check(const uint8_t *reply)
 315 {
 316         const char *function = "UNKNOWN";
 317         const char *error = NULL;
 318         char buf[8];
 319
 320         if (!(reply[0] & 0x80))
 321                 return FALSE;
 322
 323         switch (reply[0] & ~0x80) {
 324         case MODBUS_READ_COILS:
 325                 function = "MODBUS_READ_COILS";  break;
 326         case MODBUS_READ_HOLDING_REGISTERS:
 327                 function = "READ_HOLDING_REGISTERS";  break;
 328         case MODBUS_WRITE_COIL:
 329                 function = "WRITE_COIL"; break;
 330         case MODBUS_WRITE_MULTIPLE_REGISTERS:
 331                 function = "WRITE_MULTIPLE_REGISTERS"; break;
 332         }
 333
 334         switch (reply[1]) {
 335         case 0x01:
 336                 error = "ILLEGAL FUNCTION";  break;
 337         case 0x02:
 338                 error = "ILLEGAL DATA ADDRESS";  break;
 339         case 0x03:
 340                 error = "ILLEGAL DATA VALUE"; break;
 341         case 0x04:
 342                 error = "SLAVE DEVICE FAILURE"; break;
 343         case 0x05:
 344                 error = "ACKNOWLEDGE"; break;
 345         case 0x06:
 346                 error = "SLAVE DEVICE BUSY"; break;
 347         case 0x08:
 348                 error = "MEMORY PARITY ERROR"; break;
 349         case 0x0A:
 350                 error = "GATEWAY PATH UNAVAILABLE"; break;
 351         case 0x0B:
 352                 error = "GATEWAY TARGET DEVICE FAILED TO RESPOND"; break;
 353         }
 354         if (!error) {
 355                 snprintf(buf, sizeof(buf), "0x%X", reply[1]);
 356                 error = buf;
 357         }
 358
 359         sr_err("%s error executing %s function.", error, function);
 360         return TRUE;
 361 }
 362
 363 /**
 364  * Send a MODBUS read coils command and receive the corresponding coils values.
 365  *
 366  * @param modbus Previously initialized MODBUS device structure.
 367  * @param address the MODBUS address of the first coil to read,
 368  *                or -1 to read the reply of a previouly sent
 369  *                read coils command.
 370  * @param nb_coils the number of coils to read.
 371  * @param coils buffer to store all the received coils values (1 bit per coil),
 372  *              or NULL to send the read coil command without reading the reply.
 373  *
 374  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
 375  *         SR_ERR_DATA upon invalid data, or SR_ERR on failure.
 376  */
 377 SR_PRIV int sr_modbus_read_coils(struct sr_modbus_dev_inst *modbus,
 378                 int address, int nb_coils, uint8_t *coils)
 379 {
 380         uint8_t request[5], reply[2 + (nb_coils + 7) / 8];
 381         int ret;
 382
 383         if (address < -1 || address > 0xFFFF || nb_coils < 1 || nb_coils > 2000)
 384                 return SR_ERR_ARG;
 385
 386         W8  (request+0, MODBUS_READ_COILS);
 387         WB16(request+1, address);
 388         WB16(request+3, nb_coils);
 389
 390         if (address >= 0) {
 391                 ret = sr_modbus_request(modbus, request, sizeof(request));
 392                 if (ret != SR_OK)
 393                         return ret;
 394         }
 395
 396         if (coils) {
 397                 ret = sr_modbus_reply(modbus, reply, sizeof(reply));
 398                 if (ret != SR_OK)
 399                         return ret;
 400                 if (sr_modbus_error_check(reply))
 401                         return SR_ERR_DATA;
 402                 if (reply[0] != request[0] || R8(reply+1) != (uint8_t)((nb_coils+7)/8))
 403                         return SR_ERR_DATA;
 404                 memcpy(coils, reply+2, (nb_coils+7)/8);
 405         }
 406
 407         return SR_OK;
 408 }
 409
 410 /**
 411  * Send a MODBUS read holding registers command and receive the corresponding
 412  * registers values.
 413  *
 414  * @param modbus Previously initialized MODBUS device structure.
 415  * @param address the MODBUS address of the first register to read,
 416  *                or -1 to read the reply of a previouly sent
 417  *                read registers command.
 418  * @param nb_registers the number of registers to read.
 419  * @param registers buffer to store all the received registers values,
 420  *                  or NULL to send the read holding registers command
 421  *                  without reading the reply.
 422  *
 423  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
 424  *         SR_ERR_DATA upon invalid data, or SR_ERR on failure.
 425  */
 426 SR_PRIV int sr_modbus_read_holding_registers(struct sr_modbus_dev_inst *modbus,
 427                 int address, int nb_registers, uint16_t *registers)
 428 {
 429         uint8_t request[5], reply[2 + 2*nb_registers];
 430         int ret;
 431
 432         if (address < -1 || address > 0xFFFF
 433             || nb_registers < 1 || nb_registers > 125)
 434                 return SR_ERR_ARG;
 435
 436         W8  (request+0, MODBUS_READ_HOLDING_REGISTERS);
 437         WB16(request+1, address);
 438         WB16(request+3, nb_registers);
 439
 440         if (address >= 0) {
 441                 ret = sr_modbus_request(modbus, request, sizeof(request));
 442                 if (ret != SR_OK)
 443                         return ret;
 444         }
 445
 446         if (registers) {
 447                 ret = sr_modbus_reply(modbus, reply, sizeof(reply));
 448                 if (ret != SR_OK)
 449                         return ret;
 450                 if (sr_modbus_error_check(reply))
 451                         return SR_ERR_DATA;
 452                 if (reply[0] != request[0] || R8(reply+1) != (uint8_t)(2*nb_registers))
 453                         return SR_ERR_DATA;
 454                 memcpy(registers, reply+2, 2*nb_registers);
 455         }
 456
 457         return SR_OK;
 458 }
 459
 460 /**
 461  * Send a MODBUS write coil command.
 462  *
 463  * @param modbus Previously initialized MODBUS device structure.
 464  * @param address the MODBUS address of the coil to write.
 465  * @param value the new value to assign to this coil.
 466  *
 467  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
 468  *         SR_ERR_DATA upon invalid data, or SR_ERR on failure.
 469  */
 470 SR_PRIV int sr_modbus_write_coil(struct sr_modbus_dev_inst *modbus,
 471                 int address, int value)
 472 {
 473         uint8_t request[5], reply[5];
 474         int ret;
 475
 476         if (address < 0 || address > 0xFFFF)
 477                 return SR_ERR_ARG;
 478
 479         W8  (request+0, MODBUS_WRITE_COIL);
 480         WB16(request+1, address);
 481         WB16(request+3, value ? 0xFF00 : 0);
 482
 483         ret = sr_modbus_request_reply(modbus, request, sizeof(request),
 484                                               reply  , sizeof(reply));
 485         if (ret != SR_OK)
 486                 return ret;
 487         if (sr_modbus_error_check(reply))
 488                 return SR_ERR_DATA;
 489         if (memcmp(request, reply, sizeof(reply)))
 490                 return SR_ERR_DATA;
 491         return SR_OK;
 492 }
 493
 494 /**
 495  * Send a MODBUS write multiple registers command.
 496  *
 497  * @param modbus Previously initialized MODBUS device structure.
 498  * @param address the MODBUS address of the first register to write.
 499  * @param nb_registers the number of registers to write.
 500  * @param registers buffer holding all the registers values to write.
 501  *
 502  * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
 503  *         SR_ERR_DATA upon invalid data, or SR_ERR on failure.
 504  */
 505 SR_PRIV int sr_modbus_write_multiple_registers(struct sr_modbus_dev_inst*modbus,
 506                 int address, int nb_registers, uint16_t *registers)
 507 {
 508         uint8_t request[6+2*nb_registers], reply[5];
 509         int ret;
 510
 511         if (address < 0 || address > 0xFFFF
 512             || nb_registers < 1 || nb_registers > 123 || !registers)
 513                 return SR_ERR_ARG;
 514
 515         W8  (request+0, MODBUS_WRITE_MULTIPLE_REGISTERS);
 516         WB16(request+1, address);
 517         WB16(request+3, nb_registers);
 518         W8  (request+5, 2*nb_registers);
 519         memcpy(request+6, registers, 2*nb_registers);
 520
 521         ret = sr_modbus_request_reply(modbus, request, sizeof(request),
 522                                               reply  , sizeof(reply));
 523         if (ret != SR_OK)
 524                 return ret;
 525         if (sr_modbus_error_check(reply))
 526                 return SR_ERR_DATA;
 527         if (memcmp(request, reply, sizeof(reply)))
 528                 return SR_ERR_DATA;
 529         return SR_OK;
 530 }
 531
 532 /**
 533  * Close MODBUS device.
 534  *
 535  * @param modbus Previously initialized MODBUS device structure.
 536  *
 537  * @return SR_OK on success, SR_ERR on failure.
 538  */
 539 SR_PRIV int sr_modbus_close(struct sr_modbus_dev_inst *modbus)
 540 {
 541         return modbus->close(modbus->priv);
 542 }
 543
 544 /**
 545  * Free MODBUS device.
 546  *
 547  * @param modbus Previously initialized MODBUS device structure.
 548  *
 549  * @return SR_OK on success, SR_ERR on failure.
 550  */
 551 SR_PRIV void sr_modbus_free(struct sr_modbus_dev_inst *modbus)
 552 {
 553         modbus->free(modbus->priv);
 554         g_free(modbus->priv);
 555         g_free(modbus);
 556 }