Simple, tiny minimal library to implement a ModBus slave in mbed.
tinymal_modbus.cpp
- Committer:
- fbcosentino
- Date:
- 2019-02-22
- Revision:
- 0:4f5da8a923c4
File content as of revision 0:4f5da8a923c4:
#include "tinymal_modbus.h" uint16_t tiny_regs[TINYMOD_NUM_REGS]; uint8_t tinymod_address = 1; int tinymod_mode = TINYMOD_MODE_SLAVE; char tinymod_buf[257]; char tinymod_resp_buf[257]; uint32_t tiny_buf_cur = 0; // cursor to receiving buffer (and buffer length, at same time) // send ringbuffer uint32_t tiny_sendbuf_cur = 0; // cursor to send buffer start uint32_t tiny_sendbuf_len = 0; // size of send buffer bool tinymod_ready_to_process = 0; // Flag indicating when ProcessMessage should run Ticker tinymod_ticker; Serial * ser; DigitalOut * driver; uint32_t tinymod_cnt = 0; int _tx_completed = 0; void tinymod_RX(void) { uint8_t c; c = ser->getc(); if (tiny_buf_cur < 256) { // Adds this char to buffer tinymod_buf[ tiny_buf_cur ] = c; tiny_buf_cur++; // reset countdown to 9 (900us) tinymod_cnt = 9; } } void tinymod_Send(void) { if (tiny_sendbuf_cur < tiny_sendbuf_len) { driver->write(1); _tx_completed = 0; ser->putc(tinymod_resp_buf[tiny_sendbuf_cur]); tiny_sendbuf_cur++; } } void tinymod_TX(void) { if (tiny_sendbuf_cur < tiny_sendbuf_len) { // Send one more tinymod_Send(); } else { wait(0.000050); driver->write(0); _tx_completed = 1; } } // Runs every 100us. Decrements tinymod_cnt. If reaches zero, processes buffer // Every time a byte is received, tinymod_cnt is set to 9 -- that is, a 900us coundown // It will only reach zero when there is a 900us silence in RX -- that is, end of packet void tinymod_tick(void) { if (tinymod_cnt > 0) { tinymod_cnt--; if (tinymod_cnt == 0) tinymod_ready_to_process = 1; } } void tinymod_Init(Serial * ser_obj, DigitalOut * driver_obj) { int i; ser = ser_obj; driver = driver_obj; driver->write(1); wait(0.5); driver->write(0); ser->attach(tinymod_RX, RawSerial::RxIrq); ser->attach(tinymod_TX, RawSerial::TxIrq); tinymod_ticker.attach(&tinymod_tick, 0.0001); for (i=0; i<TINYMOD_NUM_REGS; i++) { tiny_regs[i] = 0xFF00 | i; } } void tinymod_Address(uint8_t addr) { tinymod_address = addr; } uint16_t tinymod_chksum(char * buf, int buflen) { uint16_t crc = 0xFFFF; int i, j; for (i=0; i<buflen; i++) { crc ^= (uint16_t)buf[i]; for (j=8; j>0; j--) { if ((crc & 0x0001) != 0) { crc >>= 1; crc ^= 0xA001; } else crc >>= 1; } } return crc; } void tinymod_ReadRegs(uint8_t * buf, uint32_t buflen) { uint16_t start_addr, reg_count, val, crc; int i, len; if (tinymod_mode == TINYMOD_MODE_SLAVE) { if (buflen < 4) return; start_addr = ((uint16_t)buf[0] << 8) | buf[1]; reg_count = ((uint16_t)buf[2] << 8) | buf[3]; // Do we have what is asked for? if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 125)) { tinymod_resp_buf[0] = tinymod_address; tinymod_resp_buf[1] = MODBUS_FUNC_READ; tinymod_resp_buf[2] = reg_count*2; len = 3; for (i = 0; i<reg_count; i++) { val = tiny_regs[start_addr + i]; tinymod_resp_buf[3 + 2*i] = (val >> 8) & 0xFF; tinymod_resp_buf[4 + 2*i] = (val ) & 0xFF; len += 2; } crc = tinymod_chksum(tinymod_resp_buf, 3 + reg_count*2); tinymod_resp_buf[3 + reg_count*2] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN tinymod_resp_buf[4 + reg_count*2] = (crc >> 8) & 0xFF; len += 2; tiny_sendbuf_cur = 0; tiny_sendbuf_len = len; driver->write(1); wait(0.0019); tinymod_Send(); while (_tx_completed == 0) wait(0.000010); wait(0.0009); } } } void tinymod_WriteSingReg(uint8_t * buf, uint32_t buflen) { // not implemented } void tinymod_WriteRegs(uint8_t * buf, uint32_t buflen) { uint16_t start_addr, reg_count, byte_count, val, crc; int i; if (tinymod_mode == TINYMOD_MODE_SLAVE) { if (buflen < 5) return; start_addr = ((uint16_t)buf[0] << 8) | buf[1]; reg_count = ((uint16_t)buf[2] << 8) | buf[3]; byte_count = ((uint8_t )buf[4]); if (byte_count != (reg_count*2)) return; // Do we have what is asked for? if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 124)) { // Write registers for (i=0; i<reg_count; i++) { val = ((uint16_t)buf[5 + 2*i] << 8) | (uint8_t)buf[6 + 2*i]; tiny_regs[start_addr + i] = val; } // Response tinymod_resp_buf[0] = tinymod_address; tinymod_resp_buf[1] = MODBUS_FUNC_WRITE_MULT; tinymod_resp_buf[2] = (start_addr >> 8) & 0xFF; tinymod_resp_buf[3] = (start_addr ) & 0xFF; tinymod_resp_buf[4] = (reg_count >> 8) & 0xFF; tinymod_resp_buf[5] = (reg_count ) & 0xFF; crc = tinymod_chksum(tinymod_resp_buf, 6); tinymod_resp_buf[6] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN tinymod_resp_buf[7] = (crc >> 8) & 0xFF; tiny_sendbuf_cur = 0; tiny_sendbuf_len = 8; driver->write(1); wait(0.0019); tinymod_Send(); while (_tx_completed == 0) wait(0.000010); wait(0.0009); } } } void tinymod_ProcessMessage(void) { int dest_addr; int func_num; uint16_t rc_chksum, cl_chksum; // Ignore if less than 4 chars (wee need at least address, function, checksum) if (tiny_buf_cur >= 4) { dest_addr = tinymod_buf[0]; // if the destination address for this packet is... me? if (dest_addr == tinymod_address) { // received checksum: rc_chksum = 0x0000 | tinymod_buf[ tiny_buf_cur-1 ]; rc_chksum = (rc_chksum << 8) | tinymod_buf[ tiny_buf_cur-2 ]; // calculated chksum: cl_chksum = tinymod_chksum(tinymod_buf, tiny_buf_cur-2 ); if (rc_chksum == cl_chksum) { // We have a valid Modbus message! func_num = tinymod_buf[1]; if (func_num == MODBUS_FUNC_READ) tinymod_ReadRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; else if (func_num == MODBUS_FUNC_WRITE_SINGLE) tinymod_WriteSingReg((uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; else if (func_num == MODBUS_FUNC_WRITE_MULT) tinymod_WriteRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; } } else { // not for me, ignore } } tiny_buf_cur = 0; } void tinymod_Check(void) { if (tinymod_ready_to_process) { tinymod_ProcessMessage(); tinymod_ready_to_process = 0; } }