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