STM32 Modbus Slave

Modbus.cpp

/* 
 * File:   Modbus.cpp
 * Author: mbains
 * 
 * Created on January 8, 2016, 5:21 PM
 */

#include "Modbus.h"

#define T35_TIMEOUT (5) //ms


//static int led_state = 0;
static DigitalOut my_led(PB_3);
static Modbus * g_ptr = NULL;

//convert to little indian word
#define li_word(high, low) (high << 8 | low)

#define high_byte(w16) ((uint8_t)(w16 >> 8))
#define low_byte(w16) ((uint8_t)(w16 & 0xff))



static uint16_t calcCRC(uint8_t * buf, uint8_t u8length)
{
    unsigned int temp, temp2, flag;
    temp = 0xFFFF;
    for (unsigned char i = 0; i < u8length; i++) {
        temp = temp ^ buf[i];
        for (unsigned char j = 1; j <= 8; j++) {
            flag = temp & 0x0001;
            temp >>= 1;
            if (flag)
                temp ^= 0xA001;
        }
    }
    // Reverse byte order. 
    temp2 = temp >> 8;
    temp = (temp << 8) | temp2;
    temp &= 0xFFFF;
    // the returned value is already swapped
    // crcLo byte is first & crcHi byte is last
    return temp;
}

Modbus::Modbus()
{

    Sul_Ring_Init(&m_rx_ring);
    Sul_Ring_Init(&m_tx_ring);
    m_device = NULL;
    m_overflow = false;
    m_tx_primed = false;
    m_rx_primed = false;
    m_rx_curr_size = 0;
    m_tx_curr_size = 0;
    m_crc_err_cnt = 0;

}

int Modbus::init(uint8_t id, Serial * device)
{


    m_id = id;
    m_device = device;
    g_ptr = this;
    m_device->attach(&Rx_interrupt, Serial::RxIrq);
    m_device->attach(&Tx_interrupt, Serial::TxIrq);
    my_led = 0;
    return 0;
};

uint8_t Modbus::poll()
{


    if (m_rx_primed) {
        //must read timeout after rx_primed is set
        //otherwise it's a race condition.
        int the_time = m_timer.read_ms();
        if (the_time > T35_TIMEOUT) {
            uint8_t data = 0;
            m_timer.stop();
            printf("rx: ");
            //printf("Packet timeout %d, pkt size = %d \r\n", the_time, Sul_Ring_Size(&m_rx_ring));
            while ((m_rx_curr_size < MODBUS_BUF_SIZ) && Sul_Ring_Dequeue(&m_rx_ring, &data)) {
                m_rx_buffer[m_rx_curr_size++] = data;
                printf("%d ", data);
            }
            //printf("crc = %d", calcCRC(m_r))
            uint8_t err = validateRequest();
            printf("\r\n func = %d ", m_rx_buffer[MBUS::FUNC]);
            printf("\r\n\r\n");

            if (err == 0) {
                handleRequest();
            } else if (err != MBUS::NO_REPLY) {
                sendException(err);
            } //if no reply, send nothing


            // my_led = 0;
            m_rx_primed = false;
        }
    }
    return 0;
}

void Modbus::resetRxBuffer()
{
    m_timer.reset();
    m_timer.start();
    //my_led = 1;
    m_rx_curr_size = 0;
}

void Modbus::slave_incoming()
{

    if (!m_rx_primed) {
        resetRxBuffer();
        m_rx_primed = true;
    }
    m_timer.reset();
    uint8_t data = 0;
    while (m_device->readable()) {
        data = m_device->getc();
        if (!Sul_Ring_Enqueue(&m_rx_ring, data)) {
            //if buffer full, overflow
            m_overflow = true;
        }
    }
}

void Modbus::slave_outgoing()
{

    uint8_t data = 0;

    //in case, we're not in interrupt context, disable interrupts
    //otherwise, TX empty IQR could fire and re-enter this function
    NVIC_DisableIRQ(USART1_IRQn);
    while (m_device->writeable()) {
        if (!Sul_Ring_Dequeue(&m_tx_ring, &data)) {
            //if empty, leave loop
            my_led = 0;
            m_tx_primed = false;
            break;
        } else {
            m_device->putc(data);
        }
    }
    NVIC_EnableIRQ(USART1_IRQn);
}

Modbus::~Modbus()
{
}

void Modbus::Rx_interrupt()
{

    g_ptr->slave_incoming();
}

/*
 * Fires after putc has been called on device
 */
void Modbus::Tx_interrupt()
{
    g_ptr->slave_outgoing();
}

uint8_t Modbus::validateRequest()
{
    // check message crc vs calculated crc
    uint16_t u16MsgCRC =
        ((m_rx_buffer[m_rx_curr_size - 2] << 8)
        | m_rx_buffer[m_rx_curr_size - 1]); // combine the crc Low & High bytes
    if (calcCRC(m_rx_buffer, (m_rx_curr_size - 2)) != u16MsgCRC) {
        m_crc_err_cnt++;
        return MBUS::NO_REPLY;
    }

    return validateRequest_impl();
}

void Modbus::sendException(uint8_t exception)
{
    uint8_t orig_func = m_rx_buffer[ MBUS::FUNC ]; // get the original FUNC code

    m_tx_buffer[ MBUS::ID ] = m_id;
    m_tx_buffer[ MBUS::FUNC ] = orig_func + 0x80;
    m_tx_buffer[ MBUS::ADD_HI_OR_EXCEP ] = exception;
    m_tx_curr_size = MBUS::EXCEPTION_SIZE;

    sendTxBuffer();
}

uint16_t Modbus::sendTxBuffer()
{
    uint16_t bytes_sent;

    putWordTxBuffer(calcCRC(m_tx_buffer, m_tx_curr_size));
    printf("tx:");
    for (int i = 0; i < m_tx_curr_size; i++) {
        //block until we get this byte on the buffer ring
        while (!Sul_Ring_Enqueue(&m_tx_ring, m_tx_buffer[i]));
        printf(" %d", m_tx_buffer[i]);

    }
    bytes_sent = m_tx_curr_size;
    m_tx_curr_size = 0;
    my_led = 1;
    printf("\r\n");
    m_tx_primed = true;
    slave_outgoing();
    return bytes_sent;
}

uint8_t Modbus::handleRequest()
{
    uint8_t bytes_to_send = 0;
    switch (m_rx_buffer[ MBUS::FUNC ]) {
        case MBUS::MB_FC_READ_COILS:
        case MBUS::MB_FC_READ_DISCRETE_INPUT:
            //return process_FC1();
            break;
        case MBUS::MB_FC_READ_INPUT_REGISTER:
        case MBUS::MB_FC_READ_REGISTERS:
            bytes_to_send = process_FC3_impl();
            break;
        case MBUS::MB_FC_WRITE_COIL:
            //return process_FC5();
            break;
        case MBUS::MB_FC_WRITE_REGISTER:
            //return process_FC6();
            break;
        case MBUS::MB_FC_WRITE_MULTIPLE_COILS:
            //return process_FC15();
            break;
        case MBUS::MB_FC_WRITE_MULTIPLE_REGISTERS:
            //return process_FC16();
            break;
        default:
            break;
    }
    if (bytes_to_send) {
        bytes_to_send = sendTxBuffer();
    }
    printf("I have %d bytes to send\r\n\n", bytes_to_send);


    return bytes_to_send;
}


/*
 * Return the starting register address
 */
uint16_t Modbus::get_addr_start()
{
    return li_word(m_rx_buffer[MBUS::ADD_HI_OR_EXCEP], m_rx_buffer[MBUS::ADD_LO]);
}

/*
 * Return the number of registers requested from the request
 */
uint16_t Modbus::get_number_requested()
{
    return li_word(m_rx_buffer[MBUS::NB_HI], m_rx_buffer[MBUS::NB_LO]);
}

/*
 * Place a byte on the tx butter, increasing the size
 */
void Modbus::putByteTxBuffer(uint8_t data)
{
    m_tx_buffer[m_tx_curr_size] = data;
    m_tx_curr_size++;
}

void Modbus::putWordTxBuffer(uint16_t data)
{
    putByteTxBuffer(high_byte(data));
    putByteTxBuffer(low_byte(data));
}

Modbus.h

namespace MBUS {

    /**
     * @enum MB_FC
     * @brief
     * Modbus function codes summary. 
     * These are the implement function codes either for Master or for Slave.
     *
     * @see also fctsupported
     * @see also modbus_t
     */
    enum MB_FC {
        MB_FC_NONE = 0, /*!< null operator */
        MB_FC_READ_COILS = 1, /*!< FCT=1 -> read coils or digital outputs */
        MB_FC_READ_DISCRETE_INPUT = 2, /*!< FCT=2 -> read digital inputs */
        MB_FC_READ_REGISTERS = 3, /*!< FCT=3 -> read registers or analog outputs */
        MB_FC_READ_INPUT_REGISTER = 4, /*!< FCT=4 -> read analog inputs */
        MB_FC_WRITE_COIL = 5, /*!< FCT=5 -> write single coil or output */
        MB_FC_WRITE_REGISTER = 6, /*!< FCT=6 -> write single register */
        MB_FC_WRITE_MULTIPLE_COILS = 15, /*!< FCT=15 -> write multiple coils or outputs */
        MB_FC_WRITE_MULTIPLE_REGISTERS = 16 /*!< FCT=16 -> write multiple registers */
    };

    enum HANDLE_ERR {
        ALL_OK = 0,
        NO_REPLY = 255,
        EXC_FUNC_CODE = 1,
        EXC_ADDR_RANGE = 2,
        EXC_REGS_QUANT = 3,
        EXC_EXECUTE = 4
    };

    enum {
        RESPONSE_SIZE = 6,
        EXCEPTION_SIZE = 3,
        CHECKSUM_SIZE = 2
    };

    enum MESSAGE {
        ID = 0, //!< ID field
        FUNC, //!< Function code position
        ADD_HI_OR_EXCEP, //!< Address high byte
        ADD_LO, //!< Address low byte
        NB_HI, //!< Number of coils or registers high byte
        NB_LO, //!< Number of coils or registers low byte
        BYTE_CNT //!< byte counter
    };

    enum COM_STATES {
        COM_IDLE = 0,
        COM_WAITING = 1

    };

    enum ERR_LIST {
        ERR_NOT_MASTER = -1,
        ERR_POLLING = -2,
        ERR_BUFF_OVERFLOW = -3,
        ERR_BAD_CRC = -4,
        ERR_EXCEPTION = -5
    };


}

/*
 * Callback for validating the request, returns 0 or ERR_LIST member
 */
typedef MBUS::HANDLE_ERR(*mbus_validatereq_cb_t) (MBUS::MB_FC func_code, uint16_t start, uint16_t count);


/**
 * Handle the request. Place bytes and words on the tx buffer by calling
 * putByteTxbuffer and putWordTxBuffer
 */
typedef void (*mbus_handlereq_cb_t) (MBUS::MB_FC * func_code, uint16_t start, uint16_t count);

class Modbus {
public:
    Modbus();
    int init(uint8_t id, Serial * device);
    uint8_t poll();

    static void Rx_interrupt();
    static void Tx_interrupt();

    virtual ~Modbus();

protected:
    Modbus(const Modbus& orig);
    uint8_t m_id;
    Serial * m_device;
    SulRingBuffer m_tx_ring;
    SulRingBuffer m_rx_ring;

    Timer m_timer;

    bool m_overflow;
    bool m_tx_primed;
    bool m_rx_primed;
    void slave_incoming();
    void slave_outgoing();

    //RX
    void resetRxBuffer();
    uint8_t validateRequest();

    uint16_t get_addr_start();
    uint16_t get_number_requested();
    uint8_t m_rx_buffer[MODBUS_BUF_SIZ];
    uint16_t m_rx_curr_size;
    uint8_t m_crc_err_cnt;


    //TX
    uint8_t handleRequest();
    void sendException(uint8_t exception);
    void putByteTxBuffer(uint8_t data);
    void putWordTxBuffer(uint16_t data);
    uint16_t sendTxBuffer();
    uint8_t m_tx_buffer[MODBUS_BUF_SIZ];
    uint16_t m_tx_curr_size;

    /*
     * Implement part 2 of validateRequest to verify addr range
     */
    virtual uint8_t validateRequest_impl() = 0;
    virtual uint8_t process_FC3_impl() = 0;
    //Protocol Handlers

};