thiagoralves/OpenPLC_Editor
1
0
Fork 0
mirror of https://github.com/thiagoralves/OpenPLC_Editor.git synced 2025-05-09 00:21:53 +08:00
Code Issues Releases Wiki Activity
OpenPLC_Editor/editor/arduino/examples/Baremetal/ModbusSlave.cpp

1103 lines
35 KiB
C++
Raw Blame History

/*
ModbusSlave.cpp - Source for Modbus Slave Library
Copyright (C) 2022 OpenPLC - Thiago Alves
*/
#include "ModbusSlave.h"
//Global Modbus vars
struct MBinfo modbus;
uint8_t mb_frame[MAX_MB_FRAME];
uint16_t mb_frame_len;
Stream* mb_serialport;
int8_t mb_txpin;
uint16_t mb_t15; // inter character time out
uint16_t mb_t35; // frame delay
#ifdef MBTCP_ETHERNET
EthernetServer mb_server(502);
uint8_t mb_mbap[MBAP_SIZE];
#ifdef BOARD_PORTENTA
EthernetClient mb_serverClients[MAX_SRV_CLIENTS];
#endif
#endif
#ifdef MBTCP_WIFI
WiFiServer mb_server(502);
uint8_t mb_mbap[MBAP_SIZE];
#if defined(BOARD_ESP8266) || defined(BOARD_ESP32) || defined(BOARD_PORTENTA)
WiFiClient mb_serverClients[MAX_SRV_CLIENTS];
#endif
#endif
bool init_mbregs(uint8_t size_holding, uint8_t size_coils, uint8_t size_inputregs, uint8_t size_inputstatus)
{
//Save sizes
modbus.holding_size = size_holding;
modbus.coils_size = size_coils;
modbus.input_regs_size = size_inputregs;
modbus.input_status_size = size_inputstatus;
//round discrete regs sizes
if (size_coils % 8 > 0)
size_coils = (size_coils / 8) + 1;
else
size_coils = size_coils / 8;
if (size_inputstatus % 8 > 0)
size_inputstatus = (size_inputstatus / 8) + 1;
else
size_inputstatus = (size_inputstatus / 8);
modbus.coils = (uint8_t *)malloc(size_coils * sizeof(uint8_t));
if (modbus.coils == NULL) return false;
memset(modbus.coils, 0, size_coils * sizeof(uint8_t));
modbus.holding = (uint16_t *)malloc(size_holding * sizeof(uint16_t));
if (modbus.holding == NULL) return false;
memset(modbus.holding, 0, size_holding * sizeof(uint8_t));
modbus.input_status = (uint8_t *)malloc(size_inputstatus * sizeof(uint8_t));
if (modbus.input_status == NULL) return false;
memset(modbus.input_status, 0, size_inputstatus * sizeof(uint8_t));
modbus.input_regs = (uint16_t *)malloc(size_inputregs * sizeof(uint16_t));
if (modbus.input_regs == NULL) return false;
memset(modbus.input_regs, 0, size_inputregs * sizeof(uint8_t));
return true;
}
bool get_discrete(uint16_t addr, bool regtype)
{
uint8_t byte_addr = addr / 8;
uint8_t bit_addr = addr % 8;
if (regtype == COILS)
return bitRead(modbus.coils[byte_addr], bit_addr);
else
return bitRead(modbus.input_status[byte_addr], bit_addr);
}
void write_discrete(uint16_t addr, bool regtype, bool value)
{
uint8_t byte_addr = addr / 8;
uint8_t bit_addr = addr % 8;
if (regtype == COILS)
bitWrite(modbus.coils[byte_addr], bit_addr, value);
else
bitWrite(modbus.input_status[byte_addr], bit_addr, value);
}
void mbconfig_serial_iface(HardwareSerial* port, long baud, int txPin)
{
mb_serialport = port;
mb_txpin = txPin;
(*port).begin(baud);
//RS-485 control
if (txPin >= 0)
{
pinMode(txPin, OUTPUT);
digitalWrite(txPin, LOW);
}
#if defined(CONTROLLINO_MAXI) || defined(CONTROLLINO_MEGA)
if (mb_serialport == &Serial3)
Controllino_RS485Init();
#endif
// Modbus states that a baud rate higher than 19200 must use a fixed 750 us
// for inter character time out. For baud rates below 19200 the timing
// is more critical and has to be calculated.
// E.g. 9600 baud in a 11 bit packet is 9600/11 = 872 characters per second
// In milliseconds this will be 872 characters per 1000ms. So for 1 character
// 1000ms/872 characters is 1.14583ms per character. Finally modbus states
// an inter-character must be 1.5T or 1.5 times longer than a character. Thus
// 1.5T = 1.14583ms * 1.5 = 1.71875ms.
// Thus the formula is T1.5(us) = (1000ms * 1000(us) * 1.5 * 11bits)/baud
// 1000ms * 1000(us) * 1.5 * 11bits = 16500000 can be calculated as a constant
if (baud > 19200)
mb_t15 = 750;
else
mb_t15 = 16500000/baud; // 1T * 1.5 = T1.5
/* The modbus definition of a frame delay is a waiting period of 3.5 character times
between packets.*/
mb_t35 = mb_t15 * 3.5;
}
#ifdef MBTCP
void mbconfig_ethernet_iface(uint8_t *mac, uint8_t *ip, uint8_t *dns, uint8_t *gateway, uint8_t *subnet)
{
#ifdef MBTCP_ETHERNET
if (ip == NULL)
Ethernet.begin(mac);
else if (dns == NULL)
Ethernet.begin(mac, IPAddress(ip));
else if (gateway == NULL)
Ethernet.begin(mac, IPAddress(ip), IPAddress(dns));
else if (subnet == NULL)
Ethernet.begin(mac, IPAddress(ip), IPAddress(dns), IPAddress(gateway));
else
Ethernet.begin(mac, IPAddress(ip), IPAddress(dns), IPAddress(gateway), IPAddress(subnet));
#endif
#ifdef MBTCP_WIFI
#if defined(BOARD_ESP8266) || defined(BOARD_ESP32)
if (ip != NULL && gateway != NULL && subnet != NULL && dns != NULL)
{
uint8_t secondaryDNS[] = {8, 8, 8, 8};
WiFi.config(IPAddress(ip), IPAddress(gateway), IPAddress(subnet), IPAddress(dns), IPAddress(secondaryDNS));
}
mb_server.setNoDelay(true);
#elif defined(BOARD_PORTENTA)
if (ip != NULL && subnet != NULL && gateway != NULL)
{
WiFi.config(IPAddress(ip), IPAddress(subnet), IPAddress(gateway));
}
#else
if (ip != NULL)
{
if (dns == NULL)
WiFi.config(IPAddress(ip));
else if (gateway == NULL)
WiFi.config(IPAddress(ip), IPAddress(dns));
else if (subnet == NULL)
WiFi.config(IPAddress(ip), IPAddress(dns), IPAddress(gateway));
else
WiFi.config(IPAddress(ip), IPAddress(dns), IPAddress(gateway), IPAddress(subnet));
}
#endif
WiFi.begin(MBTCP_SSID, MBTCP_PWD);
int num_tries = 0;
while (WiFi.status() != WL_CONNECTED)
{
delay(500);
num_tries++;
if (num_tries == 10) break;
}
#endif
mb_server.begin();
}
#endif
void mbtask()
{
#ifdef MBTCP
handle_tcp();
#endif
#ifdef MBSERIAL
handle_serial();
#endif
}
#ifdef MBTCP
void handle_tcp()
{
#ifdef MBTCP_ETHERNET
EthernetClient client = mb_server.available();
#endif
#if defined(MBTCP_WIFI) && !defined(BOARD_ESP8266) && !defined(BOARD_ESP32)
WiFiClient client = mb_server.available();
#endif
//ESP and Portenta boards have a slightly different implementation of the WiFi/Ethernet API - therefore their specific
//code lies below
#if (defined(BOARD_ESP8266) || defined(BOARD_ESP32) || defined(BOARD_PORTENTA)) && (defined(MBTCP_WIFI) || defined(MBTCP_ETHERNET))
#ifdef BOARD_PORTENTA
if (client)
#else
if (mb_server.hasClient())
#endif
{
for (int i = 0; i < MAX_SRV_CLIENTS; i++)
{
if (!mb_serverClients[i]) //equivalent to !serverClients[i].connected()
{
#ifdef BOARD_PORTENTA
mb_serverClients[i] = client;
#else
mb_serverClients[i] = mb_server.available();
#endif
break;
}
}
}
//search all clients for data
for (int i = 0; i < MAX_SRV_CLIENTS; i++)
{
int j = 0;
if (mb_serverClients[i].connected() && mb_serverClients[i].available())
{
//Read packet
while (mb_serverClients[i].available())
{
mb_mbap[j] = mb_serverClients[i].read();
j++;
if (j==MBAP_SIZE) break; //MBAP has 6 bytes (we use UnitID as SlaveID)
}
mb_frame_len = mb_mbap[4] << 8 | mb_mbap[5];
if (mb_mbap[2] !=0 || mb_mbap[3] !=0) return; //Not a MODBUSIP packet
if (mb_frame_len < 6 || mb_frame_len > MAX_MB_FRAME) return; //Packet is too small or too big
j = 0;
while (mb_serverClients[i].available())
{
mb_frame[j] = mb_serverClients[i].read();
j++;
if (j==mb_frame_len) break;
}
//Safety check - discard packages that lie about their size
if (j != mb_frame_len) return;
//Process packet and write back
process_mbpacket();
//Calculate packet length for MBAP header (mb_frame_len + 1)
mb_mbap[4] = (mb_frame_len) >> 8;
mb_mbap[5] = (mb_frame_len) & 0x00FF;
uint8_t sendbuffer[mb_frame_len + MBAP_SIZE];
//MBAP
for (j = 0 ; j < MBAP_SIZE ; j++)
sendbuffer[j] = mb_mbap[j];
//PDU Frame
for (j = 0 ; j < mb_frame_len ; j++)
sendbuffer[j+MBAP_SIZE] = mb_frame[j];
//Write back
mb_serverClients[i].write(sendbuffer, mb_frame_len + MBAP_SIZE);
}
}
//If this is not an ESP board or Portenta board, then here is the default code
#else
if (client)
{
if (client.connected())
{
int i = 0;
while (client.available())
{
mb_mbap[i] = client.read();
i++;
if (i==MBAP_SIZE) break; //MBAP has 6 bytes (we use UnitID as SlaveID)
}
mb_frame_len = mb_mbap[4] << 8 | mb_mbap[5];
if (mb_mbap[2] !=0 || mb_mbap[3] !=0) return; //Not a MODBUSIP packet
if (mb_frame_len < 6 || mb_frame_len > MAX_MB_FRAME) return; //Packet is too small or too big
i = 0;
while (client.available())
{
mb_frame[i] = client.read();
i++;
if (i==mb_frame_len || i==MAX_MB_FRAME) break;
}
//Safety check - discard packages that lie about their size
if (i != mb_frame_len) return;
//Process packet and write back
process_mbpacket();
//Calculate packet length for MBAP header (mb_frame_len + 1)
mb_mbap[4] = (mb_frame_len) >> 8;
mb_mbap[5] = (mb_frame_len) & 0x00FF;
uint8_t sendbuffer[mb_frame_len + MBAP_SIZE];
//MBAP
for (i = 0 ; i < MBAP_SIZE ; i++)
sendbuffer[i] = mb_mbap[i];
//PDU Frame
for (i = 0 ; i < mb_frame_len ; i++)
sendbuffer[i+MBAP_SIZE] = mb_frame[i];
//Write back
client.write(sendbuffer, mb_frame_len + MBAP_SIZE);
}
}
#endif
}
#endif
#ifdef MBSERIAL
void handle_serial()
{
mb_frame_len = 0;
if ((*mb_serialport).available() == 0)
return;
while ((*mb_serialport).available() > mb_frame_len)
{
mb_frame_len = (*mb_serialport).available();
delayMicroseconds(mb_t15);
}
//Check if packet is too big or too small
if ((*mb_serialport).available() > MAX_MB_FRAME || (*mb_serialport).available() < 6)
{
(*mb_serialport).flush();
return;
}
//Read packet
for (uint16_t i = 0; i < mb_frame_len; i++)
{
mb_frame[i] = (*mb_serialport).read();
}
//Validate crc
uint16_t packet_crc = ((mb_frame[mb_frame_len - 2] << 8) | mb_frame[mb_frame_len - 1]);
if (packet_crc != calcCrc())
{
(*mb_serialport).flush();
return;
}
//Validate SlaveID
if (mb_frame[0] != modbus.slaveid)
{
(*mb_serialport).flush();
return;
}
//Remove CRC (must do that before processing packet)
mb_frame_len -= 2;
//Process packet and write back
process_mbpacket();
//Add CRC
//Check if response message is too big for this device
if (mb_frame_len + 2 > MAX_MB_FRAME) exceptionResponse(mb_frame[1], MB_EX_SLAVE_FAILURE);
mb_frame_len += 2; //increase frame length by two bytes to acomodate CRC
packet_crc = calcCrc(); //calculate CRC of the new packet
mb_frame[mb_frame_len - 2] = (uint8_t)(packet_crc >> 8);
mb_frame[mb_frame_len - 1] = (uint8_t)(packet_crc & 0x00FF);
if (mb_txpin >= 0)
{
digitalWrite(mb_txpin, HIGH);
delayMicroseconds(mb_t35);
}
#if defined(CONTROLLINO_MAXI) || defined(CONTROLLINO_MEGA)
if (mb_serialport == &Serial3) // RS485 serial port
Controllino_RS485TxEnable(); // Enable RS485 chip to transmit
#endif
(*mb_serialport).write(mb_frame, mb_frame_len);
(*mb_serialport).flush();
delayMicroseconds(mb_t35);
if (mb_txpin >= 0)
digitalWrite(mb_txpin, LOW);
#if defined(CONTROLLINO_MAXI) || defined(CONTROLLINO_MEGA)
if (mb_serialport == &Serial3) // RS485 serial port
Controllino_RS485RxEnable(); // Go back to receive mode after transmitted data
#endif
}
#endif
void process_mbpacket()
{
uint8_t fcode = mb_frame[1];
uint16_t field1 = (uint16_t)mb_frame[2] << 8 | (uint16_t)mb_frame[3];
uint16_t field2 = (uint16_t)mb_frame[4] << 8 | (uint16_t)mb_frame[5];
uint8_t flag = mb_frame[4];
uint16_t len = (uint16_t)mb_frame[5] << 8 | (uint16_t)mb_frame[6];
void *value = &mb_frame[7];
void *endianness_check = &mb_frame[2];
switch (fcode)
{
case MB_FC_WRITE_REG:
//field1 = reg, field2 = value
writeSingleRegister(field1, field2);
break;
case MB_FC_READ_REGS:
//field1 = startreg, field2 = numregs
readRegisters(field1, field2);
break;
case MB_FC_WRITE_REGS:
//field1 = startreg, field2 = status
writeMultipleRegisters(field1, field2, mb_frame[6]);
break;
case MB_FC_READ_COILS:
//field1 = startreg, field2 = numregs
readCoils(field1, field2);
break;
case MB_FC_READ_INPUT_STAT:
//field1 = startreg, field2 = numregs
readInputStatus(field1, field2);
break;
case MB_FC_READ_INPUT_REGS:
//field1 = startreg, field2 = numregs
readInputRegisters(field1, field2);
break;
case MB_FC_WRITE_COIL:
//field1 = reg, field2 = status
writeSingleCoil(field1, field2);
break;
case MB_FC_WRITE_COILS:
//field1 = startreg, field2 = numoutputs
writeMultipleCoils(field1, field2, mb_frame[6]);
break;
case MB_FC_DEBUG_INFO:
debugInfo();
break;
case MB_FC_DEBUG_GET:
//field1 = startidx, field2 = endidx
debugGetTrace(field1, field2);
break;
case MB_FC_DEBUG_GET_LIST:
//field1 = numIndexes
debugGetTraceList(field1, &mb_frame[4]);
break;
case MB_FC_DEBUG_SET:
//field1 = varidx
debugSetTrace(field1, flag, len, value);
break;
case MB_FC_DEBUG_GET_MD5:
debugGetMd5(endianness_check);
break;
default:
exceptionResponse(fcode, MB_EX_ILLEGAL_FUNCTION);
}
}
//Modbus handling functions
void readRegisters(uint16_t startreg, uint16_t numregs)
{
//Check value (numregs)
if (numregs < 0x0001 || numregs > 0x007D)
{
exceptionResponse(MB_FC_READ_REGS, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address
if ((startreg+numregs) > modbus.holding_size)
{
exceptionResponse(MB_FC_READ_REGS, MB_EX_ILLEGAL_ADDRESS);
return;
}
//calculate the query reply message length
//for each register queried add 2 bytes
mb_frame_len = 3 + (numregs * 2);
if (mb_frame_len > MAX_MB_FRAME)
{
//Response message is too big for this device
exceptionResponse(MB_FC_READ_REGS, MB_EX_SLAVE_FAILURE);
return;
}
//Clean frame buffer (leave only SlaveID)
for (int i = 1; i < mb_frame_len; i++) mb_frame[i] = 0;
mb_frame[1] = MB_FC_READ_REGS;
mb_frame[2] = mb_frame_len - 3; //byte count
uint16_t val;
uint16_t i = 0;
while(numregs--)
{
//retrieve the value from the register bank for the current register
val = modbus.holding[startreg + i];
//write the high byte of the register value
mb_frame[3 + (i * 2)] = val >> 8;
//write the low byte of the register value
mb_frame[4 + (i * 2)] = val & 0xFF;
i++;
}
}
void writeSingleRegister(uint16_t reg, uint16_t value)
{
if (reg > (modbus.holding_size - 1))
{
exceptionResponse(MB_FC_WRITE_REG, MB_EX_ILLEGAL_ADDRESS);
return;
}
modbus.holding[reg] = value;
}
void writeMultipleRegisters(uint16_t startreg, uint16_t numoutputs, uint8_t bytecount)
{
//Check value
if (numoutputs < 0x0001 || numoutputs > 0x007B || bytecount != 2 * numoutputs)
{
exceptionResponse(MB_FC_WRITE_REGS, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address (startreg...startreg + numregs)
if ((startreg + numoutputs) > modbus.holding_size)
{
exceptionResponse(MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS);
return;
}
//Prepare answer frame buffer
mb_frame_len = 6;
mb_frame[1] = MB_FC_WRITE_REGS;
mb_frame[2] = startreg >> 8;
mb_frame[3] = startreg & 0x00FF;
mb_frame[4] = numoutputs >> 8;
mb_frame[5] = numoutputs & 0x00FF;
uint16_t val;
uint16_t i = 0;
while(numoutputs--)
{
val = (uint16_t)mb_frame[7+i*2] << 8 | (uint16_t)mb_frame[8+i*2];
modbus.holding[startreg + i] = val;
i++;
}
}
void exceptionResponse(uint16_t fcode, uint16_t excode)
{
//Clean frame buffer (leave only SlaveID)
mb_frame_len = 3;
for (int i = 0; i < mb_frame_len; i++) mb_frame[i] = 0;
mb_frame[0] = modbus.slaveid;
mb_frame[1] = fcode + 0x80;
mb_frame[2] = excode;
}
void readCoils(uint16_t startreg, uint16_t numregs)
{
//Check value (numregs)
if (numregs < 0x0001 || numregs > 0x07D0)
{
exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address
if (startreg + numregs > modbus.coils_size)
{
exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_ADDRESS);
return;
}
//Determine the message length = slaveid + function type + byte count and
//for each group of 8 registers the message length increases by 1
mb_frame_len = 3 + numregs/8;
if (numregs%8) mb_frame_len++; //Add 1 to the message length for the partial byte.
if (mb_frame_len > MAX_MB_FRAME)
{
//Response message is too big for this device
exceptionResponse(MB_FC_READ_COILS, MB_EX_SLAVE_FAILURE);
return;
}
//Clean frame buffer (leave only SlaveID)
for (int i = 1; i < mb_frame_len; i++) mb_frame[i] = 0;
mb_frame[1] = MB_FC_READ_COILS;
mb_frame[2] = mb_frame_len - 3; //byte count (mb_frame_len - slave id, function code and byte count)
uint8_t bitn = 0;
uint16_t totregs = numregs;
uint16_t i;
while (numregs)
{
i = (totregs - numregs--) / 8;
if (get_discrete((uint8_t)startreg, COILS))
bitSet(mb_frame[3+i], bitn);
else
bitClear(mb_frame[3+i], bitn);
//increment the bit index
bitn++;
if (bitn == 8) bitn = 0;
//increment the register
startreg++;
}
}
void readInputStatus(uint16_t startreg, uint16_t numregs)
{
//Check value (numregs)
if (numregs < 0x0001 || numregs > 0x07D0)
{
exceptionResponse(MB_FC_READ_INPUT_STAT, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address
if ((startreg + numregs) > modbus.input_status_size)
{
exceptionResponse(MB_FC_READ_INPUT_STAT, MB_EX_ILLEGAL_ADDRESS);
return;
}
//Determine the message length = function type, byte count and
//for each group of 8 registers the message length increases by 1
mb_frame_len = 3 + numregs/8;
if (numregs%8) mb_frame_len++; //Add 1 to the message length for the partial byte.
if (mb_frame_len > MAX_MB_FRAME)
{
//Response message is too big for this device
exceptionResponse(MB_FC_READ_INPUT_STAT, MB_EX_SLAVE_FAILURE);
return;
}
//Clean frame buffer (leave only SlaveID)
for (int i = 1; i < mb_frame_len; i++) mb_frame[i] = 0;
mb_frame[1] = MB_FC_READ_INPUT_STAT;
mb_frame[2] = mb_frame_len - 3;
byte bitn = 0;
uint16_t totregs = numregs;
uint16_t i;
while (numregs)
{
i = (totregs - numregs--) / 8;
if (get_discrete(startreg, INPUTSTATUS))
bitSet(mb_frame[3+i], bitn);
else
bitClear(mb_frame[3+i], bitn);
//increment the bit index
bitn++;
if (bitn == 8) bitn = 0;
//increment the register
startreg++;
}
}
void readInputRegisters(uint16_t startreg, uint16_t numregs)
{
//Check value (numregs)
if (numregs < 0x0001 || numregs > 0x007D)
{
exceptionResponse(MB_FC_READ_INPUT_REGS, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address
if ((startreg + numregs) > modbus.input_regs_size)
{
exceptionResponse(MB_FC_READ_INPUT_REGS, MB_EX_ILLEGAL_ADDRESS);
return;
}
//calculate the query reply message length
//for each register queried add 2 bytes
mb_frame_len = 3 + (numregs * 2);
if (mb_frame_len > MAX_MB_FRAME)
{
//Response message is too big for this device
exceptionResponse(MB_FC_READ_INPUT_REGS, MB_EX_SLAVE_FAILURE);
return;
}
//Clean frame buffer (leave only SlaveID)
for (int i = 1; i < mb_frame_len; i++) mb_frame[i] = 0;
mb_frame[1] = MB_FC_READ_INPUT_REGS;
mb_frame[2] = mb_frame_len - 3;
uint16_t val;
uint16_t i = 0;
while(numregs--)
{
//retrieve the value from the register bank for the current register
val = modbus.input_regs[startreg + i];
//write the high byte of the register value
mb_frame[3 + (i * 2)] = val >> 8;
//write the low byte of the register value
mb_frame[4 + (i * 2)] = val & 0xFF;
i++;
}
}
void writeSingleCoil(uint16_t reg, uint16_t status)
{
//Check value (status)
if (status != 0xFF00 && status != 0x0000)
{
exceptionResponse(MB_FC_WRITE_COIL, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address
if (reg > (modbus.coils_size - 1))
{
exceptionResponse(MB_FC_WRITE_COIL, MB_EX_ILLEGAL_ADDRESS);
return;
}
//Execute
write_discrete(reg, COILS, status == 0xFF00 ? true : false);
}
void writeMultipleCoils(uint16_t startreg, uint16_t numoutputs, uint16_t bytecount)
{
//Check value
uint8_t bytecount_calc = numoutputs / 8;
if (numoutputs%8) bytecount_calc++;
if (numoutputs < 0x0001 || numoutputs > 0x07B0 || bytecount != bytecount_calc)
{
exceptionResponse(MB_FC_WRITE_COILS, MB_EX_ILLEGAL_VALUE);
return;
}
//Check Address (startreg...startreg + numregs)
if ((startreg + numoutputs) > modbus.coils_size)
{
exceptionResponse(MB_FC_WRITE_COILS, MB_EX_ILLEGAL_ADDRESS);
return;
}
//Prepare answer frame buffer
mb_frame_len = 6;
mb_frame[1] = MB_FC_WRITE_COILS;
mb_frame[2] = startreg >> 8;
mb_frame[3] = startreg & 0x00FF;
mb_frame[4] = numoutputs >> 8;
mb_frame[5] = numoutputs & 0x00FF;
//Execute
uint8_t bitn = 0;
uint16_t totoutputs = numoutputs;
uint16_t i;
while (numoutputs)
{
i = (totoutputs - numoutputs--) / 8;
write_discrete(startreg, COILS, bitRead(mb_frame[7+i], bitn));
//increment the bit index
bitn++;
if (bitn == 8) bitn = 0;
//increment the register
startreg++;
}
}
/**
* @brief Sends a Modbus response frame for the DEBUG_INFO function code.
*
* This function constructs a Modbus response frame for the DEBUG_INFO function code.
* The response frame includes the number of variables defined in the PLC program.
*
* Modbus Response Frame (DEBUG_INFO):
* +-----+-------+-------+
* | MB | Count | Count |
* | FC | | |
* +-----+-------+-------+
* |0x41 | High | Low |
* | | Byte | Byte |
* | | | |
* +-----+-------+-------+
*
* @return void
*/
void debugInfo()
{
uint16_t variableCount = get_var_count();
mb_frame_len = 4;
mb_frame[1] = MB_FC_DEBUG_INFO;
mb_frame[2] = (uint8_t)(variableCount >> 8); // High byte
mb_frame[3] = (uint8_t)(variableCount & 0xFF); // Low byte
}
/**
* @brief Sends a Modbus response frame for the DEBUG_SET function code.
*
* This function constructs a Modbus response frame for the DEBUG_SET function code.
* The response frame indicates whether the set trace command was successful or if
* there was an error, such as an out-of-bounds index.
*
* Modbus Response Frame (DEBUG_SET):
* +-----+------+
* | MB | Resp.|
* | FC | Code |
* +-----+------+
* |0x42 | Code |
* +-----+------+
*
* @param varidx The index of the variable to set trace for.
* @param flag The trace flag.
* @param len The length of the trace data.
* @param value Pointer to the trace data.
*
* @return void
*/
void debugSetTrace(uint16_t varidx, uint8_t flag, uint16_t len, void *value)
{
uint16_t variableCount = get_var_count();
if (varidx >= variableCount || len > (MAX_MB_FRAME - 7))
{
// Respond with an error indicating that the index is out of range
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_SET;
mb_frame[2] = MB_DEBUG_ERROR_OUT_OF_BOUNDS;
return;
}
// Execute set trace command
set_trace((size_t)varidx, (bool)flag, value);
// Response
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_SET;
mb_frame[2] = MB_DEBUG_SUCCESS;
}
/**
* @brief Sends a Modbus response frame for the DEBUG_GET function code.
*
* This function constructs a Modbus response frame for the DEBUG_GET function code.
* The response frame includes the trace data for variables within the specified index range.
*
* Modbus Response Frame (DEBUG_GET):
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
* | MB | Resp. | Last | Last | Tick | Tick | Tick | Tick | Resp. | Resp.| Data |
* | FC | Code | Index | Index | | | | | Size | Size | Bytes |
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
* |0x44 | Code | High | Low | High | Mid | Mid | Low | High | Low | Data |
* | | | Byte | Byte | Byte | Byte | Byte | Byte | Byte | Byte | Bytes |
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
*
* @param startidx The start index of the variables to get trace for.
* @param endidx The end index of the variables to get trace for.
*
* @return void
*/
void debugGetTrace(uint16_t startidx, uint16_t endidx)
{
uint16_t variableCount = get_var_count();
// Verify that startidx and endidx fall within the valid range of variables
if (startidx >= variableCount || endidx >= variableCount || startidx > endidx)
{
// Respond with an error indicating that the indices are out of range
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_GET;
mb_frame[2] = MB_DEBUG_ERROR_OUT_OF_BOUNDS;
return;
}
uint16_t lastVarIdx = startidx;
size_t responseSize = 0;
uint8_t *responsePtr = &(mb_frame[11]); // Start of response data
for (uint16_t varidx = startidx; varidx <= endidx; varidx++)
{
size_t varSize = get_var_size(varidx);
if ((responseSize + 11) + varSize <= MAX_MB_FRAME) // Make sure the response fits
{
void *varAddr = get_var_addr(varidx);
// Copy the variable value to the response buffer
memcpy(responsePtr, varAddr, varSize);
// Update response pointer and size
responsePtr += varSize;
responseSize += varSize;
// Update the lastVarIdx
lastVarIdx = varidx;
}
else
{
// Response buffer is full, break the loop
break;
}
}
mb_frame_len = 7 + responseSize; // Update response length
mb_frame[1] = MB_FC_DEBUG_GET;
mb_frame[2] = MB_DEBUG_SUCCESS;
mb_frame[3] = (uint8_t)(lastVarIdx >> 8); // High byte
mb_frame[4] = (uint8_t)(lastVarIdx & 0xFF); // Low byte
mb_frame[5] = (uint8_t)((__tick >> 24) & 0xFF); // Highest byte
mb_frame[6] = (uint8_t)((__tick >> 16) & 0xFF); // Second highest byte
mb_frame[7] = (uint8_t)((__tick >> 8) & 0xFF); // Second lowest byte
mb_frame[8] = (uint8_t)(__tick & 0xFF); // Lowest byte
mb_frame[9] = (uint8_t)(responseSize >> 8); // High byte
mb_frame[10] = (uint8_t)(responseSize & 0xFF); // Low byte
}
/**
* @brief Sends a Modbus response frame for the DEBUG_GET_LIST function code.
*
* This function constructs a Modbus response frame for the DEBUG_GET_LIST function code.
* The response frame includes the trace data for variables specified in the provided index list.
*
* Modbus Response Frame (DEBUG_GET_LIST):
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
* | MB | Resp. | Last | Last | Tick | Tick | Tick | Tick | Resp. | Resp.| Data |
* | FC | Code | Index | Index | | | | | Size | Size | Bytes |
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
* |0x44 | Code | High | Low | High | Mid | Mid | Low | High | Low | Data |
* | | | Byte | Byte | Byte | Byte | Byte | Byte | Byte | Byte | Bytes |
* +-----+-------+-------+-------+-------+-------+-------+-------+-------+------+-------+
*
* @param numIndexes The number of indexes requested.
* @param indexArray Pointer to the array containing variable indexes.
*
* @return void
*/
void debugGetTraceList(uint16_t numIndexes, uint8_t *indexArray)
{
uint16_t response_idx = 11; // Start of response data in the response buffer
uint16_t responseSize = 0;
uint16_t lastVarIdx = 0;
uint16_t variableCount = get_var_count();
uint16_t *varidx_array = NULL;
// Allocate space for all indexes
varidx_array = (uint16_t *)malloc(numIndexes * sizeof(uint16_t));
if (varidx_array == NULL)
{
// Respond with a memory error
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_GET_LIST;
mb_frame[2] = MB_DEBUG_ERROR_OUT_OF_MEMORY;
return;
}
// Copy all indexes to array
for (uint16_t i = 0; i < numIndexes; i++)
{
varidx_array[i] = (uint16_t)indexArray[i * 2] << 8 | indexArray[i * 2 + 1];
}
// Validate if all requested indexes are in range
for (uint16_t i = 0; i < numIndexes; i++)
{
if (varidx_array[i] >= variableCount)
{
// Respond with an error indicating that the index is out of range
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_GET_LIST;
mb_frame[2] = MB_DEBUG_ERROR_OUT_OF_BOUNDS;
free(varidx_array);
return;
}
// Add requested indexes and their traces to the response buffer
size_t varSize = get_var_size(varidx_array[i]);
// Make sure there is enough space in the response buffer
if (response_idx + varSize <= MAX_MB_FRAME)
{
// Add variable data to the response buffer
void *varAddr = get_var_addr(varidx_array[i]);
memcpy(&mb_frame[response_idx], varAddr, varSize);
response_idx += varSize;
responseSize += varSize;
// Update the lastVarIdx
lastVarIdx = varidx_array[i];
}
else
{
// Response buffer is full, break the loop
break;
}
}
// Update response length, lastVarIdx, and response size
mb_frame_len = response_idx;
mb_frame[1] = MB_FC_DEBUG_GET_LIST;
mb_frame[2] = MB_DEBUG_SUCCESS;
mb_frame[3] = (uint8_t)(lastVarIdx >> 8); // High byte
mb_frame[4] = (uint8_t)(lastVarIdx & 0xFF); // Low byte
mb_frame[5] = (uint8_t)((__tick >> 24) & 0xFF); // Highest byte
mb_frame[6] = (uint8_t)((__tick >> 16) & 0xFF); // Second highest byte
mb_frame[7] = (uint8_t)((__tick >> 8) & 0xFF); // Second lowest byte
mb_frame[8] = (uint8_t)(__tick & 0xFF); // Lowest byte
mb_frame[9] = (uint8_t)(responseSize >> 8); // High byte
mb_frame[10] = (uint8_t)(responseSize & 0xFF); // Low byte
free(varidx_array);
}
void debugGetMd5(void *endianness)
{
// Check endianness
uint16_t endian_check = 0;
memcpy(&endian_check, endianness, 2);
if (endian_check == 0xDEAD)
{
set_endianness(SAME_ENDIANNESS);
}
else if (endian_check == 0xADDE)
{
set_endianness(REVERSE_ENDIANNESS);
}
else
{
// Respond with an error indicating that the argument is wrong
mb_frame_len = 3;
mb_frame[1] = MB_FC_DEBUG_GET_MD5;
mb_frame[2] = MB_DEBUG_ERROR_OUT_OF_BOUNDS;
//return;
}
mb_frame[1] = MB_FC_DEBUG_GET_MD5;
mb_frame[2] = MB_DEBUG_SUCCESS;
// Copy MD5 string byte by byte to mb_frame starting from index 3
const char md5[] = PROGRAM_MD5;
int md5_len = 0;
for (md5_len = 0; md5[md5_len] != '\0'; md5_len++)
{
mb_frame[md5_len + 3] = md5[md5_len];
}
// Calculate mb_frame_len (MD5 string length + 3)
mb_frame_len = md5_len + 3;
}
uint16_t calcCrc()
{
uint8_t CRCHi = 0xFF, CRCLo = 0x0FF, Index;
int i = 0;
Index = CRCHi ^ mb_frame[i];
CRCHi = CRCLo ^ _auchCRCHi[Index];
CRCLo = _auchCRCLo[Index];
i++;
while (i < (mb_frame_len - 2))
{
Index = CRCHi ^ mb_frame[i];
i++;
CRCHi = CRCLo ^ _auchCRCHi[Index];
CRCLo = _auchCRCLo[Index];
}
return ((uint16_t)CRCHi << 8) | (uint16_t)CRCLo;
}
Reference in New Issue View Git Blame Copy Permalink