Jason Engelman
First Working Master with Adaptronic simulator
Revision 0:150538eb0bf3, committed 2011-07-24
- Comitter:
- tecnosys
- Date:
- Sun Jul 24 05:04:06 2011 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/modbus.c Sun Jul 24 05:04:06 2011 +0000
@@ -0,0 +1,1367 @@
+
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include <errno.h>
+#include <limits.h>
+
+
+#include "modbus.h"
+
+#include "mbed.h"
+extern Serial serial;
+
+#define UNKNOWN_ERROR_MSG "Not defined in modbus specification"
+
+/* This structure reduces the number of params in functions and so
+ * optimizes the speed of execution (~ 37%). */
+typedef struct {
+ int slave;
+ int function;
+ int t_id;
+} sft_t;
+
+static const uint8_t NB_TAB_ERROR_MSG = 12;
+static const char *TAB_ERROR_MSG[] = {
+ /* 0x00 */ UNKNOWN_ERROR_MSG,
+ /* 0x01 */ "Illegal function code",
+ /* 0x02 */ "Illegal data address",
+ /* 0x03 */ "Illegal data value",
+ /* 0x04 */ "Slave device or server failure",
+ /* 0x05 */ "Acknowledge",
+ /* 0x06 */ "Slave device or server busy",
+ /* 0x07 */ "Negative acknowledge",
+ /* 0x08 */ "Memory parity error",
+ /* 0x09 */ UNKNOWN_ERROR_MSG,
+ /* 0x0A */ "Gateway path unavailable",
+ /* 0x0B */ "Target device failed to respond"
+};
+
+/* Table of CRC values for high-order byte */
+static uint8_t table_crc_hi[] = {
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+ 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+ 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+ 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+ 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+ 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+ 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+ 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
+};
+
+/* Table of CRC values for low-order byte */
+static uint8_t table_crc_lo[] = {
+ 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
+ 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
+ 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
+ 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
+ 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
+ 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+ 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
+ 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
+ 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
+ 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
+ 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
+ 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+ 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
+ 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
+ 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
+ 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
+ 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
+ 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+ 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
+ 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
+ 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
+ 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
+ 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
+ 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+ 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
+ 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
+};
+
+/* Treats errors and flush or close connection if necessary */
+static void error_treat(modbus_param_t *mb_param, int code, const char *string)
+{
+ printf("\nERROR %s (%d)\n", string, code);
+
+ if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) {
+ switch (code) {
+ case ILLEGAL_DATA_VALUE:
+ case ILLEGAL_DATA_ADDRESS:
+ case ILLEGAL_FUNCTION:
+ break;
+ default:
+ // tcflush(mb_param->fd, TCIOFLUSH);
+
+ }
+ }
+}
+
+/* Computes the length of the expected response */
+static unsigned int compute_response_length(modbus_param_t *mb_param,
+ uint8_t *query)
+{
+ int length;
+ int offset;
+
+ offset = mb_param->header_length;
+
+ switch (query[offset + 1]) {
+ case FC_READ_COIL_STATUS:
+ case FC_READ_INPUT_STATUS: {
+ /* Header + nb values (code from force_multiple_coils) */
+ int nb = (query[offset + 4] << 8) | query[offset + 5];
+ length = 3 + (nb / 8) + ((nb % 8) ? 1 : 0);
+ }
+ break;
+ case FC_READ_HOLDING_REGISTERS:
+ case FC_READ_INPUT_REGISTERS:
+ /* Header + 2 * nb values */
+ length = 3 + 2 * (query[offset + 4] << 8 |
+ query[offset + 5]);
+ break;
+ case FC_READ_EXCEPTION_STATUS:
+ length = 4;
+ break;
+ default:
+ length = 6;
+ }
+
+ return length + offset + mb_param->checksum_length;
+}
+
+/* Builds a RTU query header */
+ int build_query_basis_rtu(int slave, int function,
+ int start_addr, int nb,
+ uint8_t *query)
+{
+ query[0] = slave;
+ query[1] = function;
+ query[2] = start_addr >> 8;
+ query[3] = start_addr & 0x00ff;
+ query[4] = nb >> 8;
+ query[5] = nb & 0x00ff;
+
+ return PRESET_QUERY_LENGTH_RTU;
+}
+
+
+static int build_query_basis(modbus_param_t *mb_param, int slave,
+ int function, int start_addr,
+ int nb, uint8_t *query)
+{
+
+ return build_query_basis_rtu(slave, function, start_addr,
+ nb, query);
+
+}
+
+/* Builds a RTU response header */
+static int build_response_basis_rtu(sft_t *sft, uint8_t *response)
+{
+ response[0] = sft->slave;
+ response[1] = sft->function;
+
+ return PRESET_RESPONSE_LENGTH_RTU;
+}
+
+static int build_response_basis(modbus_param_t *mb_param, sft_t *sft,
+ uint8_t *response)
+{
+
+ return build_response_basis_rtu(sft, response);
+
+}
+
+
+/* Fast CRC */
+static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
+{
+ uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
+ uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
+ unsigned int i; /* will index into CRC lookup */
+
+ /* pass through message buffer */
+ while (buffer_length--) {
+ i = crc_hi ^ *buffer++; /* calculate the CRC */
+ crc_hi = crc_lo ^ table_crc_hi[i];
+ crc_lo = table_crc_lo[i];
+ }
+
+ return (crc_hi << 8 | crc_lo);
+}
+
+/* If CRC is correct returns 0 else returns INVALID_CRC */
+static int check_crc16(modbus_param_t *mb_param,
+ uint8_t *msg,
+ const int msg_length)
+{
+ int ret;
+ uint16_t crc_calc;
+ uint16_t crc_received;
+
+ crc_calc = crc16(msg, msg_length - 2);
+ crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
+
+ /* Check CRC of msg */
+ if (crc_calc == crc_received) {
+ ret = 0;
+ } else {
+ char s_error[64];
+ sprintf(s_error,
+ "invalid crc received %0X - crc_calc %0X",
+ crc_received, crc_calc);
+ ret = INVALID_CRC;
+ error_treat(mb_param, ret, s_error);
+ }
+
+ return ret;
+}
+
+ int modbus_send(modbus_param_t *mb_param, uint8_t *query,
+ int query_length)
+{
+ int ret;
+ uint16_t s_crc;
+ int i;
+
+ s_crc = crc16(query, query_length);
+ query[query_length++] = s_crc >> 8;
+ query[query_length++] = s_crc & 0x00FF;
+
+ if (mb_param->debug) {
+ for (i = 0; i < query_length; i++)
+ printf("[%.2X]", query[i]);
+ printf("\n");
+ }
+
+ //mbed
+ for (i = 0; i < query_length; i++) serial.putc(query[i]);
+
+ // ret = write(mb_param->fd, query, query_length);
+
+ /* Return the number of bytes written (0 to n)
+ or PORT_SOCKET_FAILURE on error */
+ /*
+ if ((ret == -1) || (ret != query_length)) {
+ ret = PORT_SOCKET_FAILURE;
+ error_treat(mb_param, ret, "Write port/socket failure");
+ }
+ */
+ return ret;
+}
+
+/* Computes the length of the header following the function code */
+static uint8_t compute_query_length_header(int function)
+{
+ int length;
+
+ if (function <= FC_FORCE_SINGLE_COIL ||
+ function == FC_PRESET_SINGLE_REGISTER)
+ /* Read and single write */
+ length = 4;
+ else if (function == FC_FORCE_MULTIPLE_COILS ||
+ function == FC_PRESET_MULTIPLE_REGISTERS)
+ /* Multiple write */
+ length = 5;
+ else
+ length = 0;
+
+ return length;
+}
+
+/* Computes the length of the data to write in the query */
+static int compute_query_length_data(modbus_param_t *mb_param, uint8_t *msg)
+{
+ int function = msg[mb_param->header_length + 1];
+ int length;
+
+ if (function == FC_FORCE_MULTIPLE_COILS ||
+ function == FC_PRESET_MULTIPLE_REGISTERS)
+ length = msg[mb_param->header_length + 6];
+ else
+ length = 0;
+
+ length += mb_param->checksum_length;
+
+ return length;
+}
+
+
+
+/* Waits a reply from a modbus slave or a query from a modbus master.
+ This function blocks for timeout seconds if there is no reply.
+
+ In
+ - msg_length_computed must be set to MSG_LENGTH_UNDEFINED if undefined
+
+ Out
+ - msg is an array of uint8_t to receive the message
+ - p_msg_length, the variable is assigned to the number of
+ characters received. This value won't be greater than
+ msg_length_computed.
+
+ Returns 0 in success or a negative value if an error occured.
+*/
+ int receive_msg(modbus_param_t *mb_param,
+ int msg_length_computed,
+ uint8_t *msg, int *p_msg_length)
+{
+ int select_ret;
+ int read_ret;
+// fd_set rfds;
+// struct timeval tv;
+ int length_to_read;
+
+ uint8_t *p_msg;
+
+ enum { FUNCTION, BYTE, COMPLETE };
+ int state;
+
+ if (mb_param->debug) {
+ if (msg_length_computed == MSG_LENGTH_UNDEFINED)
+ printf("Waiting for a message...\n");
+ else
+ printf("Waiting for a message (%d bytes)...\n",
+ msg_length_computed);
+ }
+ printf("0");
+ /* Add a file descriptor to the set */
+ //FD_ZERO(&rfds);
+ //FD_SET(mb_param->fd, &rfds);
+
+ if (msg_length_computed == MSG_LENGTH_UNDEFINED) {
+ /* Wait for a message */
+ //tv.tv_sec = 60;
+ //tv.tv_usec = 0;
+
+ /* The message length is undefined (query receiving) so
+ * we need to analyse the message step by step.
+ * At the first step, we want to reach the function
+ * code because all packets have that information. */
+ msg_length_computed = mb_param->header_length + 2;
+ state = FUNCTION;
+ } else {
+ // tv.tv_sec = 0;
+ // tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME;
+ state = COMPLETE;
+ }
+
+ length_to_read = msg_length_computed;
+
+ select_ret = 1;
+ // WAIT_DATA();
+
+ /* Initialize the readin the message */
+ (*p_msg_length) = 0;
+ p_msg = msg;
+
+ while (select_ret) {
+
+ read_ret=0;
+ //read_ret = read(mb_param->fd, p_msg, length_to_read);
+ printf("1");
+ while(serial.readable()) {
+ p_msg[read_ret] = serial.getc();
+ printf("x");
+ read_ret++;
+ }
+ printf("2");
+ /* Sums bytes received */
+ (*p_msg_length) += read_ret;
+
+ /* Display the hex code of each character received */
+ if (mb_param->debug) {
+ int i;
+ for (i=0; i < read_ret; i++)
+ printf("<%.2X>", p_msg[i]);
+ }
+
+ if ((*p_msg_length) < msg_length_computed) {
+ /* Message incomplete */
+ length_to_read = msg_length_computed - (*p_msg_length);
+ } else {
+ switch (state) {
+ case FUNCTION:
+ /* Function code position */
+ length_to_read = compute_query_length_header(msg[mb_param->header_length + 1]);
+ msg_length_computed += length_to_read;
+ /* It's useless to check
+ p_msg_length_computed value in this
+ case (only defined values are used). */
+ state = BYTE;
+ break;
+ case BYTE:
+ length_to_read = compute_query_length_data(mb_param, msg);
+ msg_length_computed += length_to_read;
+ if (msg_length_computed > MAX_MESSAGE_LENGTH) {
+ error_treat(mb_param, TOO_MANY_DATA, "Too many data");
+ return TOO_MANY_DATA;
+ }
+ state = COMPLETE;
+ break;
+ case COMPLETE:
+ length_to_read = 0;
+ break;
+ }
+ }
+
+ /* Moves the pointer to receive other data */
+ p_msg = &(p_msg[read_ret]);
+
+ if (length_to_read > 0) {
+ /* If no character at the buffer wait
+ TIME_OUT_END_OF_TRAME before to generate an error. */
+ //tv.tv_sec = 0;
+ //tv.tv_usec = TIME_OUT_END_OF_TRAME;
+
+ //WAIT_DATA();
+ } else {
+ /* All chars are received */
+ select_ret = FALSE;
+ }
+ }
+
+ printf("8");
+ if (mb_param->debug)
+ printf("\n");
+
+ if (mb_param->type_com == RTU) {
+ printf("CRC");
+ check_crc16(mb_param, msg, (*p_msg_length));
+ }
+
+ /* OK */
+ return 0;
+}
+
+
+/* Receives the response and checks values (and checksum in RTU).
+
+ Returns:
+ - the number of values (bits or word) if success or the response
+ length if no value is returned
+ - less than 0 for exception errors
+
+ Note: all functions used to send or receive data with modbus return
+ these values. */
+ int modbus_receive(modbus_param_t *mb_param,
+ uint8_t *query,
+ uint8_t *response)
+{
+ int ret;
+ int response_length;
+ int response_length_computed;
+ int offset = mb_param->header_length;
+
+ response_length_computed = compute_response_length(mb_param, query);
+ ret = receive_msg(mb_param, response_length_computed,
+ response, &response_length);
+ if (ret == 0) {
+ /* GOOD RESPONSE */
+ int query_nb_value;
+ int response_nb_value;
+
+ /* The number of values is returned if it's corresponding
+ * to the query */
+ switch (response[offset + 1]) {
+ case FC_READ_COIL_STATUS:
+ case FC_READ_INPUT_STATUS:
+ /* Read functions, 8 values in a byte (nb
+ * of values in the query and byte count in
+ * the response. */
+ query_nb_value = (query[offset+4] << 8) + query[offset+5];
+ query_nb_value = (query_nb_value / 8) + ((query_nb_value % 8) ? 1 : 0);
+ response_nb_value = response[offset + 2];
+ break;
+ case FC_READ_HOLDING_REGISTERS:
+ case FC_READ_INPUT_REGISTERS:
+ /* Read functions 1 value = 2 bytes */
+ query_nb_value = (query[offset+4] << 8) + query[offset+5];
+ response_nb_value = (response[offset + 2] / 2);
+ break;
+ case FC_FORCE_MULTIPLE_COILS:
+ case FC_PRESET_MULTIPLE_REGISTERS:
+ /* N Write functions */
+ query_nb_value = (query[offset+4] << 8) + query[offset+5];
+ response_nb_value = (response[offset + 4] << 8) | response[offset + 5];
+ break;
+ case FC_REPORT_SLAVE_ID:
+ /* Report slave ID (bytes received) */
+ query_nb_value = response_nb_value = response_length;
+ break;
+ default:
+ /* 1 Write functions & others */
+ query_nb_value = response_nb_value = 1;
+ }
+
+ if (query_nb_value == response_nb_value) {
+ ret = response_nb_value;
+ } else {
+ char *s_error = (char *)malloc(64 * sizeof(char));
+ sprintf(s_error, "Quantity (%d) not corresponding to the query (%d)",
+ response_nb_value, query_nb_value);
+ ret = ILLEGAL_DATA_VALUE;
+ error_treat(mb_param, ILLEGAL_DATA_VALUE, s_error);
+ free(s_error);
+ }
+ } else if (ret == COMM_TIME_OUT) {
+
+ if (response_length == (offset + 3 + mb_param->checksum_length)) {
+ /* EXCEPTION CODE RECEIVED */
+
+ /* Optimization allowed because exception response is
+ the smallest trame in modbus protocol (3) so always
+ raise a timeout error */
+
+ /* CRC must be checked here (not done in receive_msg) */
+ if (mb_param->type_com == RTU) {
+ ret = check_crc16(mb_param, response, response_length);
+ if (ret != 0)
+ return ret;
+ }
+
+ /* Check for exception response.
+ 0x80 + function is stored in the exception
+ response. */
+ if (0x80 + query[offset + 1] == response[offset + 1]) {
+
+ int exception_code = response[offset + 2];
+ // FIXME check test
+ if (exception_code < NB_TAB_ERROR_MSG) {
+ error_treat(mb_param, -exception_code,
+ TAB_ERROR_MSG[response[offset + 2]]);
+ /* RETURN THE EXCEPTION CODE */
+ /* Modbus error code is negative */
+ return -exception_code;
+ } else {
+ /* The chances are low to hit this
+ case but it can avoid a vicious
+ segfault */
+ char *s_error = (char *)malloc(64 * sizeof(char));
+ sprintf(s_error,
+ "Invalid exception code %d",
+ response[offset + 2]);
+ error_treat(mb_param, INVALID_EXCEPTION_CODE,
+ s_error);
+ free(s_error);
+ return INVALID_EXCEPTION_CODE;
+ }
+ }
+ /* If doesn't return previously, return as
+ TIME OUT here */
+ }
+
+ /* COMMUNICATION TIME OUT */
+ error_treat(mb_param, ret, "Communication time out");
+ return ret;
+ }
+
+ return ret;
+}
+
+static int response_io_status(int address, int nb,
+ uint8_t *tab_io_status,
+ uint8_t *response, int offset)
+{
+ int shift = 0;
+ int byte = 0;
+ int i;
+
+ for (i = address; i < address+nb; i++) {
+ byte |= tab_io_status[i] << shift;
+ if (shift == 7) {
+ /* Byte is full */
+ response[offset++] = byte;
+ byte = shift = 0;
+ } else {
+ shift++;
+ }
+ }
+
+ if (shift != 0)
+ response[offset++] = byte;
+
+ return offset;
+}
+
+/* Build the exception response */
+static int response_exception(modbus_param_t *mb_param, sft_t *sft,
+ int exception_code, uint8_t *response)
+{
+ int response_length;
+
+ sft->function = sft->function + 0x80;
+ response_length = build_response_basis(mb_param, sft, response);
+
+ /* Positive exception code */
+ response[response_length++] = -exception_code;
+
+ return response_length;
+}
+
+/* Manages the received query.
+ Analyses the query and constructs a response.
+ If an error occurs, this function construct the response
+ accordingly.
+*/
+void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
+ int query_length, modbus_mapping_t *mb_mapping)
+{
+ int offset = mb_param->header_length;
+ int slave = query[offset];
+ int function = query[offset+1];
+ uint16_t address = (query[offset+2] << 8) + query[offset+3];
+ uint8_t response[MAX_MESSAGE_LENGTH];
+ int resp_length = 0;
+ sft_t sft;
+
+ sft.slave = slave;
+ sft.function = function;
+// if (mb_param->type_com == TCP) {
+ // sft.t_id = (query[0] << 8) + query[1];
+ // } else {
+ sft.t_id = 0;
+ query_length -= CHECKSUM_LENGTH_RTU;
+ // }
+
+ switch (function) {
+ case FC_READ_COIL_STATUS: {
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_coil_status) {
+ printf("Illegal data address %0X in read_coil_status\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ resp_length = build_response_basis(mb_param, &sft, response);
+ response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
+ resp_length = response_io_status(address, nb,
+ mb_mapping->tab_coil_status,
+ response, resp_length);
+ }
+ }
+ break;
+ case FC_READ_INPUT_STATUS: {
+ /* Similar to coil status (but too much arguments to use a
+ * function) */
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_input_status) {
+ printf("Illegal data address %0X in read_input_status\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ resp_length = build_response_basis(mb_param, &sft, response);
+ response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
+ resp_length = response_io_status(address, nb,
+ mb_mapping->tab_input_status,
+ response, resp_length);
+ }
+ }
+ break;
+ case FC_READ_HOLDING_REGISTERS: {
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_holding_registers) {
+ printf("Illegal data address %0X in read_holding_registers\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ int i;
+
+ resp_length = build_response_basis(mb_param, &sft, response);
+ response[resp_length++] = nb << 1;
+ for (i = address; i < address + nb; i++) {
+ response[resp_length++] = mb_mapping->tab_holding_registers[i] >> 8;
+ response[resp_length++] = mb_mapping->tab_holding_registers[i] & 0xFF;
+ }
+ }
+ }
+ break;
+ case FC_READ_INPUT_REGISTERS: {
+ /* Similar to holding registers (but too much arguments to use a
+ * function) */
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_input_registers) {
+ printf("Illegal data address %0X in read_input_registers\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ int i;
+
+ resp_length = build_response_basis(mb_param, &sft, response);
+ response[resp_length++] = nb << 1;
+ for (i = address; i < address + nb; i++) {
+ response[resp_length++] = mb_mapping->tab_input_registers[i] >> 8;
+ response[resp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
+ }
+ }
+ }
+ break;
+ case FC_FORCE_SINGLE_COIL:
+ if (address >= mb_mapping->nb_coil_status) {
+ printf("Illegal data address %0X in force_singe_coil\n", address);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ int data = (query[offset+4] << 8) + query[offset+5];
+
+ if (data == 0xFF00 || data == 0x0) {
+ mb_mapping->tab_coil_status[address] = (data) ? ON : OFF;
+
+ /* In RTU mode, the CRC is computed and added
+ to the query by modbus_send, the computed
+ CRC will be same and optimisation is
+ possible here (FIXME). */
+ memcpy(response, query, query_length);
+ resp_length = query_length;
+ } else {
+ printf("Illegal data value %0X in force_single_coil request at address %0X\n",
+ data, address);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_VALUE, response);
+ }
+ }
+ break;
+ case FC_PRESET_SINGLE_REGISTER:
+ if (address >= mb_mapping->nb_holding_registers) {
+ printf("Illegal data address %0X in preset_holding_register\n", address);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ int data = (query[offset+4] << 8) + query[offset+5];
+
+ mb_mapping->tab_holding_registers[address] = data;
+ memcpy(response, query, query_length);
+ resp_length = query_length;
+ }
+ break;
+ case FC_FORCE_MULTIPLE_COILS: {
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_coil_status) {
+ printf("Illegal data address %0X in force_multiple_coils\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ /* 6 = byte count, 7 = first byte of data */
+ set_bits_from_bytes(mb_mapping->tab_coil_status, address, nb, &query[offset + 7]);
+
+ resp_length = build_response_basis(mb_param, &sft, response);
+ /* 4 to copy the coil address (2) and the quantity of coils */
+ memcpy(response + resp_length, query + resp_length, 4);
+ resp_length += 4;
+ }
+ }
+ break;
+ case FC_PRESET_MULTIPLE_REGISTERS: {
+ int nb = (query[offset+4] << 8) + query[offset+5];
+
+ if ((address + nb) > mb_mapping->nb_holding_registers) {
+ printf("Illegal data address %0X in preset_multiple_registers\n",
+ address + nb);
+ resp_length = response_exception(mb_param, &sft,
+ ILLEGAL_DATA_ADDRESS, response);
+ } else {
+ int i, j;
+ for (i = address, j = 0; i < address + nb; i++, j += 2) {
+ /* 6 = byte count, 7 and 8 = first value */
+ mb_mapping->tab_holding_registers[i] =
+ (query[offset + 7 + j] << 8) + query[offset + 8 + j];
+ }
+
+ resp_length = build_response_basis(mb_param, &sft, response);
+ /* 4 to copy the address (2) and the no. of registers */
+ memcpy(response + resp_length, query + resp_length, 4);
+ resp_length += 4;
+ }
+ }
+ break;
+ case FC_READ_EXCEPTION_STATUS:
+ case FC_REPORT_SLAVE_ID:
+ printf("Not implemented\n");
+ break;
+ }
+
+ modbus_send(mb_param, response, resp_length);
+}
+
+/* Listens any message on a socket or file descriptor.
+ Returns:
+ - 0 if OK, or a negative error number if the request fails
+ - query, message received
+ - query_length, length in bytes of the message */
+int modbus_listen(modbus_param_t *mb_param, uint8_t *query, int *query_length)
+{
+ int ret;
+
+ /* The length of the query to receive isn't known. */
+ ret = receive_msg(mb_param, MSG_LENGTH_UNDEFINED, query, query_length);
+
+ return ret;
+}
+
+/* Reads IO status */
+static int read_io_status(modbus_param_t *mb_param, int slave, int function,
+ int start_addr, int nb, uint8_t *data_dest)
+{
+ int ret;
+ int query_length;
+
+ uint8_t query[MIN_QUERY_LENGTH];
+ uint8_t response[MAX_MESSAGE_LENGTH];
+
+ query_length = build_query_basis(mb_param, slave, function,
+ start_addr, nb, query);
+
+ ret = modbus_send(mb_param, query, query_length);
+ if (ret > 0) {
+ int i, temp, bit;
+ int pos = 0;
+ int offset;
+ int offset_length;
+
+ ret = modbus_receive(mb_param, query, response);
+ if (ret < 0)
+ return ret;
+
+ offset = mb_param->header_length;
+
+ offset_length = offset + ret;
+ for (i = offset; i < offset_length; i++) {
+ /* Shift reg hi_byte to temp */
+ temp = response[3 + i];
+
+ for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
+ data_dest[pos++] = (temp & bit) ? TRUE : FALSE;
+ bit = bit << 1;
+ }
+
+ }
+ }
+
+ return ret;
+}
+
+/* Reads the boolean status of coils and sets the array elements
+ in the destination to TRUE or FALSE. */
+int read_coil_status(modbus_param_t *mb_param, int slave, int start_addr,
+ int nb, uint8_t *data_dest)
+{
+ int status;
+
+ if (nb > MAX_STATUS) {
+ printf("ERROR Too many coils status requested (%d > %d)\n",
+ nb, MAX_STATUS);
+ return TOO_MANY_DATA;
+ }
+
+ status = read_io_status(mb_param, slave, FC_READ_COIL_STATUS,
+ start_addr, nb, data_dest);
+
+ if (status > 0)
+ status = nb;
+
+ return status;
+}
+
+
+/* Same as read_coil_status but reads the slaves input table */
+int read_input_status(modbus_param_t *mb_param, int slave, int start_addr,
+ int nb, uint8_t *data_dest)
+{
+ int status;
+
+ if (nb > MAX_STATUS) {
+ printf("ERROR Too many input status requested (%d > %d)\n",
+ nb, MAX_STATUS);
+ return TOO_MANY_DATA;
+ }
+
+ status = read_io_status(mb_param, slave, FC_READ_INPUT_STATUS,
+ start_addr, nb, data_dest);
+
+ if (status > 0)
+ status = nb;
+
+ return status;
+}
+
+/* Reads the data from a modbus slave and put that data into an array */
+static int read_registers(modbus_param_t *mb_param, int slave, int function,
+ int start_addr, int nb, uint16_t *data_dest)
+{
+ int ret;
+ int query_length;
+ uint8_t query[MIN_QUERY_LENGTH];
+ uint8_t response[MAX_MESSAGE_LENGTH];
+
+ if (nb > MAX_REGISTERS) {
+ printf("EROOR Too many holding registers requested (%d > %d)\n",
+ nb, MAX_REGISTERS);
+ return TOO_MANY_DATA;
+ }
+
+ query_length = build_query_basis(mb_param, slave, function,
+ start_addr, nb, query);
+
+ ret = modbus_send(mb_param, query, query_length);
+
+ /*
+ if (ret > 0) {
+ int offset;
+ int i;
+
+ ret = modbus_receive(mb_param, query, response);
+
+ offset = mb_param->header_length;
+
+ // If ret is negative, the loop is jumped !
+ for (i = 0; i < ret; i++) {
+ // shift reg hi_byte to temp OR with lo_byte
+ data_dest[i] = (response[offset + 3 + (i << 1)] << 8) |
+ response[offset + 4 + (i << 1)];
+ }
+ }
+ */
+ return ret;
+}
+
+/* Reads the holding registers in a slave and put the data into an
+ array */
+int read_holding_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint16_t *data_dest)
+{
+ int status;
+
+ if (nb > MAX_REGISTERS) {
+ printf("ERROR Too many holding registers requested (%d > %d)\n",
+ nb, MAX_REGISTERS);
+ return TOO_MANY_DATA;
+ }
+
+ status = read_registers(mb_param, slave, FC_READ_HOLDING_REGISTERS,
+ start_addr, nb, data_dest);
+ return status;
+}
+
+/* Reads the input registers in a slave and put the data into
+ an array */
+int read_input_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint16_t *data_dest)
+{
+ int status;
+
+ if (nb > MAX_REGISTERS) {
+ printf("ERROR Too many input registers requested (%d > %d)\n",
+ nb, MAX_REGISTERS);
+ return TOO_MANY_DATA;
+ }
+
+ status = read_registers(mb_param, slave, FC_READ_INPUT_REGISTERS,
+ start_addr, nb, data_dest);
+
+ return status;
+}
+
+/* Sends a value to a register in a slave.
+ Used by force_single_coil and preset_single_register */
+static int set_single(modbus_param_t *mb_param, int slave, int function,
+ int addr, int value)
+{
+ int ret;
+ int query_length;
+ uint8_t query[MIN_QUERY_LENGTH];
+
+ query_length = build_query_basis(mb_param, slave, function,
+ addr, value, query);
+
+ ret = modbus_send(mb_param, query, query_length);
+ if (ret > 0) {
+ /* Used by force_single_coil and
+ * preset_single_register */
+ uint8_t response[MIN_QUERY_LENGTH];
+ ret = modbus_receive(mb_param, query, response);
+ }
+
+ return ret;
+}
+
+/* Turns ON or OFF a single coil in the slave device */
+int force_single_coil(modbus_param_t *mb_param, int slave,
+ int coil_addr, int state)
+{
+ int status;
+
+ if (state)
+ state = 0xFF00;
+
+ status = set_single(mb_param, slave, FC_FORCE_SINGLE_COIL,
+ coil_addr, state);
+
+ return status;
+}
+
+/* Sets a value in one holding register in the slave device */
+int preset_single_register(modbus_param_t *mb_param, int slave,
+ int reg_addr, int value)
+{
+ int status;
+
+ status = set_single(mb_param, slave, FC_PRESET_SINGLE_REGISTER,
+ reg_addr, value);
+
+ return status;
+}
+
+/* Sets/resets the coils in the slave from an array in argument */
+int force_multiple_coils(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb,
+ const uint8_t *data_src)
+{
+ int ret;
+ int i;
+ int byte_count;
+ int query_length;
+ int coil_check = 0;
+ int pos = 0;
+
+ uint8_t query[MAX_MESSAGE_LENGTH];
+
+ if (nb > MAX_STATUS) {
+ printf("ERROR Writing to too many coils (%d > %d)\n",
+ nb, MAX_STATUS);
+ return TOO_MANY_DATA;
+ }
+
+ query_length = build_query_basis(mb_param, slave,
+ FC_FORCE_MULTIPLE_COILS,
+ start_addr, nb, query);
+ byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
+ query[query_length++] = byte_count;
+
+ for (i = 0; i < byte_count; i++) {
+ int bit;
+
+ bit = 0x01;
+ query[query_length] = 0;
+
+ while ((bit & 0xFF) && (coil_check++ < nb)) {
+ if (data_src[pos++])
+ query[query_length] |= bit;
+ else
+ query[query_length] &=~ bit;
+
+ bit = bit << 1;
+ }
+ query_length++;
+ }
+
+ ret = modbus_send(mb_param, query, query_length);
+ if (ret > 0) {
+ uint8_t response[MAX_MESSAGE_LENGTH];
+ ret = modbus_receive(mb_param, query, response);
+ }
+
+
+ return ret;
+}
+
+/* Copies the values in the slave from the array given in argument */
+int preset_multiple_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb,
+ const uint16_t *data_src)
+{
+ int ret;
+ int i;
+ int query_length;
+ int byte_count;
+
+ uint8_t query[MAX_MESSAGE_LENGTH];
+
+ if (nb > MAX_REGISTERS) {
+ printf("ERROR Trying to write to too many registers (%d > %d)\n",
+ nb, MAX_REGISTERS);
+ return TOO_MANY_DATA;
+ }
+
+ query_length = build_query_basis(mb_param, slave,
+ FC_PRESET_MULTIPLE_REGISTERS,
+ start_addr, nb, query);
+ byte_count = nb * 2;
+ query[query_length++] = byte_count;
+
+ for (i = 0; i < nb; i++) {
+ query[query_length++] = data_src[i] >> 8;
+ query[query_length++] = data_src[i] & 0x00FF;
+ }
+
+ ret = modbus_send(mb_param, query, query_length);
+ if (ret > 0) {
+ uint8_t response[MAX_MESSAGE_LENGTH];
+ ret = modbus_receive(mb_param, query, response);
+ }
+
+ return ret;
+}
+
+/* Returns the slave id! */
+int report_slave_id(modbus_param_t *mb_param, int slave,
+ uint8_t *data_dest)
+{
+ int ret;
+ int query_length;
+ uint8_t query[MIN_QUERY_LENGTH];
+
+ query_length = build_query_basis(mb_param, slave, FC_REPORT_SLAVE_ID,
+ 0, 0, query);
+
+ /* HACKISH, start_addr and count are not used */
+ query_length -= 4;
+
+ ret = modbus_send(mb_param, query, query_length);
+ if (ret > 0) {
+ int i;
+ int offset;
+ int offset_length;
+ uint8_t response[MAX_MESSAGE_LENGTH];
+
+ /* Byte count, slave id, run indicator status,
+ additional data */
+ ret = modbus_receive(mb_param, query, response);
+ if (ret < 0)
+ return ret;
+
+ offset = mb_param->header_length;
+ offset_length = offset + ret;
+
+ for (i = offset; i < offset_length; i++)
+ data_dest[i] = response[i];
+ }
+
+ return ret;
+}
+
+/* Initializes the modbus_param_t structure for RTU
+ - device: "/dev/ttyS0"
+ - baud: 9600, 19200, 57600, 115200, etc
+ - parity: "even", "odd" or "none"
+ - data_bits: 5, 6, 7, 8
+ - stop_bits: 1, 2
+*/
+void modbus_init_rtu(modbus_param_t *mb_param, const char *device,
+ int baud, const char *parity, int data_bit,
+ int stop_bit)
+{
+ memset(mb_param, 0, sizeof(modbus_param_t));
+ strcpy(mb_param->device, device);
+ mb_param->baud = baud;
+ strcpy(mb_param->parity, parity);
+ mb_param->debug = FALSE;
+ mb_param->data_bit = data_bit;
+ mb_param->stop_bit = stop_bit;
+ mb_param->type_com = RTU;
+ mb_param->header_length = HEADER_LENGTH_RTU;
+ mb_param->checksum_length = CHECKSUM_LENGTH_RTU;
+}
+
+
+/* By default, the error handling mode used is FLUSH_OR_RECONNECT_ON_ERROR.
+
+ With FLUSH_OR_RECONNECT_ON_ERROR, the library will flush to I/O
+ port in RTU mode or attempt an immediate reconnection which may
+ hang for several seconds if the network to the remote target unit
+ is down in TCP mode.
+
+ With NOP_ON_ERROR, it is expected that the application will
+ check for error returns and deal with them as necessary.
+*/
+void modbus_set_error_handling(modbus_param_t *mb_param,
+ error_handling_t error_handling)
+{
+ if (error_handling == FLUSH_OR_RECONNECT_ON_ERROR ||
+ error_handling == NOP_ON_ERROR) {
+ mb_param->error_handling = error_handling;
+ } else {
+ printf("Invalid setting for error handling (not changed)\n");
+ }
+}
+
+
+/* Sets up a serial port for RTU communications */
+static int modbus_connect_rtu(modbus_param_t *mb_param)
+{
+
+ return 0;
+}
+
+
+/* Establishes a modbus connexion.
+ Returns -1 if an error occured. */
+int modbus_connect(modbus_param_t *mb_param)
+{
+ int ret;
+ ret = modbus_connect_rtu(mb_param);
+
+ return ret;
+}
+
+/* Closes the file descriptor in RTU mode */
+static void modbus_close_rtu(modbus_param_t *mb_param)
+{
+
+}
+
+/* Closes a modbus connection */
+void modbus_close(modbus_param_t *mb_param)
+{
+ modbus_close_rtu(mb_param);
+
+ }
+
+/* Activates the debug messages */
+void modbus_set_debug(modbus_param_t *mb_param, int boolean)
+{
+ mb_param->debug = boolean;
+}
+
+/* Allocates 4 arrays to store coils, input status, input registers and
+ holding registers. The pointers are stored in modbus_mapping structure.
+
+ Returns: TRUE if ok, FALSE on failure
+*/
+int modbus_mapping_new(modbus_mapping_t *mb_mapping,
+ int nb_coil_status, int nb_input_status,
+ int nb_holding_registers, int nb_input_registers)
+{
+ /* 0X */
+ mb_mapping->nb_coil_status = nb_coil_status;
+ mb_mapping->tab_coil_status =
+ (uint8_t *) malloc(nb_coil_status * sizeof(uint8_t));
+ memset(mb_mapping->tab_coil_status, 0,
+ nb_coil_status * sizeof(uint8_t));
+ if (mb_mapping->tab_coil_status == NULL)
+ return FALSE;
+
+ /* 1X */
+ mb_mapping->nb_input_status = nb_input_status;
+ mb_mapping->tab_input_status =
+ (uint8_t *) malloc(nb_input_status * sizeof(uint8_t));
+ memset(mb_mapping->tab_input_status, 0,
+ nb_input_status * sizeof(uint8_t));
+ if (mb_mapping->tab_input_status == NULL) {
+ free(mb_mapping->tab_coil_status);
+ return FALSE;
+ }
+
+ /* 4X */
+ mb_mapping->nb_holding_registers = nb_holding_registers;
+ mb_mapping->tab_holding_registers =
+ (uint16_t *) malloc(nb_holding_registers * sizeof(uint16_t));
+ memset(mb_mapping->tab_holding_registers, 0,
+ nb_holding_registers * sizeof(uint16_t));
+ if (mb_mapping->tab_holding_registers == NULL) {
+ free(mb_mapping->tab_coil_status);
+ free(mb_mapping->tab_input_status);
+ return FALSE;
+ }
+
+ /* 3X */
+ mb_mapping->nb_input_registers = nb_input_registers;
+ mb_mapping->tab_input_registers =
+ (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
+ memset(mb_mapping->tab_input_registers, 0,
+ nb_input_registers * sizeof(uint16_t));
+ if (mb_mapping->tab_input_registers == NULL) {
+ free(mb_mapping->tab_coil_status);
+ free(mb_mapping->tab_input_status);
+ free(mb_mapping->tab_holding_registers);
+ return FALSE;
+ }
+
+ return TRUE;
+}
+
+/* Frees the 4 arrays */
+void modbus_mapping_free(modbus_mapping_t *mb_mapping)
+{
+ free(mb_mapping->tab_coil_status);
+ free(mb_mapping->tab_input_status);
+ free(mb_mapping->tab_holding_registers);
+ free(mb_mapping->tab_input_registers);
+}
+/** Utils **/
+
+/* Sets many input/coil status from a single byte value (all 8 bits of
+ the byte value are setted) */
+void set_bits_from_byte(uint8_t *dest, int address, const uint8_t value)
+{
+ int i;
+
+ for (i=0; i<8; i++) {
+ dest[address+i] = (value & (1 << i)) ? ON : OFF;
+ }
+}
+
+/* Sets many input/coil status from a table of bytes (only the bits
+ between address and address + nb_bits are setted) */
+void set_bits_from_bytes(uint8_t *dest, int address, int nb_bits,
+ const uint8_t tab_byte[])
+{
+ int i;
+ int shift = 0;
+
+ for (i = address; i < address + nb_bits; i++) {
+ dest[i] = tab_byte[(i - address) / 8] & (1 << shift) ? ON : OFF;
+ /* gcc doesn't like: shift = (++shift) % 8; */
+ shift++;
+ shift %= 8;
+ }
+}
+
+/* Gets the byte value from many input/coil status.
+ To obtain a full byte, set nb_bits to 8. */
+uint8_t get_byte_from_bits(const uint8_t *src, int address, int nb_bits)
+{
+ int i;
+ uint8_t value = 0;
+
+ if (nb_bits > 8) {
+ printf("Error: nb_bits is too big\n");
+ nb_bits = 8;
+ }
+
+ for (i=0; i < nb_bits; i++) {
+ value |= (src[address+i] << i);
+ }
+
+ return value;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/modbus.h Sun Jul 24 05:04:06 2011 +0000
@@ -0,0 +1,321 @@
+/*
+ * Copyright � 2001-2008 St�phane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+//#include <termios.h>
+//#include <arpa/inet.h>
+
+
+#define MODBUS_TCP_DEFAULT_PORT 502
+
+#define HEADER_LENGTH_RTU 0
+#define PRESET_QUERY_LENGTH_RTU 6
+#define PRESET_RESPONSE_LENGTH_RTU 2
+
+#define HEADER_LENGTH_TCP 6
+#define PRESET_QUERY_LENGTH_TCP 12
+#define PRESET_RESPONSE_LENGTH_TCP 8
+
+#define CHECKSUM_LENGTH_RTU 2
+#define CHECKSUM_LENGTH_TCP 0
+
+/* It's not really the minimal length (the real one is report slave ID
+ * in RTU (4 bytes)) but it's a convenient size to use in RTU or TCP
+ * communications to read many values or write a single one.
+ * Maximum between :
+ * - HEADER_LENGTH_TCP (6) + slave (1) + function (1) + address (2) +
+ * number (2)
+ * - HEADER_LENGTH_RTU (0) + slave (1) + function (1) + address (2) +
+ * number (2) + CRC (2)
+*/
+#define MIN_QUERY_LENGTH 12
+
+/* Page 102, Application Notes of PI�MBUS�300:
+ * The maximum length of the entire message must not exceed 256
+ * bytes.
+ */
+#define MAX_MESSAGE_LENGTH 256
+
+#define MAX_STATUS 800
+#define MAX_REGISTERS 100
+
+#define REPORT_SLAVE_ID_LENGTH 75
+
+/* Time out between trames in microsecond */
+#define TIME_OUT_BEGIN_OF_TRAME 500000
+#define TIME_OUT_END_OF_TRAME 500000
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef OFF
+#define OFF 0
+#endif
+
+#ifndef ON
+#define ON 1
+#endif
+
+/* Function codes */
+#define FC_READ_COIL_STATUS 0x01 /* discretes inputs */
+#define FC_READ_INPUT_STATUS 0x02 /* discretes outputs */
+#define FC_READ_HOLDING_REGISTERS 0x03
+#define FC_READ_INPUT_REGISTERS 0x04
+#define FC_FORCE_SINGLE_COIL 0x05
+#define FC_PRESET_SINGLE_REGISTER 0x06
+#define FC_READ_EXCEPTION_STATUS 0x07
+#define FC_FORCE_MULTIPLE_COILS 0x0F
+#define FC_PRESET_MULTIPLE_REGISTERS 0x10
+#define FC_REPORT_SLAVE_ID 0x11
+
+/* Protocol exceptions */
+#define ILLEGAL_FUNCTION -0x01
+#define ILLEGAL_DATA_ADDRESS -0x02
+#define ILLEGAL_DATA_VALUE -0x03
+#define SLAVE_DEVICE_FAILURE -0x04
+#define SERVER_FAILURE -0x04
+#define ACKNOWLEDGE -0x05
+#define SLAVE_DEVICE_BUSY -0x06
+#define SERVER_BUSY -0x06
+#define NEGATIVE_ACKNOWLEDGE -0x07
+#define MEMORY_PARITY_ERROR -0x08
+#define GATEWAY_PROBLEM_PATH -0x0A
+#define GATEWAY_PROBLEM_TARGET -0x0B
+
+/* Local */
+#define COMM_TIME_OUT -0x0C
+#define PORT_SOCKET_FAILURE -0x0D
+#define SELECT_FAILURE -0x0E
+#define TOO_MANY_DATA -0x0F
+#define INVALID_CRC -0x10
+#define INVALID_EXCEPTION_CODE -0x11
+#define CONNECTION_CLOSED -0x12
+
+/* Internal using */
+#define MSG_LENGTH_UNDEFINED -1
+
+typedef enum { RTU, TCP } type_com_t;
+typedef enum { FLUSH_OR_RECONNECT_ON_ERROR, NOP_ON_ERROR } error_handling_t;
+
+/* This structure is byte-aligned */
+typedef struct {
+ /* Descriptor (tty or socket) */
+ int fd;
+ /* Communication mode: RTU or TCP */
+ type_com_t type_com;
+ /* Flag debug */
+ int debug;
+ /* Header length used for offset */
+ int header_length;
+ /* Checksum length RTU = 2 and TCP = 0 */
+ int checksum_length;
+ /* TCP port */
+ int port;
+ /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*"
+ on Mac OS X for KeySpan USB<->Serial adapters this string
+ had to be made bigger on OS X as the directory+file name
+ was bigger than 19 bytes. Making it 67 bytes for now, but
+ OS X does support 256 byte file names. May become a problem
+ in the future. */
+#ifdef __APPLE_CC__
+ char device[64];
+#else
+ char device[16];
+#endif
+ /* Bauds: 9600, 19200, 57600, 115200, etc */
+ int baud;
+ /* Data bit */
+ uint8_t data_bit;
+ /* Stop bit */
+ uint8_t stop_bit;
+ /* Parity: "even", "odd", "none" */
+ char parity[5];
+ /* In error_treat with TCP, do a reconnect or just dump the error */
+ uint8_t error_handling;
+ /* IP address */
+ char ip[16];
+ /* Save old termios settings */
+ // struct termios old_tios;
+} modbus_param_t;
+
+typedef struct {
+ int nb_coil_status;
+ int nb_input_status;
+ int nb_input_registers;
+ int nb_holding_registers;
+ uint8_t *tab_coil_status;
+ uint8_t *tab_input_status;
+ uint16_t *tab_input_registers;
+ uint16_t *tab_holding_registers;
+} modbus_mapping_t;
+
+
+/* All functions used for sending or receiving data return:
+ - the numbers of values (bits or word) if success (0 or more)
+ - less than 0 for exceptions errors
+*/
+
+/* Reads the boolean status of coils and sets the array elements in
+ the destination to TRUE or FALSE */
+int read_coil_status(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint8_t *dest);
+
+/* Same as read_coil_status but reads the slaves input table */
+int read_input_status(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint8_t *dest);
+
+/* Reads the holding registers in a slave and put the data into an
+ array */
+int read_holding_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint16_t *dest);
+
+/* Reads the input registers in a slave and put the data into an
+ array */
+int read_input_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, uint16_t *dest);
+
+/* Turns ON or OFF a single coil in the slave device */
+int force_single_coil(modbus_param_t *mb_param, int slave,
+ int coil_addr, int state);
+
+/* Sets a value in one holding register in the slave device */
+int preset_single_register(modbus_param_t *mb_param, int slave,
+ int reg_addr, int value);
+
+/* Sets/resets the coils in the slave from an array in argument */
+int force_multiple_coils(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, const uint8_t *data);
+
+/* Copies the values in the slave from the array given in argument */
+int preset_multiple_registers(modbus_param_t *mb_param, int slave,
+ int start_addr, int nb, const uint16_t *data);
+
+/* Returns the slave id! */
+int report_slave_id(modbus_param_t *mb_param, int slave, uint8_t *dest);
+
+/* Initializes the modbus_param_t structure for RTU.
+ - device: "/dev/ttyS0"
+ - baud: 9600, 19200, 57600, 115200, etc
+ - parity: "even", "odd" or "none"
+ - data_bits: 5, 6, 7, 8
+ - stop_bits: 1, 2
+*/
+void modbus_init_rtu(modbus_param_t *mb_param, const char *device,
+ int baud, const char *parity, int data_bit,
+ int stop_bit);
+
+/* Initializes the modbus_param_t structure for TCP.
+ - ip : "192.168.0.5"
+ - port : 1099
+
+ Set the port to MODBUS_TCP_DEFAULT_PORT to use the default one
+ (502). It's convenient to use a port number greater than or equal
+ to 1024 because it's not necessary to be root to use this port
+ number.
+*/
+
+/* By default, the error handling mode used is RECONNECT_ON_ERROR.
+
+ With RECONNECT_ON_ERROR, the library will attempt an immediate
+ reconnection which may hang for several seconds if the network to
+ the remote target unit is down.
+
+ With NOP_ON_ERROR, it is expected that the application will
+ check for network error returns and deal with them as necessary.
+
+ This function is only useful in TCP mode.
+ */
+void modbus_set_error_handling(modbus_param_t *mb_param, error_handling_t error_handling);
+
+/* Establishes a modbus connexion.
+ Returns -1 if an error occured. */
+int modbus_connect(modbus_param_t *mb_param);
+
+/* Closes a modbus connection */
+void modbus_close(modbus_param_t *mb_param);
+
+/* Activates the debug messages */
+void modbus_set_debug(modbus_param_t *mb_param, int boolean);
+
+/**
+ * SLAVE/CLIENT FUNCTIONS
+ **/
+
+/* Allocates 4 arrays to store coils, input status, input registers and
+ holding registers. The pointers are stored in modbus_mapping structure.
+
+ Returns: TRUE if ok, FALSE on failure
+ */
+int modbus_mapping_new(modbus_mapping_t *mb_mapping,
+ int nb_coil_status, int nb_input_status,
+ int nb_holding_registers, int nb_input_registers);
+
+/* Frees the 4 arrays */
+void modbus_mapping_free(modbus_mapping_t *mb_mapping);
+
+
+/* Listens for any query from a modbus master in TCP
+ Not tested in RTU communication. */
+int modbus_listen(modbus_param_t *mb_param, uint8_t *query, int *query_length);
+
+/* Manages the received query.
+ Analyses the query and constructs a response.
+
+ If an error occurs, this function construct the response
+ accordingly.
+*/
+void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
+ int query_length, modbus_mapping_t *mb_mapping);
+
+
+/**
+ * UTILS FUNCTIONS
+ **/
+
+/* Sets many input/coil status from a single byte value (all 8 bits of
+ the byte value are setted) */
+void set_bits_from_byte(uint8_t *dest, int address, const uint8_t value);
+
+/* Sets many input/coil status from a table of bytes (only the bits
+ between address and address + nb_bits are setted) */
+void set_bits_from_bytes(uint8_t *dest, int address, int nb_bits,
+ const uint8_t *tab_byte);
+
+/* Gets the byte value from many input/coil status.
+ To obtain a full byte, set nb_bits to 8. */
+uint8_t get_byte_from_bits(const uint8_t *src, int address, int nb_bits);
+
+ int build_query_basis_rtu(int slave, int function,
+ int start_addr, int nb,
+ uint8_t *query);
+ int modbus_send(modbus_param_t *mb_param, uint8_t *query,
+ int query_length);
+
+ int modbus_receive(modbus_param_t *mb_param,
+ uint8_t *query,
+ uint8_t *response);
+ int receive_msg(modbus_param_t *mb_param,
+ int msg_length_computed,
+ uint8_t *msg, int *p_msg_length);
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jul 24 05:04:06 2011 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+#define PROTOPLATFORM
+//#define MBEDPLATFORM
+#include "Protocol/modbus.h"
+
+//LEDs
+#define LED_1 (LED1)
+#define LED_2 (LED1)
+
+//Serial(1) rs232
+#define RS232_TX (p13)
+#define RS232_RX (p14)
+
+//Serial(2) rs232/ttl/iphone
+#define TTL_TX (p28)
+#define TTL_RX (p27)
+#define TTL_GND (p26)
+
+//Console
+#define CONSOLETX (USBTX)
+#define CONSOLERX (USBRX)
+
+
+
+
+//MODBUS STUFF
+#define LOOP 1
+#define SLAVE 0x01
+#define ADDRESS_START 4096
+#define ADDRESS_END 4100
+
+
+ int nb = ADDRESS_END - ADDRESS_START;;
+
+ uint8_t tab_rq_status[ADDRESS_END - ADDRESS_START];
+ uint8_t tab_rp_status[ADDRESS_END - ADDRESS_START];
+ uint16_t tab_rq_registers[ADDRESS_END - ADDRESS_START];
+ uint16_t tab_rp_registers[ADDRESS_END - ADDRESS_START];
+
+DigitalOut myled(LED_1);
+Serial console_port(CONSOLETX, CONSOLERX);
+
+Serial serial(RS232_TX,RS232_RX);
+
+modbus_param_t mb_param;
+ uint8_t query[MIN_QUERY_LENGTH];
+ uint8_t response[MAX_MESSAGE_LENGTH];
+
+void handleInterrupt() {
+
+ int ret;
+ int nb_fail;
+ int nb_loop;
+ int addr;
+
+
+
+ printf("**");
+
+
+ modbus_receive(&mb_param, query, response);
+
+ // if (ret > 0) {
+ int offset;
+ int i;
+
+ // ret = modbus_receive(mb_param, query, response);
+
+ offset = mb_param.header_length;
+
+ // If ret is negative, the loop is jumped !
+ for (i = 0; i < ret; i++) {
+ // shift reg hi_byte to temp OR with lo_byte
+ tab_rq_registers[i] = (response[offset + 3 + (i << 1)] << 8) |
+ response[offset + 4 + (i << 1)];
+ printf("%d -- %d", tab_rq_registers[i],i);
+ }
+ // }
+
+ /*
+ for (int i=0; i<nb; i++) {
+ if (tab_rq_registers[i] != tab_rp_registers[i]) {
+ printf("ERROR read_holding_registers\n");
+ printf("Slave = %d, address = %d, value %d (0x%X) != %d (0x%X)\n",
+ SLAVE, addr,
+ tab_rq_registers[i], tab_rq_registers[i],
+ tab_rp_registers[i], tab_rp_registers[i]);
+ nb_fail++;
+ }
+ }
+ */
+
+}
+
+void handleInterrupt2() {
+
+while(serial.readable()) {
+ printf("<%.2X>", serial.getc());
+}
+
+}
+
+void modbus_test() {
+
+
+ int ret;
+ int nb_fail;
+ int nb_loop;
+ int addr;
+
+ modbus_init_rtu(&mb_param, "/dev/ttyS0", 19200, "none", 8, 1);
+
+ //modbus_set_debug(&mb_param, TRUE);
+ if (modbus_connect(&mb_param) == -1) {
+ printf("ERROR Connection failed\n");
+ exit(1);
+ }
+
+ nb = ADDRESS_END - ADDRESS_START;
+
+/*
+ tab_rq_status = (uint8_t *) malloc(nb * sizeof(uint8_t));
+ memset(tab_rq_status, 0, nb * sizeof(uint8_t));
+
+ tab_rp_status = (uint8_t *) malloc(nb * sizeof(uint8_t));
+ memset(tab_rp_status, 0, nb * sizeof(uint8_t));
+
+ tab_rq_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
+ memset(tab_rq_registers, 0, nb * sizeof(uint16_t));
+
+ tab_rp_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
+ memset(tab_rp_registers, 0, nb * sizeof(uint16_t));
+*/
+ serial.attach( handleInterrupt2);
+
+ nb_loop = nb_fail = 0;
+ while (nb_loop++ < LOOP) {
+ for (addr = ADDRESS_START; addr <= ADDRESS_END; addr++) {
+ int i;
+
+ for (i=0; i<nb; i++) {
+ tab_rq_registers[i] = (uint16_t) 44097+i;;
+ tab_rq_status[i] = tab_rq_registers[i] % 2;
+ }
+ nb = ADDRESS_END - addr;
+/*
+ ret = preset_multiple_registers(&mb_param, SLAVE,
+ addr, nb, tab_rq_registers);
+ if (ret != nb) {
+ printf("ERROR preset_multiple_registers (%d)\n", ret);
+ printf("Slave = %d, address = %d, nb = %d\n",
+ SLAVE, addr, nb);
+ nb_fail++;
+ } else {
+ */
+ /*
+ ret = read_holding_registers(&mb_param, SLAVE,
+ addr, nb, tab_rp_registers);
+
+
+ printf("HERER");
+ */
+
+ int ret;
+ int query_length;
+
+
+ query_length = build_query_basis_rtu(SLAVE, FC_READ_HOLDING_REGISTERS, addr, nb, query);
+ // query_length = build_query_basis(&mb_param, SLAVE, FC_READ_HOLDING_REGISTERS, addr, nb, query);
+
+ ret = modbus_send(&mb_param, query, query_length);
+
+ //}
+
+ }
+
+ printf("Test: ");
+ if (nb_fail)
+ printf("%d FAILS\n", nb_fail);
+ else
+ printf("SUCCESS\n");
+ }
+ // handleInterrupt();
+/*
+ free(tab_rq_status);
+ free(tab_rp_status);
+ free(tab_rq_registers);
+ free(tab_rp_registers);
+
+ modbus_close(&mb_param);
+ */
+}
+
+
+int main() {
+ myled = 1;
+ console_port.baud(230400);
+ serial.baud(57600);
+
+ printf("Starting\n");
+
+modbus_test();
+
+ while(1) {
+ myled = 1;
+ wait(0.2);
+ myled = 0;
+ wait(0.2);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Jul 24 05:04:06 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e