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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers tinymal_modbus.cpp Source File

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 }