Kyrylo Cherneha / ModBusTCP

ModBusTCP with some fixes

ModBus-Req.cpp@5:1bf121618c2f, 2020-12-03 (annotated)

Committer:
kcherneha_aitheon
Date:
Thu Dec 03 14:01:33 2020 +0000
Revision:
5:1bf121618c2f
Parent:
3:ebea8e061ae6 
fixed error jump to case label [-fpermissive]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gulchi 3:ebea8e061ae6 1
gulchi 3:ebea8e061ae6 2
gulchi 3:ebea8e061ae6 3 #include "ModBus-TCP.h"
gulchi 3:ebea8e061ae6 4
gulchi 3:ebea8e061ae6 5 void Modbus::recvData (int id, char buffer[], int size) {
gulchi 3:ebea8e061ae6 6
gulchi 3:ebea8e061ae6 7 int bytes_rec;
gulchi 3:ebea8e061ae6 8 int returnLength;
gulchi 3:ebea8e061ae6 9 unsigned short transaction_number;
gulchi 3:ebea8e061ae6 10 unsigned short protokoll;
gulchi 3:ebea8e061ae6 11 unsigned short number_of_bytes;
gulchi 3:ebea8e061ae6 12 char adress;
gulchi 3:ebea8e061ae6 13 int port;
gulchi 3:ebea8e061ae6 14
gulchi 3:ebea8e061ae6 15 transaction_number = buffer[0] + (buffer[1] << 8);
gulchi 3:ebea8e061ae6 16 protokoll = buffer[2] + (buffer[3] << 8);
gulchi 3:ebea8e061ae6 17 number_of_bytes = (buffer[4] << 8) + buffer[5] ;
gulchi 3:ebea8e061ae6 18 adress = buffer[6];
gulchi 3:ebea8e061ae6 19
gulchi 3:ebea8e061ae6 20 if(protokoll != 0x0000) {
gulchi 3:ebea8e061ae6 21 printf("No Modbus protkoll");
gulchi 3:ebea8e061ae6 22 }
gulchi 3:ebea8e061ae6 23
gulchi 3:ebea8e061ae6 24 #ifdef DEBUG
gulchi 3:ebea8e061ae6 25 //printf("tn: 0x%x, p: 0x%x, nb: %i, a: %i\r\n", transaction_number, protokoll, number_of_bytes, adress);
gulchi 3:ebea8e061ae6 26 #endif
gulchi 3:ebea8e061ae6 27
gulchi 3:ebea8e061ae6 28 for(int i=0; i<8; i++) {
gulchi 3:ebea8e061ae6 29 _state[id].start_message[i] = buffer[i];
gulchi 3:ebea8e061ae6 30 }
gulchi 3:ebea8e061ae6 31
gulchi 3:ebea8e061ae6 32 //returnLength = mb_handle_data(buffer, number_of_bytes);
gulchi 3:ebea8e061ae6 33
gulchi 3:ebea8e061ae6 34 char fc;
gulchi 3:ebea8e061ae6 35 fc = buffer[7];
gulchi 3:ebea8e061ae6 36
gulchi 3:ebea8e061ae6 37
gulchi 3:ebea8e061ae6 38 unsigned short start_adress;
gulchi 3:ebea8e061ae6 39 unsigned short number_of_registers;
gulchi 3:ebea8e061ae6 40
gulchi 3:ebea8e061ae6 41
gulchi 3:ebea8e061ae6 42 switch(fc) {
gulchi 3:ebea8e061ae6 43 case 0x04: //return mb_read_register(data_in, length);
kcherneha_aitheon 5:1bf121618c2f 44 {
gulchi 3:ebea8e061ae6 45 //Read Requet received
gulchi 3:ebea8e061ae6 46 // Getting Start Address and Length
gulchi 3:ebea8e061ae6 47
gulchi 3:ebea8e061ae6 48 //DBG("Read Request received\r\n");
gulchi 3:ebea8e061ae6 49
gulchi 3:ebea8e061ae6 50 start_adress = (buffer[8] << 8) + buffer[9];
gulchi 3:ebea8e061ae6 51 number_of_registers = (buffer[10] << 8) + buffer[11];
gulchi 3:ebea8e061ae6 52
gulchi 3:ebea8e061ae6 53
gulchi 3:ebea8e061ae6 54 int i = getHandler(0);
gulchi 3:ebea8e061ae6 55 //DBG("GetHandler %i",i);
gulchi 3:ebea8e061ae6 56
gulchi 3:ebea8e061ae6 57 if(number_of_registers < 0x0001 || number_of_registers > 0x007d ) {
gulchi 3:ebea8e061ae6 58 mb_exception_handler(id, buffer, number_of_registers, 0x03);
gulchi 3:ebea8e061ae6 59 }
gulchi 3:ebea8e061ae6 60
gulchi 3:ebea8e061ae6 61 if(start_adress >= MODBUS_MAX_REGISTER) {
gulchi 3:ebea8e061ae6 62 mb_exception_handler(id, buffer, number_of_registers, 0x02);
gulchi 3:ebea8e061ae6 63 }
gulchi 3:ebea8e061ae6 64
gulchi 3:ebea8e061ae6 65 if(number_of_registers < 0x0001 || number_of_registers + start_adress > MODBUS_MAX_REGISTER) {
gulchi 3:ebea8e061ae6 66 mb_exception_handler(id, buffer, number_of_registers, 0x04);
gulchi 3:ebea8e061ae6 67 }
gulchi 3:ebea8e061ae6 68
gulchi 3:ebea8e061ae6 69
gulchi 3:ebea8e061ae6 70
gulchi 3:ebea8e061ae6 71 _handler[i].funcCgiRead(id, start_adress, number_of_registers);
gulchi 3:ebea8e061ae6 72 break;
kcherneha_aitheon 5:1bf121618c2f 73 }
gulchi 3:ebea8e061ae6 74 case 0x10: //return mb_write_register(data_in, length);
kcherneha_aitheon 5:1bf121618c2f 75 {
gulchi 3:ebea8e061ae6 76 break;
kcherneha_aitheon 5:1bf121618c2f 77 }
kcherneha_aitheon 5:1bf121618c2f 78 default:
kcherneha_aitheon 5:1bf121618c2f 79 {
kcherneha_aitheon 5:1bf121618c2f 80 mb_exception_handler(id, buffer, size, 0x01);
kcherneha_aitheon 5:1bf121618c2f 81 }
gulchi 3:ebea8e061ae6 82 }
gulchi 3:ebea8e061ae6 83 }
gulchi 3:ebea8e061ae6 84
gulchi 3:ebea8e061ae6 85 int Modbus::mb_exception_handler(int id, char data_in[], unsigned short length, char error) {
gulchi 3:ebea8e061ae6 86 unsigned short number_of_bytes;
gulchi 3:ebea8e061ae6 87
gulchi 3:ebea8e061ae6 88 data_in[7] += 0x80;
gulchi 3:ebea8e061ae6 89 data_in[8] += error;
gulchi 3:ebea8e061ae6 90
gulchi 3:ebea8e061ae6 91 number_of_bytes = 3;
gulchi 3:ebea8e061ae6 92
gulchi 3:ebea8e061ae6 93 data_in[4] = (char)(number_of_bytes >> 8);
gulchi 3:ebea8e061ae6 94 data_in[5] = (char)(number_of_bytes & 0x00ff);
gulchi 3:ebea8e061ae6 95
gulchi 3:ebea8e061ae6 96 sendToClient(id, data_in, number_of_bytes + 7);
gulchi 3:ebea8e061ae6 97
gulchi 3:ebea8e061ae6 98 return 0;
gulchi 3:ebea8e061ae6 99 }
gulchi 3:ebea8e061ae6 100
gulchi 3:ebea8e061ae6 101