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