Jason Engelman
First Working Master with Adaptronic simulator
Embed:
(wiki syntax)
Show/hide line numbers
modbus.c
00001 00002 00003 #include <stdio.h> 00004 #include <string.h> 00005 #include <stdlib.h> 00006 #include <stdint.h> 00007 00008 #include <errno.h> 00009 #include <limits.h> 00010 00011 00012 #include "modbus.h" 00013 00014 #include "mbed.h" 00015 extern Serial serial; 00016 00017 #define UNKNOWN_ERROR_MSG "Not defined in modbus specification" 00018 00019 /* This structure reduces the number of params in functions and so 00020 * optimizes the speed of execution (~ 37%). */ 00021 typedef struct { 00022 int slave; 00023 int function; 00024 int t_id; 00025 } sft_t; 00026 00027 static const uint8_t NB_TAB_ERROR_MSG = 12; 00028 static const char *TAB_ERROR_MSG[] = { 00029 /* 0x00 */ UNKNOWN_ERROR_MSG, 00030 /* 0x01 */ "Illegal function code", 00031 /* 0x02 */ "Illegal data address", 00032 /* 0x03 */ "Illegal data value", 00033 /* 0x04 */ "Slave device or server failure", 00034 /* 0x05 */ "Acknowledge", 00035 /* 0x06 */ "Slave device or server busy", 00036 /* 0x07 */ "Negative acknowledge", 00037 /* 0x08 */ "Memory parity error", 00038 /* 0x09 */ UNKNOWN_ERROR_MSG, 00039 /* 0x0A */ "Gateway path unavailable", 00040 /* 0x0B */ "Target device failed to respond" 00041 }; 00042 00043 /* Table of CRC values for high-order byte */ 00044 static uint8_t table_crc_hi[] = { 00045 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00046 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00047 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00048 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00049 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00050 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 00051 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00052 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00053 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00054 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00055 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00056 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00057 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00058 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00059 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00060 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00061 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00062 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00063 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00064 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00065 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00066 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00067 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00068 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00069 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00070 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 00071 }; 00072 00073 /* Table of CRC values for low-order byte */ 00074 static uint8_t table_crc_lo[] = { 00075 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 00076 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 00077 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 00078 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 00079 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 00080 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 00081 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 00082 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 00083 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 00084 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 00085 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 00086 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 00087 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 00088 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 00089 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 00090 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 00091 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 00092 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 00093 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 00094 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 00095 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 00096 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 00097 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 00098 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 00099 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 00100 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 00101 }; 00102 00103 /* Treats errors and flush or close connection if necessary */ 00104 static void error_treat(modbus_param_t *mb_param, int code, const char *string) 00105 { 00106 printf("\nERROR %s (%d)\n", string, code); 00107 00108 if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) { 00109 switch (code) { 00110 case ILLEGAL_DATA_VALUE: 00111 case ILLEGAL_DATA_ADDRESS: 00112 case ILLEGAL_FUNCTION: 00113 break; 00114 default: 00115 // tcflush(mb_param->fd, TCIOFLUSH); 00116 00117 } 00118 } 00119 } 00120 00121 /* Computes the length of the expected response */ 00122 static unsigned int compute_response_length(modbus_param_t *mb_param, 00123 uint8_t *query) 00124 { 00125 int length; 00126 int offset; 00127 00128 offset = mb_param->header_length; 00129 00130 switch (query[offset + 1]) { 00131 case FC_READ_COIL_STATUS: 00132 case FC_READ_INPUT_STATUS: { 00133 /* Header + nb values (code from force_multiple_coils) */ 00134 int nb = (query[offset + 4] << 8) | query[offset + 5]; 00135 length = 3 + (nb / 8) + ((nb % 8) ? 1 : 0); 00136 } 00137 break; 00138 case FC_READ_HOLDING_REGISTERS: 00139 case FC_READ_INPUT_REGISTERS: 00140 /* Header + 2 * nb values */ 00141 length = 3 + 2 * (query[offset + 4] << 8 | 00142 query[offset + 5]); 00143 break; 00144 case FC_READ_EXCEPTION_STATUS: 00145 length = 4; 00146 break; 00147 default: 00148 length = 6; 00149 } 00150 00151 return length + offset + mb_param->checksum_length; 00152 } 00153 00154 /* Builds a RTU query header */ 00155 int build_query_basis_rtu(int slave, int function, 00156 int start_addr, int nb, 00157 uint8_t *query) 00158 { 00159 query[0] = slave; 00160 query[1] = function; 00161 query[2] = start_addr >> 8; 00162 query[3] = start_addr & 0x00ff; 00163 query[4] = nb >> 8; 00164 query[5] = nb & 0x00ff; 00165 00166 return PRESET_QUERY_LENGTH_RTU; 00167 } 00168 00169 00170 static int build_query_basis(modbus_param_t *mb_param, int slave, 00171 int function, int start_addr, 00172 int nb, uint8_t *query) 00173 { 00174 00175 return build_query_basis_rtu(slave, function, start_addr, 00176 nb, query); 00177 00178 } 00179 00180 /* Builds a RTU response header */ 00181 static int build_response_basis_rtu(sft_t *sft, uint8_t *response) 00182 { 00183 response[0] = sft->slave; 00184 response[1] = sft->function; 00185 00186 return PRESET_RESPONSE_LENGTH_RTU; 00187 } 00188 00189 static int build_response_basis(modbus_param_t *mb_param, sft_t *sft, 00190 uint8_t *response) 00191 { 00192 00193 return build_response_basis_rtu(sft, response); 00194 00195 } 00196 00197 00198 /* Fast CRC */ 00199 static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length) 00200 { 00201 uint8_t crc_hi = 0xFF; /* high CRC byte initialized */ 00202 uint8_t crc_lo = 0xFF; /* low CRC byte initialized */ 00203 unsigned int i; /* will index into CRC lookup */ 00204 00205 /* pass through message buffer */ 00206 while (buffer_length--) { 00207 i = crc_hi ^ *buffer++; /* calculate the CRC */ 00208 crc_hi = crc_lo ^ table_crc_hi[i]; 00209 crc_lo = table_crc_lo[i]; 00210 } 00211 00212 return (crc_hi << 8 | crc_lo); 00213 } 00214 00215 /* If CRC is correct returns 0 else returns INVALID_CRC */ 00216 static int check_crc16(modbus_param_t *mb_param, 00217 uint8_t *msg, 00218 const int msg_length) 00219 { 00220 int ret; 00221 uint16_t crc_calc; 00222 uint16_t crc_received; 00223 00224 crc_calc = crc16(msg, msg_length - 2); 00225 crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1]; 00226 00227 /* Check CRC of msg */ 00228 if (crc_calc == crc_received) { 00229 ret = 0; 00230 } else { 00231 char s_error[64]; 00232 sprintf(s_error, 00233 "invalid crc received %0X - crc_calc %0X", 00234 crc_received, crc_calc); 00235 ret = INVALID_CRC; 00236 error_treat(mb_param, ret, s_error); 00237 } 00238 00239 return ret; 00240 } 00241 00242 int modbus_send(modbus_param_t *mb_param, uint8_t *query, 00243 int query_length) 00244 { 00245 int ret; 00246 uint16_t s_crc; 00247 int i; 00248 00249 s_crc = crc16(query, query_length); 00250 query[query_length++] = s_crc >> 8; 00251 query[query_length++] = s_crc & 0x00FF; 00252 00253 if (mb_param->debug) { 00254 for (i = 0; i < query_length; i++) 00255 printf("[%.2X]", query[i]); 00256 printf("\n"); 00257 } 00258 00259 //mbed 00260 for (i = 0; i < query_length; i++) serial.putc(query[i]); 00261 00262 // ret = write(mb_param->fd, query, query_length); 00263 00264 /* Return the number of bytes written (0 to n) 00265 or PORT_SOCKET_FAILURE on error */ 00266 /* 00267 if ((ret == -1) || (ret != query_length)) { 00268 ret = PORT_SOCKET_FAILURE; 00269 error_treat(mb_param, ret, "Write port/socket failure"); 00270 } 00271 */ 00272 return ret; 00273 } 00274 00275 /* Computes the length of the header following the function code */ 00276 static uint8_t compute_query_length_header(int function) 00277 { 00278 int length; 00279 00280 if (function <= FC_FORCE_SINGLE_COIL || 00281 function == FC_PRESET_SINGLE_REGISTER) 00282 /* Read and single write */ 00283 length = 4; 00284 else if (function == FC_FORCE_MULTIPLE_COILS || 00285 function == FC_PRESET_MULTIPLE_REGISTERS) 00286 /* Multiple write */ 00287 length = 5; 00288 else 00289 length = 0; 00290 00291 return length; 00292 } 00293 00294 /* Computes the length of the data to write in the query */ 00295 static int compute_query_length_data(modbus_param_t *mb_param, uint8_t *msg) 00296 { 00297 int function = msg[mb_param->header_length + 1]; 00298 int length; 00299 00300 if (function == FC_FORCE_MULTIPLE_COILS || 00301 function == FC_PRESET_MULTIPLE_REGISTERS) 00302 length = msg[mb_param->header_length + 6]; 00303 else 00304 length = 0; 00305 00306 length += mb_param->checksum_length; 00307 00308 return length; 00309 } 00310 00311 00312 00313 /* Waits a reply from a modbus slave or a query from a modbus master. 00314 This function blocks for timeout seconds if there is no reply. 00315 00316 In 00317 - msg_length_computed must be set to MSG_LENGTH_UNDEFINED if undefined 00318 00319 Out 00320 - msg is an array of uint8_t to receive the message 00321 - p_msg_length, the variable is assigned to the number of 00322 characters received. This value won't be greater than 00323 msg_length_computed. 00324 00325 Returns 0 in success or a negative value if an error occured. 00326 */ 00327 int receive_msg(modbus_param_t *mb_param, 00328 int msg_length_computed, 00329 uint8_t *msg, int *p_msg_length) 00330 { 00331 int select_ret; 00332 int read_ret; 00333 // fd_set rfds; 00334 // struct timeval tv; 00335 int length_to_read; 00336 00337 uint8_t *p_msg; 00338 00339 enum { FUNCTION, BYTE, COMPLETE }; 00340 int state; 00341 00342 if (mb_param->debug) { 00343 if (msg_length_computed == MSG_LENGTH_UNDEFINED) 00344 printf("Waiting for a message...\n"); 00345 else 00346 printf("Waiting for a message (%d bytes)...\n", 00347 msg_length_computed); 00348 } 00349 printf("0"); 00350 /* Add a file descriptor to the set */ 00351 //FD_ZERO(&rfds); 00352 //FD_SET(mb_param->fd, &rfds); 00353 00354 if (msg_length_computed == MSG_LENGTH_UNDEFINED) { 00355 /* Wait for a message */ 00356 //tv.tv_sec = 60; 00357 //tv.tv_usec = 0; 00358 00359 /* The message length is undefined (query receiving) so 00360 * we need to analyse the message step by step. 00361 * At the first step, we want to reach the function 00362 * code because all packets have that information. */ 00363 msg_length_computed = mb_param->header_length + 2; 00364 state = FUNCTION; 00365 } else { 00366 // tv.tv_sec = 0; 00367 // tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME; 00368 state = COMPLETE; 00369 } 00370 00371 length_to_read = msg_length_computed; 00372 00373 select_ret = 1; 00374 // WAIT_DATA(); 00375 00376 /* Initialize the readin the message */ 00377 (*p_msg_length) = 0; 00378 p_msg = msg; 00379 00380 while (select_ret) { 00381 00382 read_ret=0; 00383 //read_ret = read(mb_param->fd, p_msg, length_to_read); 00384 printf("1"); 00385 while(serial.readable()) { 00386 p_msg[read_ret] = serial.getc(); 00387 printf("x"); 00388 read_ret++; 00389 } 00390 printf("2"); 00391 /* Sums bytes received */ 00392 (*p_msg_length) += read_ret; 00393 00394 /* Display the hex code of each character received */ 00395 if (mb_param->debug) { 00396 int i; 00397 for (i=0; i < read_ret; i++) 00398 printf("<%.2X>", p_msg[i]); 00399 } 00400 00401 if ((*p_msg_length) < msg_length_computed) { 00402 /* Message incomplete */ 00403 length_to_read = msg_length_computed - (*p_msg_length); 00404 } else { 00405 switch (state) { 00406 case FUNCTION: 00407 /* Function code position */ 00408 length_to_read = compute_query_length_header(msg[mb_param->header_length + 1]); 00409 msg_length_computed += length_to_read; 00410 /* It's useless to check 00411 p_msg_length_computed value in this 00412 case (only defined values are used). */ 00413 state = BYTE; 00414 break; 00415 case BYTE: 00416 length_to_read = compute_query_length_data(mb_param, msg); 00417 msg_length_computed += length_to_read; 00418 if (msg_length_computed > MAX_MESSAGE_LENGTH) { 00419 error_treat(mb_param, TOO_MANY_DATA, "Too many data"); 00420 return TOO_MANY_DATA; 00421 } 00422 state = COMPLETE; 00423 break; 00424 case COMPLETE: 00425 length_to_read = 0; 00426 break; 00427 } 00428 } 00429 00430 /* Moves the pointer to receive other data */ 00431 p_msg = &(p_msg[read_ret]); 00432 00433 if (length_to_read > 0) { 00434 /* If no character at the buffer wait 00435 TIME_OUT_END_OF_TRAME before to generate an error. */ 00436 //tv.tv_sec = 0; 00437 //tv.tv_usec = TIME_OUT_END_OF_TRAME; 00438 00439 //WAIT_DATA(); 00440 } else { 00441 /* All chars are received */ 00442 select_ret = FALSE; 00443 } 00444 } 00445 00446 printf("8"); 00447 if (mb_param->debug) 00448 printf("\n"); 00449 00450 if (mb_param->type_com == RTU) { 00451 printf("CRC"); 00452 check_crc16(mb_param, msg, (*p_msg_length)); 00453 } 00454 00455 /* OK */ 00456 return 0; 00457 } 00458 00459 00460 /* Receives the response and checks values (and checksum in RTU). 00461 00462 Returns: 00463 - the number of values (bits or word) if success or the response 00464 length if no value is returned 00465 - less than 0 for exception errors 00466 00467 Note: all functions used to send or receive data with modbus return 00468 these values. */ 00469 int modbus_receive(modbus_param_t *mb_param, 00470 uint8_t *query, 00471 uint8_t *response) 00472 { 00473 int ret; 00474 int response_length; 00475 int response_length_computed; 00476 int offset = mb_param->header_length; 00477 00478 response_length_computed = compute_response_length(mb_param, query); 00479 ret = receive_msg(mb_param, response_length_computed, 00480 response, &response_length); 00481 if (ret == 0) { 00482 /* GOOD RESPONSE */ 00483 int query_nb_value; 00484 int response_nb_value; 00485 00486 /* The number of values is returned if it's corresponding 00487 * to the query */ 00488 switch (response[offset + 1]) { 00489 case FC_READ_COIL_STATUS: 00490 case FC_READ_INPUT_STATUS: 00491 /* Read functions, 8 values in a byte (nb 00492 * of values in the query and byte count in 00493 * the response. */ 00494 query_nb_value = (query[offset+4] << 8) + query[offset+5]; 00495 query_nb_value = (query_nb_value / 8) + ((query_nb_value % 8) ? 1 : 0); 00496 response_nb_value = response[offset + 2]; 00497 break; 00498 case FC_READ_HOLDING_REGISTERS: 00499 case FC_READ_INPUT_REGISTERS: 00500 /* Read functions 1 value = 2 bytes */ 00501 query_nb_value = (query[offset+4] << 8) + query[offset+5]; 00502 response_nb_value = (response[offset + 2] / 2); 00503 break; 00504 case FC_FORCE_MULTIPLE_COILS: 00505 case FC_PRESET_MULTIPLE_REGISTERS: 00506 /* N Write functions */ 00507 query_nb_value = (query[offset+4] << 8) + query[offset+5]; 00508 response_nb_value = (response[offset + 4] << 8) | response[offset + 5]; 00509 break; 00510 case FC_REPORT_SLAVE_ID: 00511 /* Report slave ID (bytes received) */ 00512 query_nb_value = response_nb_value = response_length; 00513 break; 00514 default: 00515 /* 1 Write functions & others */ 00516 query_nb_value = response_nb_value = 1; 00517 } 00518 00519 if (query_nb_value == response_nb_value) { 00520 ret = response_nb_value; 00521 } else { 00522 char *s_error = (char *)malloc(64 * sizeof(char)); 00523 sprintf(s_error, "Quantity (%d) not corresponding to the query (%d)", 00524 response_nb_value, query_nb_value); 00525 ret = ILLEGAL_DATA_VALUE; 00526 error_treat(mb_param, ILLEGAL_DATA_VALUE, s_error); 00527 free(s_error); 00528 } 00529 } else if (ret == COMM_TIME_OUT) { 00530 00531 if (response_length == (offset + 3 + mb_param->checksum_length)) { 00532 /* EXCEPTION CODE RECEIVED */ 00533 00534 /* Optimization allowed because exception response is 00535 the smallest trame in modbus protocol (3) so always 00536 raise a timeout error */ 00537 00538 /* CRC must be checked here (not done in receive_msg) */ 00539 if (mb_param->type_com == RTU) { 00540 ret = check_crc16(mb_param, response, response_length); 00541 if (ret != 0) 00542 return ret; 00543 } 00544 00545 /* Check for exception response. 00546 0x80 + function is stored in the exception 00547 response. */ 00548 if (0x80 + query[offset + 1] == response[offset + 1]) { 00549 00550 int exception_code = response[offset + 2]; 00551 // FIXME check test 00552 if (exception_code < NB_TAB_ERROR_MSG) { 00553 error_treat(mb_param, -exception_code, 00554 TAB_ERROR_MSG[response[offset + 2]]); 00555 /* RETURN THE EXCEPTION CODE */ 00556 /* Modbus error code is negative */ 00557 return -exception_code; 00558 } else { 00559 /* The chances are low to hit this 00560 case but it can avoid a vicious 00561 segfault */ 00562 char *s_error = (char *)malloc(64 * sizeof(char)); 00563 sprintf(s_error, 00564 "Invalid exception code %d", 00565 response[offset + 2]); 00566 error_treat(mb_param, INVALID_EXCEPTION_CODE, 00567 s_error); 00568 free(s_error); 00569 return INVALID_EXCEPTION_CODE; 00570 } 00571 } 00572 /* If doesn't return previously, return as 00573 TIME OUT here */ 00574 } 00575 00576 /* COMMUNICATION TIME OUT */ 00577 error_treat(mb_param, ret, "Communication time out"); 00578 return ret; 00579 } 00580 00581 return ret; 00582 } 00583 00584 static int response_io_status(int address, int nb, 00585 uint8_t *tab_io_status, 00586 uint8_t *response, int offset) 00587 { 00588 int shift = 0; 00589 int byte = 0; 00590 int i; 00591 00592 for (i = address; i < address+nb; i++) { 00593 byte |= tab_io_status[i] << shift; 00594 if (shift == 7) { 00595 /* Byte is full */ 00596 response[offset++] = byte; 00597 byte = shift = 0; 00598 } else { 00599 shift++; 00600 } 00601 } 00602 00603 if (shift != 0) 00604 response[offset++] = byte; 00605 00606 return offset; 00607 } 00608 00609 /* Build the exception response */ 00610 static int response_exception(modbus_param_t *mb_param, sft_t *sft, 00611 int exception_code, uint8_t *response) 00612 { 00613 int response_length; 00614 00615 sft->function = sft->function + 0x80; 00616 response_length = build_response_basis(mb_param, sft, response); 00617 00618 /* Positive exception code */ 00619 response[response_length++] = -exception_code; 00620 00621 return response_length; 00622 } 00623 00624 /* Manages the received query. 00625 Analyses the query and constructs a response. 00626 If an error occurs, this function construct the response 00627 accordingly. 00628 */ 00629 void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query, 00630 int query_length, modbus_mapping_t *mb_mapping) 00631 { 00632 int offset = mb_param->header_length; 00633 int slave = query[offset]; 00634 int function = query[offset+1]; 00635 uint16_t address = (query[offset+2] << 8) + query[offset+3]; 00636 uint8_t response[MAX_MESSAGE_LENGTH]; 00637 int resp_length = 0; 00638 sft_t sft; 00639 00640 sft.slave = slave; 00641 sft.function = function; 00642 // if (mb_param->type_com == TCP) { 00643 // sft.t_id = (query[0] << 8) + query[1]; 00644 // } else { 00645 sft.t_id = 0; 00646 query_length -= CHECKSUM_LENGTH_RTU; 00647 // } 00648 00649 switch (function) { 00650 case FC_READ_COIL_STATUS: { 00651 int nb = (query[offset+4] << 8) + query[offset+5]; 00652 00653 if ((address + nb) > mb_mapping->nb_coil_status) { 00654 printf("Illegal data address %0X in read_coil_status\n", 00655 address + nb); 00656 resp_length = response_exception(mb_param, &sft, 00657 ILLEGAL_DATA_ADDRESS, response); 00658 } else { 00659 resp_length = build_response_basis(mb_param, &sft, response); 00660 response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); 00661 resp_length = response_io_status(address, nb, 00662 mb_mapping->tab_coil_status, 00663 response, resp_length); 00664 } 00665 } 00666 break; 00667 case FC_READ_INPUT_STATUS: { 00668 /* Similar to coil status (but too much arguments to use a 00669 * function) */ 00670 int nb = (query[offset+4] << 8) + query[offset+5]; 00671 00672 if ((address + nb) > mb_mapping->nb_input_status) { 00673 printf("Illegal data address %0X in read_input_status\n", 00674 address + nb); 00675 resp_length = response_exception(mb_param, &sft, 00676 ILLEGAL_DATA_ADDRESS, response); 00677 } else { 00678 resp_length = build_response_basis(mb_param, &sft, response); 00679 response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); 00680 resp_length = response_io_status(address, nb, 00681 mb_mapping->tab_input_status, 00682 response, resp_length); 00683 } 00684 } 00685 break; 00686 case FC_READ_HOLDING_REGISTERS: { 00687 int nb = (query[offset+4] << 8) + query[offset+5]; 00688 00689 if ((address + nb) > mb_mapping->nb_holding_registers) { 00690 printf("Illegal data address %0X in read_holding_registers\n", 00691 address + nb); 00692 resp_length = response_exception(mb_param, &sft, 00693 ILLEGAL_DATA_ADDRESS, response); 00694 } else { 00695 int i; 00696 00697 resp_length = build_response_basis(mb_param, &sft, response); 00698 response[resp_length++] = nb << 1; 00699 for (i = address; i < address + nb; i++) { 00700 response[resp_length++] = mb_mapping->tab_holding_registers[i] >> 8; 00701 response[resp_length++] = mb_mapping->tab_holding_registers[i] & 0xFF; 00702 } 00703 } 00704 } 00705 break; 00706 case FC_READ_INPUT_REGISTERS: { 00707 /* Similar to holding registers (but too much arguments to use a 00708 * function) */ 00709 int nb = (query[offset+4] << 8) + query[offset+5]; 00710 00711 if ((address + nb) > mb_mapping->nb_input_registers) { 00712 printf("Illegal data address %0X in read_input_registers\n", 00713 address + nb); 00714 resp_length = response_exception(mb_param, &sft, 00715 ILLEGAL_DATA_ADDRESS, response); 00716 } else { 00717 int i; 00718 00719 resp_length = build_response_basis(mb_param, &sft, response); 00720 response[resp_length++] = nb << 1; 00721 for (i = address; i < address + nb; i++) { 00722 response[resp_length++] = mb_mapping->tab_input_registers[i] >> 8; 00723 response[resp_length++] = mb_mapping->tab_input_registers[i] & 0xFF; 00724 } 00725 } 00726 } 00727 break; 00728 case FC_FORCE_SINGLE_COIL: 00729 if (address >= mb_mapping->nb_coil_status) { 00730 printf("Illegal data address %0X in force_singe_coil\n", address); 00731 resp_length = response_exception(mb_param, &sft, 00732 ILLEGAL_DATA_ADDRESS, response); 00733 } else { 00734 int data = (query[offset+4] << 8) + query[offset+5]; 00735 00736 if (data == 0xFF00 || data == 0x0) { 00737 mb_mapping->tab_coil_status[address] = (data) ? ON : OFF; 00738 00739 /* In RTU mode, the CRC is computed and added 00740 to the query by modbus_send, the computed 00741 CRC will be same and optimisation is 00742 possible here (FIXME). */ 00743 memcpy(response, query, query_length); 00744 resp_length = query_length; 00745 } else { 00746 printf("Illegal data value %0X in force_single_coil request at address %0X\n", 00747 data, address); 00748 resp_length = response_exception(mb_param, &sft, 00749 ILLEGAL_DATA_VALUE, response); 00750 } 00751 } 00752 break; 00753 case FC_PRESET_SINGLE_REGISTER: 00754 if (address >= mb_mapping->nb_holding_registers) { 00755 printf("Illegal data address %0X in preset_holding_register\n", address); 00756 resp_length = response_exception(mb_param, &sft, 00757 ILLEGAL_DATA_ADDRESS, response); 00758 } else { 00759 int data = (query[offset+4] << 8) + query[offset+5]; 00760 00761 mb_mapping->tab_holding_registers[address] = data; 00762 memcpy(response, query, query_length); 00763 resp_length = query_length; 00764 } 00765 break; 00766 case FC_FORCE_MULTIPLE_COILS: { 00767 int nb = (query[offset+4] << 8) + query[offset+5]; 00768 00769 if ((address + nb) > mb_mapping->nb_coil_status) { 00770 printf("Illegal data address %0X in force_multiple_coils\n", 00771 address + nb); 00772 resp_length = response_exception(mb_param, &sft, 00773 ILLEGAL_DATA_ADDRESS, response); 00774 } else { 00775 /* 6 = byte count, 7 = first byte of data */ 00776 set_bits_from_bytes(mb_mapping->tab_coil_status, address, nb, &query[offset + 7]); 00777 00778 resp_length = build_response_basis(mb_param, &sft, response); 00779 /* 4 to copy the coil address (2) and the quantity of coils */ 00780 memcpy(response + resp_length, query + resp_length, 4); 00781 resp_length += 4; 00782 } 00783 } 00784 break; 00785 case FC_PRESET_MULTIPLE_REGISTERS: { 00786 int nb = (query[offset+4] << 8) + query[offset+5]; 00787 00788 if ((address + nb) > mb_mapping->nb_holding_registers) { 00789 printf("Illegal data address %0X in preset_multiple_registers\n", 00790 address + nb); 00791 resp_length = response_exception(mb_param, &sft, 00792 ILLEGAL_DATA_ADDRESS, response); 00793 } else { 00794 int i, j; 00795 for (i = address, j = 0; i < address + nb; i++, j += 2) { 00796 /* 6 = byte count, 7 and 8 = first value */ 00797 mb_mapping->tab_holding_registers[i] = 00798 (query[offset + 7 + j] << 8) + query[offset + 8 + j]; 00799 } 00800 00801 resp_length = build_response_basis(mb_param, &sft, response); 00802 /* 4 to copy the address (2) and the no. of registers */ 00803 memcpy(response + resp_length, query + resp_length, 4); 00804 resp_length += 4; 00805 } 00806 } 00807 break; 00808 case FC_READ_EXCEPTION_STATUS: 00809 case FC_REPORT_SLAVE_ID: 00810 printf("Not implemented\n"); 00811 break; 00812 } 00813 00814 modbus_send(mb_param, response, resp_length); 00815 } 00816 00817 /* Listens any message on a socket or file descriptor. 00818 Returns: 00819 - 0 if OK, or a negative error number if the request fails 00820 - query, message received 00821 - query_length, length in bytes of the message */ 00822 int modbus_listen(modbus_param_t *mb_param, uint8_t *query, int *query_length) 00823 { 00824 int ret; 00825 00826 /* The length of the query to receive isn't known. */ 00827 ret = receive_msg(mb_param, MSG_LENGTH_UNDEFINED, query, query_length); 00828 00829 return ret; 00830 } 00831 00832 /* Reads IO status */ 00833 static int read_io_status(modbus_param_t *mb_param, int slave, int function, 00834 int start_addr, int nb, uint8_t *data_dest) 00835 { 00836 int ret; 00837 int query_length; 00838 00839 uint8_t query[MIN_QUERY_LENGTH]; 00840 uint8_t response[MAX_MESSAGE_LENGTH]; 00841 00842 query_length = build_query_basis(mb_param, slave, function, 00843 start_addr, nb, query); 00844 00845 ret = modbus_send(mb_param, query, query_length); 00846 if (ret > 0) { 00847 int i, temp, bit; 00848 int pos = 0; 00849 int offset; 00850 int offset_length; 00851 00852 ret = modbus_receive(mb_param, query, response); 00853 if (ret < 0) 00854 return ret; 00855 00856 offset = mb_param->header_length; 00857 00858 offset_length = offset + ret; 00859 for (i = offset; i < offset_length; i++) { 00860 /* Shift reg hi_byte to temp */ 00861 temp = response[3 + i]; 00862 00863 for (bit = 0x01; (bit & 0xff) && (pos < nb);) { 00864 data_dest[pos++] = (temp & bit) ? TRUE : FALSE; 00865 bit = bit << 1; 00866 } 00867 00868 } 00869 } 00870 00871 return ret; 00872 } 00873 00874 /* Reads the boolean status of coils and sets the array elements 00875 in the destination to TRUE or FALSE. */ 00876 int read_coil_status(modbus_param_t *mb_param, int slave, int start_addr, 00877 int nb, uint8_t *data_dest) 00878 { 00879 int status; 00880 00881 if (nb > MAX_STATUS) { 00882 printf("ERROR Too many coils status requested (%d > %d)\n", 00883 nb, MAX_STATUS); 00884 return TOO_MANY_DATA; 00885 } 00886 00887 status = read_io_status(mb_param, slave, FC_READ_COIL_STATUS, 00888 start_addr, nb, data_dest); 00889 00890 if (status > 0) 00891 status = nb; 00892 00893 return status; 00894 } 00895 00896 00897 /* Same as read_coil_status but reads the slaves input table */ 00898 int read_input_status(modbus_param_t *mb_param, int slave, int start_addr, 00899 int nb, uint8_t *data_dest) 00900 { 00901 int status; 00902 00903 if (nb > MAX_STATUS) { 00904 printf("ERROR Too many input status requested (%d > %d)\n", 00905 nb, MAX_STATUS); 00906 return TOO_MANY_DATA; 00907 } 00908 00909 status = read_io_status(mb_param, slave, FC_READ_INPUT_STATUS, 00910 start_addr, nb, data_dest); 00911 00912 if (status > 0) 00913 status = nb; 00914 00915 return status; 00916 } 00917 00918 /* Reads the data from a modbus slave and put that data into an array */ 00919 static int read_registers(modbus_param_t *mb_param, int slave, int function, 00920 int start_addr, int nb, uint16_t *data_dest) 00921 { 00922 int ret; 00923 int query_length; 00924 uint8_t query[MIN_QUERY_LENGTH]; 00925 uint8_t response[MAX_MESSAGE_LENGTH]; 00926 00927 if (nb > MAX_REGISTERS) { 00928 printf("EROOR Too many holding registers requested (%d > %d)\n", 00929 nb, MAX_REGISTERS); 00930 return TOO_MANY_DATA; 00931 } 00932 00933 query_length = build_query_basis(mb_param, slave, function, 00934 start_addr, nb, query); 00935 00936 ret = modbus_send(mb_param, query, query_length); 00937 00938 /* 00939 if (ret > 0) { 00940 int offset; 00941 int i; 00942 00943 ret = modbus_receive(mb_param, query, response); 00944 00945 offset = mb_param->header_length; 00946 00947 // If ret is negative, the loop is jumped ! 00948 for (i = 0; i < ret; i++) { 00949 // shift reg hi_byte to temp OR with lo_byte 00950 data_dest[i] = (response[offset + 3 + (i << 1)] << 8) | 00951 response[offset + 4 + (i << 1)]; 00952 } 00953 } 00954 */ 00955 return ret; 00956 } 00957 00958 /* Reads the holding registers in a slave and put the data into an 00959 array */ 00960 int read_holding_registers(modbus_param_t *mb_param, int slave, 00961 int start_addr, int nb, uint16_t *data_dest) 00962 { 00963 int status; 00964 00965 if (nb > MAX_REGISTERS) { 00966 printf("ERROR Too many holding registers requested (%d > %d)\n", 00967 nb, MAX_REGISTERS); 00968 return TOO_MANY_DATA; 00969 } 00970 00971 status = read_registers(mb_param, slave, FC_READ_HOLDING_REGISTERS, 00972 start_addr, nb, data_dest); 00973 return status; 00974 } 00975 00976 /* Reads the input registers in a slave and put the data into 00977 an array */ 00978 int read_input_registers(modbus_param_t *mb_param, int slave, 00979 int start_addr, int nb, uint16_t *data_dest) 00980 { 00981 int status; 00982 00983 if (nb > MAX_REGISTERS) { 00984 printf("ERROR Too many input registers requested (%d > %d)\n", 00985 nb, MAX_REGISTERS); 00986 return TOO_MANY_DATA; 00987 } 00988 00989 status = read_registers(mb_param, slave, FC_READ_INPUT_REGISTERS, 00990 start_addr, nb, data_dest); 00991 00992 return status; 00993 } 00994 00995 /* Sends a value to a register in a slave. 00996 Used by force_single_coil and preset_single_register */ 00997 static int set_single(modbus_param_t *mb_param, int slave, int function, 00998 int addr, int value) 00999 { 01000 int ret; 01001 int query_length; 01002 uint8_t query[MIN_QUERY_LENGTH]; 01003 01004 query_length = build_query_basis(mb_param, slave, function, 01005 addr, value, query); 01006 01007 ret = modbus_send(mb_param, query, query_length); 01008 if (ret > 0) { 01009 /* Used by force_single_coil and 01010 * preset_single_register */ 01011 uint8_t response[MIN_QUERY_LENGTH]; 01012 ret = modbus_receive(mb_param, query, response); 01013 } 01014 01015 return ret; 01016 } 01017 01018 /* Turns ON or OFF a single coil in the slave device */ 01019 int force_single_coil(modbus_param_t *mb_param, int slave, 01020 int coil_addr, int state) 01021 { 01022 int status; 01023 01024 if (state) 01025 state = 0xFF00; 01026 01027 status = set_single(mb_param, slave, FC_FORCE_SINGLE_COIL, 01028 coil_addr, state); 01029 01030 return status; 01031 } 01032 01033 /* Sets a value in one holding register in the slave device */ 01034 int preset_single_register(modbus_param_t *mb_param, int slave, 01035 int reg_addr, int value) 01036 { 01037 int status; 01038 01039 status = set_single(mb_param, slave, FC_PRESET_SINGLE_REGISTER, 01040 reg_addr, value); 01041 01042 return status; 01043 } 01044 01045 /* Sets/resets the coils in the slave from an array in argument */ 01046 int force_multiple_coils(modbus_param_t *mb_param, int slave, 01047 int start_addr, int nb, 01048 const uint8_t *data_src) 01049 { 01050 int ret; 01051 int i; 01052 int byte_count; 01053 int query_length; 01054 int coil_check = 0; 01055 int pos = 0; 01056 01057 uint8_t query[MAX_MESSAGE_LENGTH]; 01058 01059 if (nb > MAX_STATUS) { 01060 printf("ERROR Writing to too many coils (%d > %d)\n", 01061 nb, MAX_STATUS); 01062 return TOO_MANY_DATA; 01063 } 01064 01065 query_length = build_query_basis(mb_param, slave, 01066 FC_FORCE_MULTIPLE_COILS, 01067 start_addr, nb, query); 01068 byte_count = (nb / 8) + ((nb % 8) ? 1 : 0); 01069 query[query_length++] = byte_count; 01070 01071 for (i = 0; i < byte_count; i++) { 01072 int bit; 01073 01074 bit = 0x01; 01075 query[query_length] = 0; 01076 01077 while ((bit & 0xFF) && (coil_check++ < nb)) { 01078 if (data_src[pos++]) 01079 query[query_length] |= bit; 01080 else 01081 query[query_length] &=~ bit; 01082 01083 bit = bit << 1; 01084 } 01085 query_length++; 01086 } 01087 01088 ret = modbus_send(mb_param, query, query_length); 01089 if (ret > 0) { 01090 uint8_t response[MAX_MESSAGE_LENGTH]; 01091 ret = modbus_receive(mb_param, query, response); 01092 } 01093 01094 01095 return ret; 01096 } 01097 01098 /* Copies the values in the slave from the array given in argument */ 01099 int preset_multiple_registers(modbus_param_t *mb_param, int slave, 01100 int start_addr, int nb, 01101 const uint16_t *data_src) 01102 { 01103 int ret; 01104 int i; 01105 int query_length; 01106 int byte_count; 01107 01108 uint8_t query[MAX_MESSAGE_LENGTH]; 01109 01110 if (nb > MAX_REGISTERS) { 01111 printf("ERROR Trying to write to too many registers (%d > %d)\n", 01112 nb, MAX_REGISTERS); 01113 return TOO_MANY_DATA; 01114 } 01115 01116 query_length = build_query_basis(mb_param, slave, 01117 FC_PRESET_MULTIPLE_REGISTERS, 01118 start_addr, nb, query); 01119 byte_count = nb * 2; 01120 query[query_length++] = byte_count; 01121 01122 for (i = 0; i < nb; i++) { 01123 query[query_length++] = data_src[i] >> 8; 01124 query[query_length++] = data_src[i] & 0x00FF; 01125 } 01126 01127 ret = modbus_send(mb_param, query, query_length); 01128 if (ret > 0) { 01129 uint8_t response[MAX_MESSAGE_LENGTH]; 01130 ret = modbus_receive(mb_param, query, response); 01131 } 01132 01133 return ret; 01134 } 01135 01136 /* Returns the slave id! */ 01137 int report_slave_id(modbus_param_t *mb_param, int slave, 01138 uint8_t *data_dest) 01139 { 01140 int ret; 01141 int query_length; 01142 uint8_t query[MIN_QUERY_LENGTH]; 01143 01144 query_length = build_query_basis(mb_param, slave, FC_REPORT_SLAVE_ID, 01145 0, 0, query); 01146 01147 /* HACKISH, start_addr and count are not used */ 01148 query_length -= 4; 01149 01150 ret = modbus_send(mb_param, query, query_length); 01151 if (ret > 0) { 01152 int i; 01153 int offset; 01154 int offset_length; 01155 uint8_t response[MAX_MESSAGE_LENGTH]; 01156 01157 /* Byte count, slave id, run indicator status, 01158 additional data */ 01159 ret = modbus_receive(mb_param, query, response); 01160 if (ret < 0) 01161 return ret; 01162 01163 offset = mb_param->header_length; 01164 offset_length = offset + ret; 01165 01166 for (i = offset; i < offset_length; i++) 01167 data_dest[i] = response[i]; 01168 } 01169 01170 return ret; 01171 } 01172 01173 /* Initializes the modbus_param_t structure for RTU 01174 - device: "/dev/ttyS0" 01175 - baud: 9600, 19200, 57600, 115200, etc 01176 - parity: "even", "odd" or "none" 01177 - data_bits: 5, 6, 7, 8 01178 - stop_bits: 1, 2 01179 */ 01180 void modbus_init_rtu(modbus_param_t *mb_param, const char *device, 01181 int baud, const char *parity, int data_bit, 01182 int stop_bit) 01183 { 01184 memset(mb_param, 0, sizeof(modbus_param_t)); 01185 strcpy(mb_param->device, device); 01186 mb_param->baud = baud; 01187 strcpy(mb_param->parity, parity); 01188 mb_param->debug = FALSE; 01189 mb_param->data_bit = data_bit; 01190 mb_param->stop_bit = stop_bit; 01191 mb_param->type_com = RTU; 01192 mb_param->header_length = HEADER_LENGTH_RTU; 01193 mb_param->checksum_length = CHECKSUM_LENGTH_RTU; 01194 } 01195 01196 01197 /* By default, the error handling mode used is FLUSH_OR_RECONNECT_ON_ERROR. 01198 01199 With FLUSH_OR_RECONNECT_ON_ERROR, the library will flush to I/O 01200 port in RTU mode or attempt an immediate reconnection which may 01201 hang for several seconds if the network to the remote target unit 01202 is down in TCP mode. 01203 01204 With NOP_ON_ERROR, it is expected that the application will 01205 check for error returns and deal with them as necessary. 01206 */ 01207 void modbus_set_error_handling(modbus_param_t *mb_param, 01208 error_handling_t error_handling) 01209 { 01210 if (error_handling == FLUSH_OR_RECONNECT_ON_ERROR || 01211 error_handling == NOP_ON_ERROR) { 01212 mb_param->error_handling = error_handling; 01213 } else { 01214 printf("Invalid setting for error handling (not changed)\n"); 01215 } 01216 } 01217 01218 01219 /* Sets up a serial port for RTU communications */ 01220 static int modbus_connect_rtu(modbus_param_t *mb_param) 01221 { 01222 01223 return 0; 01224 } 01225 01226 01227 /* Establishes a modbus connexion. 01228 Returns -1 if an error occured. */ 01229 int modbus_connect(modbus_param_t *mb_param) 01230 { 01231 int ret; 01232 ret = modbus_connect_rtu(mb_param); 01233 01234 return ret; 01235 } 01236 01237 /* Closes the file descriptor in RTU mode */ 01238 static void modbus_close_rtu(modbus_param_t *mb_param) 01239 { 01240 01241 } 01242 01243 /* Closes a modbus connection */ 01244 void modbus_close(modbus_param_t *mb_param) 01245 { 01246 modbus_close_rtu(mb_param); 01247 01248 } 01249 01250 /* Activates the debug messages */ 01251 void modbus_set_debug(modbus_param_t *mb_param, int boolean) 01252 { 01253 mb_param->debug = boolean; 01254 } 01255 01256 /* Allocates 4 arrays to store coils, input status, input registers and 01257 holding registers. The pointers are stored in modbus_mapping structure. 01258 01259 Returns: TRUE if ok, FALSE on failure 01260 */ 01261 int modbus_mapping_new(modbus_mapping_t *mb_mapping, 01262 int nb_coil_status, int nb_input_status, 01263 int nb_holding_registers, int nb_input_registers) 01264 { 01265 /* 0X */ 01266 mb_mapping->nb_coil_status = nb_coil_status; 01267 mb_mapping->tab_coil_status = 01268 (uint8_t *) malloc(nb_coil_status * sizeof(uint8_t)); 01269 memset(mb_mapping->tab_coil_status, 0, 01270 nb_coil_status * sizeof(uint8_t)); 01271 if (mb_mapping->tab_coil_status == NULL) 01272 return FALSE; 01273 01274 /* 1X */ 01275 mb_mapping->nb_input_status = nb_input_status; 01276 mb_mapping->tab_input_status = 01277 (uint8_t *) malloc(nb_input_status * sizeof(uint8_t)); 01278 memset(mb_mapping->tab_input_status, 0, 01279 nb_input_status * sizeof(uint8_t)); 01280 if (mb_mapping->tab_input_status == NULL) { 01281 free(mb_mapping->tab_coil_status); 01282 return FALSE; 01283 } 01284 01285 /* 4X */ 01286 mb_mapping->nb_holding_registers = nb_holding_registers; 01287 mb_mapping->tab_holding_registers = 01288 (uint16_t *) malloc(nb_holding_registers * sizeof(uint16_t)); 01289 memset(mb_mapping->tab_holding_registers, 0, 01290 nb_holding_registers * sizeof(uint16_t)); 01291 if (mb_mapping->tab_holding_registers == NULL) { 01292 free(mb_mapping->tab_coil_status); 01293 free(mb_mapping->tab_input_status); 01294 return FALSE; 01295 } 01296 01297 /* 3X */ 01298 mb_mapping->nb_input_registers = nb_input_registers; 01299 mb_mapping->tab_input_registers = 01300 (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t)); 01301 memset(mb_mapping->tab_input_registers, 0, 01302 nb_input_registers * sizeof(uint16_t)); 01303 if (mb_mapping->tab_input_registers == NULL) { 01304 free(mb_mapping->tab_coil_status); 01305 free(mb_mapping->tab_input_status); 01306 free(mb_mapping->tab_holding_registers); 01307 return FALSE; 01308 } 01309 01310 return TRUE; 01311 } 01312 01313 /* Frees the 4 arrays */ 01314 void modbus_mapping_free(modbus_mapping_t *mb_mapping) 01315 { 01316 free(mb_mapping->tab_coil_status); 01317 free(mb_mapping->tab_input_status); 01318 free(mb_mapping->tab_holding_registers); 01319 free(mb_mapping->tab_input_registers); 01320 } 01321 /** Utils **/ 01322 01323 /* Sets many input/coil status from a single byte value (all 8 bits of 01324 the byte value are setted) */ 01325 void set_bits_from_byte(uint8_t *dest, int address, const uint8_t value) 01326 { 01327 int i; 01328 01329 for (i=0; i<8; i++) { 01330 dest[address+i] = (value & (1 << i)) ? ON : OFF; 01331 } 01332 } 01333 01334 /* Sets many input/coil status from a table of bytes (only the bits 01335 between address and address + nb_bits are setted) */ 01336 void set_bits_from_bytes(uint8_t *dest, int address, int nb_bits, 01337 const uint8_t tab_byte[]) 01338 { 01339 int i; 01340 int shift = 0; 01341 01342 for (i = address; i < address + nb_bits; i++) { 01343 dest[i] = tab_byte[(i - address) / 8] & (1 << shift) ? ON : OFF; 01344 /* gcc doesn't like: shift = (++shift) % 8; */ 01345 shift++; 01346 shift %= 8; 01347 } 01348 } 01349 01350 /* Gets the byte value from many input/coil status. 01351 To obtain a full byte, set nb_bits to 8. */ 01352 uint8_t get_byte_from_bits(const uint8_t *src, int address, int nb_bits) 01353 { 01354 int i; 01355 uint8_t value = 0; 01356 01357 if (nb_bits > 8) { 01358 printf("Error: nb_bits is too big\n"); 01359 nb_bits = 8; 01360 } 01361 01362 for (i=0; i < nb_bits; i++) { 01363 value |= (src[address+i] << i); 01364 } 01365 01366 return value; 01367 }
Generated on Wed Jul 20 2022 21:56:37 by
1.7.2