Fernando Cosentino / Tinymal_Modbus

Simple, tiny minimal library to implement a ModBus slave in mbed.

tinymal_modbus.cpp@0:4f5da8a923c4, 2019-02-22 (annotated)

Committer:
fbcosentino
Date:
Fri Feb 22 14:31:13 2019 +0000
Revision:
0:4f5da8a923c4
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbcosentino 0:4f5da8a923c4 1 #include "tinymal_modbus.h"
fbcosentino 0:4f5da8a923c4 2
fbcosentino 0:4f5da8a923c4 3 uint16_t tiny_regs[TINYMOD_NUM_REGS];
fbcosentino 0:4f5da8a923c4 4
fbcosentino 0:4f5da8a923c4 5 uint8_t tinymod_address = 1;
fbcosentino 0:4f5da8a923c4 6
fbcosentino 0:4f5da8a923c4 7 int tinymod_mode = TINYMOD_MODE_SLAVE;
fbcosentino 0:4f5da8a923c4 8
fbcosentino 0:4f5da8a923c4 9 char tinymod_buf[257];
fbcosentino 0:4f5da8a923c4 10 char tinymod_resp_buf[257];
fbcosentino 0:4f5da8a923c4 11 uint32_t tiny_buf_cur = 0; // cursor to receiving buffer (and buffer length, at same time)
fbcosentino 0:4f5da8a923c4 12
fbcosentino 0:4f5da8a923c4 13 // send ringbuffer
fbcosentino 0:4f5da8a923c4 14 uint32_t tiny_sendbuf_cur = 0; // cursor to send buffer start
fbcosentino 0:4f5da8a923c4 15 uint32_t tiny_sendbuf_len = 0; // size of send buffer
fbcosentino 0:4f5da8a923c4 16
fbcosentino 0:4f5da8a923c4 17 bool tinymod_ready_to_process = 0; // Flag indicating when ProcessMessage should run
fbcosentino 0:4f5da8a923c4 18
fbcosentino 0:4f5da8a923c4 19 Ticker tinymod_ticker;
fbcosentino 0:4f5da8a923c4 20
fbcosentino 0:4f5da8a923c4 21 Serial * ser;
fbcosentino 0:4f5da8a923c4 22 DigitalOut * driver;
fbcosentino 0:4f5da8a923c4 23
fbcosentino 0:4f5da8a923c4 24 uint32_t tinymod_cnt = 0;
fbcosentino 0:4f5da8a923c4 25
fbcosentino 0:4f5da8a923c4 26 int _tx_completed = 0;
fbcosentino 0:4f5da8a923c4 27
fbcosentino 0:4f5da8a923c4 28 void tinymod_RX(void) {
fbcosentino 0:4f5da8a923c4 29 uint8_t c;
fbcosentino 0:4f5da8a923c4 30 c = ser->getc();
fbcosentino 0:4f5da8a923c4 31 if (tiny_buf_cur < 256) {
fbcosentino 0:4f5da8a923c4 32 // Adds this char to buffer
fbcosentino 0:4f5da8a923c4 33 tinymod_buf[ tiny_buf_cur ] = c;
fbcosentino 0:4f5da8a923c4 34 tiny_buf_cur++;
fbcosentino 0:4f5da8a923c4 35 // reset countdown to 9 (900us)
fbcosentino 0:4f5da8a923c4 36 tinymod_cnt = 9;
fbcosentino 0:4f5da8a923c4 37 }
fbcosentino 0:4f5da8a923c4 38 }
fbcosentino 0:4f5da8a923c4 39 void tinymod_Send(void) {
fbcosentino 0:4f5da8a923c4 40 if (tiny_sendbuf_cur < tiny_sendbuf_len) {
fbcosentino 0:4f5da8a923c4 41 driver->write(1);
fbcosentino 0:4f5da8a923c4 42 _tx_completed = 0;
fbcosentino 0:4f5da8a923c4 43 ser->putc(tinymod_resp_buf[tiny_sendbuf_cur]);
fbcosentino 0:4f5da8a923c4 44 tiny_sendbuf_cur++;
fbcosentino 0:4f5da8a923c4 45 }
fbcosentino 0:4f5da8a923c4 46 }
fbcosentino 0:4f5da8a923c4 47 void tinymod_TX(void) {
fbcosentino 0:4f5da8a923c4 48 if (tiny_sendbuf_cur < tiny_sendbuf_len) {
fbcosentino 0:4f5da8a923c4 49 // Send one more
fbcosentino 0:4f5da8a923c4 50 tinymod_Send();
fbcosentino 0:4f5da8a923c4 51 }
fbcosentino 0:4f5da8a923c4 52 else {
fbcosentino 0:4f5da8a923c4 53 wait(0.000050);
fbcosentino 0:4f5da8a923c4 54 driver->write(0);
fbcosentino 0:4f5da8a923c4 55 _tx_completed = 1;
fbcosentino 0:4f5da8a923c4 56 }
fbcosentino 0:4f5da8a923c4 57 }
fbcosentino 0:4f5da8a923c4 58
fbcosentino 0:4f5da8a923c4 59
fbcosentino 0:4f5da8a923c4 60 // Runs every 100us. Decrements tinymod_cnt. If reaches zero, processes buffer
fbcosentino 0:4f5da8a923c4 61 // Every time a byte is received, tinymod_cnt is set to 9 -- that is, a 900us coundown
fbcosentino 0:4f5da8a923c4 62 // It will only reach zero when there is a 900us silence in RX -- that is, end of packet
fbcosentino 0:4f5da8a923c4 63 void tinymod_tick(void) {
fbcosentino 0:4f5da8a923c4 64 if (tinymod_cnt > 0) {
fbcosentino 0:4f5da8a923c4 65 tinymod_cnt--;
fbcosentino 0:4f5da8a923c4 66 if (tinymod_cnt == 0) tinymod_ready_to_process = 1;
fbcosentino 0:4f5da8a923c4 67 }
fbcosentino 0:4f5da8a923c4 68 }
fbcosentino 0:4f5da8a923c4 69
fbcosentino 0:4f5da8a923c4 70 void tinymod_Init(Serial * ser_obj, DigitalOut * driver_obj) {
fbcosentino 0:4f5da8a923c4 71 int i;
fbcosentino 0:4f5da8a923c4 72 ser = ser_obj;
fbcosentino 0:4f5da8a923c4 73 driver = driver_obj;
fbcosentino 0:4f5da8a923c4 74
fbcosentino 0:4f5da8a923c4 75 driver->write(1);
fbcosentino 0:4f5da8a923c4 76 wait(0.5);
fbcosentino 0:4f5da8a923c4 77 driver->write(0);
fbcosentino 0:4f5da8a923c4 78
fbcosentino 0:4f5da8a923c4 79 ser->attach(tinymod_RX, RawSerial::RxIrq);
fbcosentino 0:4f5da8a923c4 80 ser->attach(tinymod_TX, RawSerial::TxIrq);
fbcosentino 0:4f5da8a923c4 81
fbcosentino 0:4f5da8a923c4 82 tinymod_ticker.attach(&tinymod_tick, 0.0001);
fbcosentino 0:4f5da8a923c4 83
fbcosentino 0:4f5da8a923c4 84 for (i=0; i<TINYMOD_NUM_REGS; i++) {
fbcosentino 0:4f5da8a923c4 85 tiny_regs[i] = 0xFF00 | i;
fbcosentino 0:4f5da8a923c4 86 }
fbcosentino 0:4f5da8a923c4 87
fbcosentino 0:4f5da8a923c4 88
fbcosentino 0:4f5da8a923c4 89 }
fbcosentino 0:4f5da8a923c4 90
fbcosentino 0:4f5da8a923c4 91 void tinymod_Address(uint8_t addr) {
fbcosentino 0:4f5da8a923c4 92 tinymod_address = addr;
fbcosentino 0:4f5da8a923c4 93 }
fbcosentino 0:4f5da8a923c4 94
fbcosentino 0:4f5da8a923c4 95 uint16_t tinymod_chksum(char * buf, int buflen) {
fbcosentino 0:4f5da8a923c4 96 uint16_t crc = 0xFFFF;
fbcosentino 0:4f5da8a923c4 97 int i, j;
fbcosentino 0:4f5da8a923c4 98 for (i=0; i<buflen; i++) {
fbcosentino 0:4f5da8a923c4 99 crc ^= (uint16_t)buf[i];
fbcosentino 0:4f5da8a923c4 100
fbcosentino 0:4f5da8a923c4 101 for (j=8; j>0; j--) {
fbcosentino 0:4f5da8a923c4 102 if ((crc & 0x0001) != 0) {
fbcosentino 0:4f5da8a923c4 103 crc >>= 1;
fbcosentino 0:4f5da8a923c4 104 crc ^= 0xA001;
fbcosentino 0:4f5da8a923c4 105 }
fbcosentino 0:4f5da8a923c4 106 else crc >>= 1;
fbcosentino 0:4f5da8a923c4 107 }
fbcosentino 0:4f5da8a923c4 108 }
fbcosentino 0:4f5da8a923c4 109 return crc;
fbcosentino 0:4f5da8a923c4 110 }
fbcosentino 0:4f5da8a923c4 111
fbcosentino 0:4f5da8a923c4 112 void tinymod_ReadRegs(uint8_t * buf, uint32_t buflen) {
fbcosentino 0:4f5da8a923c4 113 uint16_t start_addr, reg_count, val, crc;
fbcosentino 0:4f5da8a923c4 114 int i, len;
fbcosentino 0:4f5da8a923c4 115
fbcosentino 0:4f5da8a923c4 116 if (tinymod_mode == TINYMOD_MODE_SLAVE) {
fbcosentino 0:4f5da8a923c4 117 if (buflen < 4) return;
fbcosentino 0:4f5da8a923c4 118 start_addr = ((uint16_t)buf[0] << 8) | buf[1];
fbcosentino 0:4f5da8a923c4 119 reg_count = ((uint16_t)buf[2] << 8) | buf[3];
fbcosentino 0:4f5da8a923c4 120 // Do we have what is asked for?
fbcosentino 0:4f5da8a923c4 121 if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 125)) {
fbcosentino 0:4f5da8a923c4 122 tinymod_resp_buf[0] = tinymod_address;
fbcosentino 0:4f5da8a923c4 123 tinymod_resp_buf[1] = MODBUS_FUNC_READ;
fbcosentino 0:4f5da8a923c4 124 tinymod_resp_buf[2] = reg_count*2;
fbcosentino 0:4f5da8a923c4 125 len = 3;
fbcosentino 0:4f5da8a923c4 126 for (i = 0; i<reg_count; i++) {
fbcosentino 0:4f5da8a923c4 127 val = tiny_regs[start_addr + i];
fbcosentino 0:4f5da8a923c4 128 tinymod_resp_buf[3 + 2*i] = (val >> 8) & 0xFF;
fbcosentino 0:4f5da8a923c4 129 tinymod_resp_buf[4 + 2*i] = (val ) & 0xFF;
fbcosentino 0:4f5da8a923c4 130 len += 2;
fbcosentino 0:4f5da8a923c4 131 }
fbcosentino 0:4f5da8a923c4 132 crc = tinymod_chksum(tinymod_resp_buf, 3 + reg_count*2);
fbcosentino 0:4f5da8a923c4 133 tinymod_resp_buf[3 + reg_count*2] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN
fbcosentino 0:4f5da8a923c4 134 tinymod_resp_buf[4 + reg_count*2] = (crc >> 8) & 0xFF;
fbcosentino 0:4f5da8a923c4 135 len += 2;
fbcosentino 0:4f5da8a923c4 136
fbcosentino 0:4f5da8a923c4 137 tiny_sendbuf_cur = 0;
fbcosentino 0:4f5da8a923c4 138 tiny_sendbuf_len = len;
fbcosentino 0:4f5da8a923c4 139
fbcosentino 0:4f5da8a923c4 140 driver->write(1);
fbcosentino 0:4f5da8a923c4 141
fbcosentino 0:4f5da8a923c4 142 wait(0.0019);
fbcosentino 0:4f5da8a923c4 143
fbcosentino 0:4f5da8a923c4 144 tinymod_Send();
fbcosentino 0:4f5da8a923c4 145 while (_tx_completed == 0) wait(0.000010);
fbcosentino 0:4f5da8a923c4 146
fbcosentino 0:4f5da8a923c4 147 wait(0.0009);
fbcosentino 0:4f5da8a923c4 148 }
fbcosentino 0:4f5da8a923c4 149
fbcosentino 0:4f5da8a923c4 150 }
fbcosentino 0:4f5da8a923c4 151 }
fbcosentino 0:4f5da8a923c4 152 void tinymod_WriteSingReg(uint8_t * buf, uint32_t buflen) {
fbcosentino 0:4f5da8a923c4 153 // not implemented
fbcosentino 0:4f5da8a923c4 154 }
fbcosentino 0:4f5da8a923c4 155 void tinymod_WriteRegs(uint8_t * buf, uint32_t buflen) {
fbcosentino 0:4f5da8a923c4 156 uint16_t start_addr, reg_count, byte_count, val, crc;
fbcosentino 0:4f5da8a923c4 157 int i;
fbcosentino 0:4f5da8a923c4 158
fbcosentino 0:4f5da8a923c4 159 if (tinymod_mode == TINYMOD_MODE_SLAVE) {
fbcosentino 0:4f5da8a923c4 160 if (buflen < 5) return;
fbcosentino 0:4f5da8a923c4 161 start_addr = ((uint16_t)buf[0] << 8) | buf[1];
fbcosentino 0:4f5da8a923c4 162 reg_count = ((uint16_t)buf[2] << 8) | buf[3];
fbcosentino 0:4f5da8a923c4 163 byte_count = ((uint8_t )buf[4]);
fbcosentino 0:4f5da8a923c4 164 if (byte_count != (reg_count*2)) return;
fbcosentino 0:4f5da8a923c4 165 // Do we have what is asked for?
fbcosentino 0:4f5da8a923c4 166 if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 124)) {
fbcosentino 0:4f5da8a923c4 167 // Write registers
fbcosentino 0:4f5da8a923c4 168 for (i=0; i<reg_count; i++) {
fbcosentino 0:4f5da8a923c4 169 val = ((uint16_t)buf[5 + 2*i] << 8) | (uint8_t)buf[6 + 2*i];
fbcosentino 0:4f5da8a923c4 170 tiny_regs[start_addr + i] = val;
fbcosentino 0:4f5da8a923c4 171 }
fbcosentino 0:4f5da8a923c4 172
fbcosentino 0:4f5da8a923c4 173 // Response
fbcosentino 0:4f5da8a923c4 174 tinymod_resp_buf[0] = tinymod_address;
fbcosentino 0:4f5da8a923c4 175 tinymod_resp_buf[1] = MODBUS_FUNC_WRITE_MULT;
fbcosentino 0:4f5da8a923c4 176 tinymod_resp_buf[2] = (start_addr >> 8) & 0xFF;
fbcosentino 0:4f5da8a923c4 177 tinymod_resp_buf[3] = (start_addr ) & 0xFF;
fbcosentino 0:4f5da8a923c4 178 tinymod_resp_buf[4] = (reg_count >> 8) & 0xFF;
fbcosentino 0:4f5da8a923c4 179 tinymod_resp_buf[5] = (reg_count ) & 0xFF;
fbcosentino 0:4f5da8a923c4 180
fbcosentino 0:4f5da8a923c4 181 crc = tinymod_chksum(tinymod_resp_buf, 6);
fbcosentino 0:4f5da8a923c4 182 tinymod_resp_buf[6] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN
fbcosentino 0:4f5da8a923c4 183 tinymod_resp_buf[7] = (crc >> 8) & 0xFF;
fbcosentino 0:4f5da8a923c4 184
fbcosentino 0:4f5da8a923c4 185 tiny_sendbuf_cur = 0;
fbcosentino 0:4f5da8a923c4 186 tiny_sendbuf_len = 8;
fbcosentino 0:4f5da8a923c4 187
fbcosentino 0:4f5da8a923c4 188 driver->write(1);
fbcosentino 0:4f5da8a923c4 189 wait(0.0019);
fbcosentino 0:4f5da8a923c4 190
fbcosentino 0:4f5da8a923c4 191 tinymod_Send();
fbcosentino 0:4f5da8a923c4 192 while (_tx_completed == 0) wait(0.000010);
fbcosentino 0:4f5da8a923c4 193
fbcosentino 0:4f5da8a923c4 194 wait(0.0009);
fbcosentino 0:4f5da8a923c4 195 }
fbcosentino 0:4f5da8a923c4 196 }
fbcosentino 0:4f5da8a923c4 197
fbcosentino 0:4f5da8a923c4 198 }
fbcosentino 0:4f5da8a923c4 199
fbcosentino 0:4f5da8a923c4 200 void tinymod_ProcessMessage(void) {
fbcosentino 0:4f5da8a923c4 201 int dest_addr;
fbcosentino 0:4f5da8a923c4 202 int func_num;
fbcosentino 0:4f5da8a923c4 203 uint16_t rc_chksum, cl_chksum;
fbcosentino 0:4f5da8a923c4 204
fbcosentino 0:4f5da8a923c4 205 // Ignore if less than 4 chars (wee need at least address, function, checksum)
fbcosentino 0:4f5da8a923c4 206 if (tiny_buf_cur >= 4) {
fbcosentino 0:4f5da8a923c4 207 dest_addr = tinymod_buf[0];
fbcosentino 0:4f5da8a923c4 208 // if the destination address for this packet is... me?
fbcosentino 0:4f5da8a923c4 209 if (dest_addr == tinymod_address) {
fbcosentino 0:4f5da8a923c4 210 // received checksum:
fbcosentino 0:4f5da8a923c4 211 rc_chksum = 0x0000 | tinymod_buf[ tiny_buf_cur-1 ];
fbcosentino 0:4f5da8a923c4 212 rc_chksum = (rc_chksum << 8) | tinymod_buf[ tiny_buf_cur-2 ];
fbcosentino 0:4f5da8a923c4 213 // calculated chksum:
fbcosentino 0:4f5da8a923c4 214 cl_chksum = tinymod_chksum(tinymod_buf, tiny_buf_cur-2 );
fbcosentino 0:4f5da8a923c4 215 if (rc_chksum == cl_chksum) {
fbcosentino 0:4f5da8a923c4 216 // We have a valid Modbus message!
fbcosentino 0:4f5da8a923c4 217 func_num = tinymod_buf[1];
fbcosentino 0:4f5da8a923c4 218 if (func_num == MODBUS_FUNC_READ) tinymod_ReadRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ;
fbcosentino 0:4f5da8a923c4 219 else if (func_num == MODBUS_FUNC_WRITE_SINGLE) tinymod_WriteSingReg((uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ;
fbcosentino 0:4f5da8a923c4 220 else if (func_num == MODBUS_FUNC_WRITE_MULT) tinymod_WriteRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ;
fbcosentino 0:4f5da8a923c4 221 }
fbcosentino 0:4f5da8a923c4 222
fbcosentino 0:4f5da8a923c4 223 }
fbcosentino 0:4f5da8a923c4 224 else {
fbcosentino 0:4f5da8a923c4 225 // not for me, ignore
fbcosentino 0:4f5da8a923c4 226 }
fbcosentino 0:4f5da8a923c4 227 }
fbcosentino 0:4f5da8a923c4 228
fbcosentino 0:4f5da8a923c4 229 tiny_buf_cur = 0;
fbcosentino 0:4f5da8a923c4 230 }
fbcosentino 0:4f5da8a923c4 231
fbcosentino 0:4f5da8a923c4 232
fbcosentino 0:4f5da8a923c4 233 void tinymod_Check(void) {
fbcosentino 0:4f5da8a923c4 234 if (tinymod_ready_to_process) {
fbcosentino 0:4f5da8a923c4 235 tinymod_ProcessMessage();
fbcosentino 0:4f5da8a923c4 236
fbcosentino 0:4f5da8a923c4 237 tinymod_ready_to_process = 0;
fbcosentino 0:4f5da8a923c4 238 }
fbcosentino 0:4f5da8a923c4 239 }