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