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