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;
    }
}