Simple, tiny minimal library to implement a ModBus slave in mbed.
Embed:
(wiki syntax)
Show/hide line numbers
tinymal_modbus.cpp
00001 #include "tinymal_modbus.h" 00002 00003 uint16_t tiny_regs[TINYMOD_NUM_REGS]; 00004 00005 uint8_t tinymod_address = 1; 00006 00007 int tinymod_mode = TINYMOD_MODE_SLAVE; 00008 00009 char tinymod_buf[257]; 00010 char tinymod_resp_buf[257]; 00011 uint32_t tiny_buf_cur = 0; // cursor to receiving buffer (and buffer length, at same time) 00012 00013 // send ringbuffer 00014 uint32_t tiny_sendbuf_cur = 0; // cursor to send buffer start 00015 uint32_t tiny_sendbuf_len = 0; // size of send buffer 00016 00017 bool tinymod_ready_to_process = 0; // Flag indicating when ProcessMessage should run 00018 00019 Ticker tinymod_ticker; 00020 00021 Serial * ser; 00022 DigitalOut * driver; 00023 00024 uint32_t tinymod_cnt = 0; 00025 00026 int _tx_completed = 0; 00027 00028 void tinymod_RX(void) { 00029 uint8_t c; 00030 c = ser->getc(); 00031 if (tiny_buf_cur < 256) { 00032 // Adds this char to buffer 00033 tinymod_buf[ tiny_buf_cur ] = c; 00034 tiny_buf_cur++; 00035 // reset countdown to 9 (900us) 00036 tinymod_cnt = 9; 00037 } 00038 } 00039 void tinymod_Send(void) { 00040 if (tiny_sendbuf_cur < tiny_sendbuf_len) { 00041 driver->write(1); 00042 _tx_completed = 0; 00043 ser->putc(tinymod_resp_buf[tiny_sendbuf_cur]); 00044 tiny_sendbuf_cur++; 00045 } 00046 } 00047 void tinymod_TX(void) { 00048 if (tiny_sendbuf_cur < tiny_sendbuf_len) { 00049 // Send one more 00050 tinymod_Send(); 00051 } 00052 else { 00053 wait(0.000050); 00054 driver->write(0); 00055 _tx_completed = 1; 00056 } 00057 } 00058 00059 00060 // Runs every 100us. Decrements tinymod_cnt. If reaches zero, processes buffer 00061 // Every time a byte is received, tinymod_cnt is set to 9 -- that is, a 900us coundown 00062 // It will only reach zero when there is a 900us silence in RX -- that is, end of packet 00063 void tinymod_tick(void) { 00064 if (tinymod_cnt > 0) { 00065 tinymod_cnt--; 00066 if (tinymod_cnt == 0) tinymod_ready_to_process = 1; 00067 } 00068 } 00069 00070 void tinymod_Init(Serial * ser_obj, DigitalOut * driver_obj) { 00071 int i; 00072 ser = ser_obj; 00073 driver = driver_obj; 00074 00075 driver->write(1); 00076 wait(0.5); 00077 driver->write(0); 00078 00079 ser->attach(tinymod_RX, RawSerial::RxIrq); 00080 ser->attach(tinymod_TX, RawSerial::TxIrq); 00081 00082 tinymod_ticker.attach(&tinymod_tick, 0.0001); 00083 00084 for (i=0; i<TINYMOD_NUM_REGS; i++) { 00085 tiny_regs[i] = 0xFF00 | i; 00086 } 00087 00088 00089 } 00090 00091 void tinymod_Address(uint8_t addr) { 00092 tinymod_address = addr; 00093 } 00094 00095 uint16_t tinymod_chksum(char * buf, int buflen) { 00096 uint16_t crc = 0xFFFF; 00097 int i, j; 00098 for (i=0; i<buflen; i++) { 00099 crc ^= (uint16_t)buf[i]; 00100 00101 for (j=8; j>0; j--) { 00102 if ((crc & 0x0001) != 0) { 00103 crc >>= 1; 00104 crc ^= 0xA001; 00105 } 00106 else crc >>= 1; 00107 } 00108 } 00109 return crc; 00110 } 00111 00112 void tinymod_ReadRegs(uint8_t * buf, uint32_t buflen) { 00113 uint16_t start_addr, reg_count, val, crc; 00114 int i, len; 00115 00116 if (tinymod_mode == TINYMOD_MODE_SLAVE) { 00117 if (buflen < 4) return; 00118 start_addr = ((uint16_t)buf[0] << 8) | buf[1]; 00119 reg_count = ((uint16_t)buf[2] << 8) | buf[3]; 00120 // Do we have what is asked for? 00121 if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 125)) { 00122 tinymod_resp_buf[0] = tinymod_address; 00123 tinymod_resp_buf[1] = MODBUS_FUNC_READ; 00124 tinymod_resp_buf[2] = reg_count*2; 00125 len = 3; 00126 for (i = 0; i<reg_count; i++) { 00127 val = tiny_regs[start_addr + i]; 00128 tinymod_resp_buf[3 + 2*i] = (val >> 8) & 0xFF; 00129 tinymod_resp_buf[4 + 2*i] = (val ) & 0xFF; 00130 len += 2; 00131 } 00132 crc = tinymod_chksum(tinymod_resp_buf, 3 + reg_count*2); 00133 tinymod_resp_buf[3 + reg_count*2] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN 00134 tinymod_resp_buf[4 + reg_count*2] = (crc >> 8) & 0xFF; 00135 len += 2; 00136 00137 tiny_sendbuf_cur = 0; 00138 tiny_sendbuf_len = len; 00139 00140 driver->write(1); 00141 00142 wait(0.0019); 00143 00144 tinymod_Send(); 00145 while (_tx_completed == 0) wait(0.000010); 00146 00147 wait(0.0009); 00148 } 00149 00150 } 00151 } 00152 void tinymod_WriteSingReg(uint8_t * buf, uint32_t buflen) { 00153 // not implemented 00154 } 00155 void tinymod_WriteRegs(uint8_t * buf, uint32_t buflen) { 00156 uint16_t start_addr, reg_count, byte_count, val, crc; 00157 int i; 00158 00159 if (tinymod_mode == TINYMOD_MODE_SLAVE) { 00160 if (buflen < 5) return; 00161 start_addr = ((uint16_t)buf[0] << 8) | buf[1]; 00162 reg_count = ((uint16_t)buf[2] << 8) | buf[3]; 00163 byte_count = ((uint8_t )buf[4]); 00164 if (byte_count != (reg_count*2)) return; 00165 // Do we have what is asked for? 00166 if ((start_addr < TINYMOD_NUM_REGS) && ( (start_addr+reg_count) <= TINYMOD_NUM_REGS ) && (reg_count <= 124)) { 00167 // Write registers 00168 for (i=0; i<reg_count; i++) { 00169 val = ((uint16_t)buf[5 + 2*i] << 8) | (uint8_t)buf[6 + 2*i]; 00170 tiny_regs[start_addr + i] = val; 00171 } 00172 00173 // Response 00174 tinymod_resp_buf[0] = tinymod_address; 00175 tinymod_resp_buf[1] = MODBUS_FUNC_WRITE_MULT; 00176 tinymod_resp_buf[2] = (start_addr >> 8) & 0xFF; 00177 tinymod_resp_buf[3] = (start_addr ) & 0xFF; 00178 tinymod_resp_buf[4] = (reg_count >> 8) & 0xFF; 00179 tinymod_resp_buf[5] = (reg_count ) & 0xFF; 00180 00181 crc = tinymod_chksum(tinymod_resp_buf, 6); 00182 tinymod_resp_buf[6] = (crc ) & 0xFF; // CRC IS LITTLE ENDIAN 00183 tinymod_resp_buf[7] = (crc >> 8) & 0xFF; 00184 00185 tiny_sendbuf_cur = 0; 00186 tiny_sendbuf_len = 8; 00187 00188 driver->write(1); 00189 wait(0.0019); 00190 00191 tinymod_Send(); 00192 while (_tx_completed == 0) wait(0.000010); 00193 00194 wait(0.0009); 00195 } 00196 } 00197 00198 } 00199 00200 void tinymod_ProcessMessage(void) { 00201 int dest_addr; 00202 int func_num; 00203 uint16_t rc_chksum, cl_chksum; 00204 00205 // Ignore if less than 4 chars (wee need at least address, function, checksum) 00206 if (tiny_buf_cur >= 4) { 00207 dest_addr = tinymod_buf[0]; 00208 // if the destination address for this packet is... me? 00209 if (dest_addr == tinymod_address) { 00210 // received checksum: 00211 rc_chksum = 0x0000 | tinymod_buf[ tiny_buf_cur-1 ]; 00212 rc_chksum = (rc_chksum << 8) | tinymod_buf[ tiny_buf_cur-2 ]; 00213 // calculated chksum: 00214 cl_chksum = tinymod_chksum(tinymod_buf, tiny_buf_cur-2 ); 00215 if (rc_chksum == cl_chksum) { 00216 // We have a valid Modbus message! 00217 func_num = tinymod_buf[1]; 00218 if (func_num == MODBUS_FUNC_READ) tinymod_ReadRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; 00219 else if (func_num == MODBUS_FUNC_WRITE_SINGLE) tinymod_WriteSingReg((uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; 00220 else if (func_num == MODBUS_FUNC_WRITE_MULT) tinymod_WriteRegs( (uint8_t *)&tinymod_buf[2], tiny_buf_cur-4) ; 00221 } 00222 00223 } 00224 else { 00225 // not for me, ignore 00226 } 00227 } 00228 00229 tiny_buf_cur = 0; 00230 } 00231 00232 00233 void tinymod_Check(void) { 00234 if (tinymod_ready_to_process) { 00235 tinymod_ProcessMessage(); 00236 00237 tinymod_ready_to_process = 0; 00238 } 00239 }
Generated on Thu Jul 14 2022 10:53:29 by
1.7.2