2 * This file is part of the libsigrok project.
4 * Copyright (C) 2015 Aurelien Jacobs <aurel@gnuage.org>
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.
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.
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/>.
23 #include <libsigrok/libsigrok.h>
24 #include "libsigrok-internal.h"
26 #define LOG_PREFIX "modbus"
28 SR_PRIV extern const struct sr_modbus_dev_inst modbus_serial_rtu_dev;
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. */
36 static const unsigned int modbus_devs_size = ARRAY_SIZE(modbus_devs);
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))
42 struct sr_modbus_dev_inst *modbus;
43 struct sr_dev_inst *sdi;
45 if (!(modbus = modbus_dev_inst_new(resource, serialcomm, modbusaddr)))
48 if (sr_modbus_open(modbus) != SR_OK) {
49 sr_info("Couldn't open Modbus device.");
50 sr_modbus_free(modbus);
54 if ((sdi = probe_device(modbus)))
57 sr_modbus_close(modbus);
58 sr_modbus_free(modbus);
64 * Scan for Modbus devices which match a probing function.
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.
72 * @return A list of the devices found or NULL if no devices were found.
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))
77 GSList *resources, *l, *devices;
78 struct sr_dev_inst *sdi;
79 const char *resource = NULL;
80 const char *serialcomm = NULL;
85 for (l = options; l; l = l->next) {
86 struct sr_config *src = l->data;
89 resource = g_variant_get_string(src->data, NULL);
91 case SR_CONF_SERIALCOMM:
92 serialcomm = g_variant_get_string(src->data, NULL);
94 case SR_CONF_MODBUSADDR:
95 modbusaddr = g_variant_get_uint64(src->data);
101 for (i = 0; i < modbus_devs_size; i++) {
102 if ((resource && strcmp(resource, modbus_devs[i]->prefix))
103 || !modbus_devs[i]->scan)
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);
116 g_slist_free_full(resources, g_free);
119 if (!devices && resource) {
120 sdi = sr_modbus_scan_resource(resource, serialcomm, modbusaddr,
123 devices = g_slist_append(NULL, sdi);
126 /* Tack a copy of the newly found devices onto the driver list. */
128 drvc->instances = g_slist_concat(drvc->instances, g_slist_copy(devices));
134 * Allocate and initialize a struct for a Modbus device instance.
136 * @param resource The resource description string.
137 * @param serialcomm Additionnal parameters for serial port resources.
139 * @return The allocated sr_modbus_dev_inst structure or NULL on failure.
141 SR_PRIV struct sr_modbus_dev_inst *modbus_dev_inst_new(const char *resource,
142 const char *serialcomm, int modbusaddr)
144 struct sr_modbus_dev_inst *modbus = NULL;
145 const struct sr_modbus_dev_inst *modbus_dev;
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);
172 * Open the specified Modbus device.
174 * @param modbus Previously initialized Modbus device structure.
176 * @return SR_OK on success, SR_ERR on failure.
178 SR_PRIV int sr_modbus_open(struct sr_modbus_dev_inst *modbus)
180 return modbus->open(modbus->priv);
184 * Add an event source for a Modbus device.
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.
193 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
194 * SR_ERR_MALLOC upon memory allocation errors.
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)
200 return modbus->source_add(session, modbus->priv, events, timeout, cb, cb_data);
204 * Remove event source for a Modbus device.
206 * @param session The session to remove the event source from.
207 * @param modbus Previously initialized Modbus device structure.
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
213 SR_PRIV int sr_modbus_source_remove(struct sr_session *session,
214 struct sr_modbus_dev_inst *modbus)
216 return modbus->source_remove(session, modbus->priv);
220 * Send a Modbus command.
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.
226 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
229 SR_PRIV int sr_modbus_request(struct sr_modbus_dev_inst *modbus,
230 uint8_t *request, int request_size)
232 if (!request || request_size < 1)
235 return modbus->send(modbus->priv, request, request_size);
239 * Receive a Modbus reply.
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.
245 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
248 SR_PRIV int sr_modbus_reply(struct sr_modbus_dev_inst *modbus,
249 uint8_t *reply, int reply_size)
253 unsigned int elapsed_ms;
255 if (!reply || reply_size < 2)
258 laststart = g_get_monotonic_time();
260 ret = modbus->read_begin(modbus->priv, reply);
269 while (reply_size > 0) {
270 len = modbus->read_data(modbus->priv, reply, reply_size);
272 sr_err("Incompletely read Modbus response.");
274 } else if (len > 0) {
275 laststart = g_get_monotonic_time();
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.");
286 ret = modbus->read_end(modbus->priv);
294 * Send a Modbus command and receive the corresponding reply.
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.
302 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments, or
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)
309 ret = sr_modbus_request(modbus, request, request_size);
312 return sr_modbus_reply(modbus, reply, reply_size);
316 MODBUS_READ_COILS = 0x01,
317 MODBUS_READ_HOLDING_REGISTERS = 0x03,
318 MODBUS_WRITE_COIL = 0x05,
319 MODBUS_WRITE_MULTIPLE_REGISTERS = 0x10,
322 static int sr_modbus_error_check(const uint8_t *reply)
324 const char *function = "UNKNOWN";
325 const char *error = NULL;
328 if (!(reply[0] & 0x80))
331 switch (reply[0] & ~0x80) {
332 case MODBUS_READ_COILS:
333 function = "MODBUS_READ_COILS";
335 case MODBUS_READ_HOLDING_REGISTERS:
336 function = "READ_HOLDING_REGISTERS";
338 case MODBUS_WRITE_COIL:
339 function = "WRITE_COIL";
341 case MODBUS_WRITE_MULTIPLE_REGISTERS:
342 function = "WRITE_MULTIPLE_REGISTERS";
348 error = "ILLEGAL FUNCTION";
351 error = "ILLEGAL DATA ADDRESS";
354 error = "ILLEGAL DATA VALUE";
357 error = "SLAVE DEVICE FAILURE";
360 error = "ACKNOWLEDGE";
363 error = "SLAVE DEVICE BUSY";
366 error = "MEMORY PARITY ERROR";
369 error = "GATEWAY PATH UNAVAILABLE";
372 error = "GATEWAY TARGET DEVICE FAILED TO RESPOND";
376 snprintf(buf, sizeof(buf), "0x%X", reply[1]);
380 sr_err("%s error executing %s function.", error, function);
386 * Send a Modbus read coils command and receive the corresponding coils values.
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.
395 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
396 * SR_ERR_DATA upon invalid data, or SR_ERR on failure.
398 SR_PRIV int sr_modbus_read_coils(struct sr_modbus_dev_inst *modbus,
399 int address, int nb_coils, uint8_t *coils)
401 uint8_t request[5], reply[2 + (nb_coils + 7) / 8];
404 if (address < -1 || address > 0xFFFF || nb_coils < 1 || nb_coils > 2000)
407 W8(request + 0, MODBUS_READ_COILS);
408 WB16(request + 1, address);
409 WB16(request + 3, nb_coils);
412 ret = sr_modbus_request(modbus, request, sizeof(request));
418 ret = sr_modbus_reply(modbus, reply, sizeof(reply));
421 if (sr_modbus_error_check(reply))
423 if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)((nb_coils + 7) / 8))
425 memcpy(coils, reply + 2, (nb_coils + 7) / 8);
432 * Send a Modbus read holding registers command and receive the corresponding
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.
443 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
444 * SR_ERR_DATA upon invalid data, or SR_ERR on failure.
446 SR_PRIV int sr_modbus_read_holding_registers(struct sr_modbus_dev_inst *modbus,
447 int address, int nb_registers, uint16_t *registers)
449 uint8_t request[5], reply[2 + (2 * nb_registers)];
452 if (address < -1 || address > 0xFFFF
453 || nb_registers < 1 || nb_registers > 125)
456 W8(request + 0, MODBUS_READ_HOLDING_REGISTERS);
457 WB16(request + 1, address);
458 WB16(request + 3, nb_registers);
461 ret = sr_modbus_request(modbus, request, sizeof(request));
467 ret = sr_modbus_reply(modbus, reply, sizeof(reply));
470 if (sr_modbus_error_check(reply))
472 if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)(2 * nb_registers))
474 memcpy(registers, reply + 2, 2 * nb_registers);
481 * Send a Modbus write coil command.
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.
487 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
488 * SR_ERR_DATA upon invalid data, or SR_ERR on failure.
490 SR_PRIV int sr_modbus_write_coil(struct sr_modbus_dev_inst *modbus,
491 int address, int value)
493 uint8_t request[5], reply[5];
496 if (address < 0 || address > 0xFFFF)
499 W8(request + 0, MODBUS_WRITE_COIL);
500 WB16(request + 1, address);
501 WB16(request + 3, value ? 0xFF00 : 0);
503 ret = sr_modbus_request_reply(modbus, request, sizeof(request),
504 reply, sizeof(reply));
507 if (sr_modbus_error_check(reply))
509 if (memcmp(request, reply, sizeof(reply)))
516 * Send a Modbus write multiple registers command.
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.
523 * @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
524 * SR_ERR_DATA upon invalid data, or SR_ERR on failure.
526 SR_PRIV int sr_modbus_write_multiple_registers(struct sr_modbus_dev_inst*modbus,
527 int address, int nb_registers, uint16_t *registers)
529 uint8_t request[6 + (2 * nb_registers)], reply[5];
532 if (address < 0 || address > 0xFFFF
533 || nb_registers < 1 || nb_registers > 123 || !registers)
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);
542 ret = sr_modbus_request_reply(modbus, request, sizeof(request),
543 reply, sizeof(reply));
546 if (sr_modbus_error_check(reply))
548 if (memcmp(request, reply, sizeof(reply)))
555 * Close Modbus device.
557 * @param modbus Previously initialized Modbus device structure.
559 * @return SR_OK on success, SR_ERR on failure.
561 SR_PRIV int sr_modbus_close(struct sr_modbus_dev_inst *modbus)
563 return modbus->close(modbus->priv);
567 * Free Modbus device.
569 * @param modbus Previously initialized Modbus device structure.
571 * @return SR_OK on success, SR_ERR on failure.
573 SR_PRIV void sr_modbus_free(struct sr_modbus_dev_inst *modbus)
575 modbus->free(modbus->priv);
576 g_free(modbus->priv);