#include "modbus.h"

//Initialize modbus variables. Must be called before using modbus library.
void mb_init(void (*(*mb_cb_funcs))(Mb_rx_struct*, Mb_tx_struct*), void (*mb_cb_tx)(uint8_t), uint8_t mode, uint8_t address)
{
    #ifdef MB_DYNAMIC
        //Dynamic memory assignment.  Not recommended for small processors.
        mb_tx_struct = malloc(sizeof(Mb_tx_struct));        //Modbus tx struct.
        mb_rx_struct = malloc(sizeof(Mb_rx_struct));        //Modbus rx struct.
        mb_diagnostics = malloc(sizeof(Mb_diagnostics));    //Modbus diagnostics struct.
    #else
        //Static memory assignment.
        static Mb_tx_struct mb_trans_struct;
        static Mb_rx_struct mb_rec_struct;
        static Mb_diagnostics mb_diag_struct;
        
        mb_tx_struct = &mb_trans_struct;
        mb_rx_struct = &mb_rec_struct;
        mb_diagnostics = &mb_diag_struct;
    #endif
    
    mb_callback_funcs = mb_cb_funcs;                    //Get pointer to callback functions.
    mb_callback_tx    = mb_cb_tx;                       //Get pointer to callback transmit function.
    
    mb_clear_event_log();                               //Initialize event log.
    mb_ascii_delimiter = 0x0A;                          //Set default ASCII frame delimiter.
    
    //Determine if in RTU or ASCII mode.
    if(mode == MB_ASCII)
    {
        mb_mode = MB_ASCII;
        mb_max_frame_size = MB_MAX_RX_ASCII_FRAME;      //513 byte frame limit.
    }
    else
    {
        mb_mode = MB_RTU;
        mb_max_frame_size = MB_MAX_RX_RTU_FRAME;        //256 byte frame limit.
    }
        
    mb_this_address = address;                          //Set slave address of device.
}

//Clear dynamic memory.
void mb_close()
{
    if(mb_tx_struct)
        free(mb_tx_struct);
    
    if(mb_rx_struct)
        free(mb_rx_struct);
        
    if(mb_diagnostics)
        free(mb_diagnostics);
}

//Fill modbus rx frame buffer.
void mb_rx_byte(uint8_t rx_byte, uint8_t parity_error)
{
    //Save byte only if there is room.
    if(mb_rx_struct->mb_rx_index < mb_max_frame_size)
    {
        mb_rx_struct->mb_rx_frame[mb_rx_struct->mb_rx_index++] = rx_byte;
    }
    
    //Set overrun flag, if necessary.
    else
    {
        mb_rx_struct->mb_rx_char_overrun = MB_ERROR;
    }
    
    //Check for parity error.
    if(parity_error)
        mb_rx_struct->mb_rx_parity_error = MB_ERROR;
}

void mb_tx_frame()
{
    uint16_t tx_string_index = 0;
    
    //Transmit frame in RTU format.
    if(mb_mode == MB_RTU)
    {
        //Use callback function to transmit individual bytes.
        while(tx_string_index < mb_tx_struct->mb_tx_index)
            (*mb_callback_tx)(mb_tx_struct->mb_tx_frame[tx_string_index++]);
    }
    
    //Transmit frame in ASCII format.
    else
    {
        
    }
    
    mb_rx_tx_reset();   //Reset indexes for next frame.
}

//The function returns the CRC by means of calculation.
uint16_t mb_crc_16(uint8_t* data_frame, uint16_t data_len)
{
    const uint16_t POLY = 0xA001;   //Calculation polynomial.
    uint16_t CRC = 0xFFFF;          //Set initial CRC register contents.
    uint8_t n;                      //Bit shifting counter.

    while(data_len--)               //Do all bytes.
    {
        CRC ^= *data_frame++;
        n = 0;

        while(n++ < 8)              //Shift all bits.
        {
            if(CRC & 0x0001)        //If LSB is set, XOR with CRC polynomial.
            CRC = (CRC >> 1) ^ POLY;
            else
            CRC >>= 1;
        }
    }
    
    return CRC;
}

//Return 16-bit starting address from rx frame.
uint16_t mb_rtu_get_address()
{
    uint16_t address;
    
    address = mb_rx_struct->mb_rx_frame[MB_RTU_ADDR_HI] << 8;
    address |= mb_rx_struct->mb_rx_frame[MB_RTU_ADDR_LO];
    
    return address;
}

//Return 16-bit quantity of inputs/outputs from rx frame.
uint16_t mb_rtu_get_quantity()
{
    uint16_t quantity;
    
    quantity = mb_rx_struct->mb_rx_frame[MB_RTU_QTY_HI] << 8;
    quantity |= mb_rx_struct->mb_rx_frame[MB_RTU_QTY_LO];
    
    return quantity;
}

//Create tx frame for exception.
void mb_rtu_exception(uint8_t mb_func, uint8_t error_code)
{
    uint16_t crc;
    
    //Assemble first part of frame.
    mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = mb_this_address;
    mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = mb_func | 0x80;
    mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = error_code;
    
    //Calculate CRC.
    crc = mb_crc_16(mb_tx_struct->mb_tx_frame, mb_tx_struct->mb_tx_index);
    
    //Add CRC to end of frame.
    mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc;
    mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc >> 8;
}

//Resets variables in rx and tx structs for next frame.
void mb_rx_tx_reset()
{
    //Reset indexes for next frame.
    mb_rx_struct->mb_rx_index = 0;
    mb_tx_struct->mb_tx_index = 0;
    mb_rx_struct->mb_rx_char_overrun = MB_NO_ERROR;
    mb_rx_struct->mb_rx_parity_error = MB_NO_ERROR;
    mb_tx_struct->mb_tx_exception = 0;
}

//This function adds an event to the modbus event log.
void mb_update_event_log(uint8_t event_type)
{
    //Add event to log.
    mb_diagnostics->mb_event_log[mb_diagnostics->mb_log_head++] = event_type;

    //If head pointer at end of log, wrap around to the beginning.
    if(mb_diagnostics->mb_log_head == MB_MAX_EVENT_LOG + 1)
    {
        mb_diagnostics->mb_log_head = 0;
    }

    //If head pointer == tail pointer, move tail pointer forward.
    if(mb_diagnostics->mb_log_head == mb_diagnostics->mb_log_tail)
    {
        mb_diagnostics->mb_log_tail++;
    }

    //If tail pointer at end of log, wrap around to the beginning.
    if(mb_diagnostics->mb_log_tail == MB_MAX_EVENT_LOG + 1)
    {
        mb_diagnostics->mb_log_tail = 0;
    }
}

//Clears the event log and places a com reset in the log.
void mb_clear_event_log()
{
    //Set head and tail to initial positions.
    mb_diagnostics->mb_log_head = 1;
    mb_diagnostics->mb_log_tail = 0;

    //Put Reset event in log.
    mb_diagnostics->mb_event_log[0] = MB_COMM_RESET;
}

//Copies the event log to the tx buffer.
void mb_get_event_log()
{
    //Get current location of most recent event.
    uint8_t log_pointer = mb_diagnostics->mb_log_head;

    //Loop while log pointer != last event in log.
    while(log_pointer != mb_diagnostics->mb_log_tail)
    {
        //If log pointer at end of log, wrap around to the beginning.
        if(log_pointer == 0)
        {
            log_pointer = MB_MAX_EVENT_LOG;
        }

        else
        {
            log_pointer--;
        }

        //Write event to tx buffer.
        mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = mb_diagnostics->mb_event_log[log_pointer];
    }
}

//Parse the modbus rx array into its sub-components(RTU only).
uint8_t mb_rtu_rx_parse()
{
    uint16_t crc;
    
    /*************************************************************Message Pre-processing************************************************************/
    if(mb_rx_struct->mb_rx_char_overrun)                                            //Check for frame overrun.
    {
        mb_diagnostics->mb_char_overrun_count++;                                    //Increment frame overrun counter.
        mb_update_event_log(MB_RX_CHAR_OVERRUN);                                    //Update event log.
        mb_rx_tx_reset();                                                           //Reset indexes for next frame.
        return MB_ERR_OVERRUN;
    }
    
    if(mb_rx_struct->mb_rx_parity_error)                                            //Check for parity errors.
    {
        mb_diagnostics->mb_bus_com_err_count++;                                     //Increment comm error counter.
        mb_update_event_log(MB_RX_COM_ERROR);                                       //Update event log.
        mb_rx_tx_reset();                                                           //Reset indexes for next frame.
        return MB_ERR_PARITY;
    }
    
    if(mb_rx_struct->mb_rx_index < 3)                                               //If frame is less than 3 characters, exit with error.
    {
        mb_diagnostics->mb_bus_com_err_count++;                                     //Increment comm error counter.
        mb_update_event_log(MB_RX_COM_ERROR);                                       //Update event log.
        mb_rx_tx_reset();                                                           //Reset indexes for next frame.
        return MB_ERR_SMALL_FRAME;
    }
    
    mb_rx_struct->mb_rx_crc_hi =                                                    //Get received CRC value.
                  mb_rx_struct->mb_rx_frame[mb_rx_struct->mb_rx_index - 1];
    mb_rx_struct->mb_rx_crc_lo =
                  mb_rx_struct->mb_rx_frame[mb_rx_struct->mb_rx_index - 2];
    
    crc = mb_crc_16(mb_rx_struct->mb_rx_frame, mb_rx_struct->mb_rx_index - 2);      //Calculate CRC value and save in rx struct.
    mb_rx_struct->mb_calc_crc_hi = crc >> 8;
    mb_rx_struct->mb_calc_crc_lo = crc;
    
    if(mb_rx_struct->mb_calc_crc_hi != mb_rx_struct->mb_rx_crc_hi ||                //If CRCs do not match, exit with error.
       mb_rx_struct->mb_calc_crc_lo != mb_rx_struct->mb_rx_crc_lo)
    {
        mb_diagnostics->mb_bus_com_err_count++;                                     //Increment comm error counter.
        mb_update_event_log(MB_RX_COM_ERROR);                                       //Update event log.
        mb_rx_tx_reset();                                                           //Reset indexes for next frame.
        return MB_ERR_PARITY;
    }
    
    mb_diagnostics->mb_bus_msg_count++;                                             //Valid message.  Increment bus message counter.
       
    mb_rx_struct->mb_slave_addr = mb_rx_struct->mb_rx_frame[MB_RTU_SLAVE_ADDR];     //Get slave address.
    
    if(mb_rx_struct->mb_slave_addr != mb_this_address &&                            //Check if message is for this device.
       mb_rx_struct->mb_slave_addr != 0)
    {
        mb_rx_tx_reset();                                                           //Reset indexes for next frame.
        return MB_NO_ERROR;
    }
    
    mb_diagnostics->mb_slave_message_count++;                                       //Message is for this device.  Increment slave message counter.
    
    if(mb_rx_struct->mb_slave_addr == 0)                                            //Check if broadcast message.
    {
        mb_diagnostics->mb_no_response_count++;                                     //Increment no response counter.
        mb_update_event_log(MB_RX_BROADCAST_RCVD);                                  //Update event log.
    }
    
    mb_rx_struct->mb_function_code = mb_rx_struct->mb_rx_frame[MB_RTU_FUNC_CODE];   //Get function code.
    
    switch(mb_rx_struct->mb_function_code)                                          //Determine how to proceed next.
    {
        /*****************************************************************Read Coils****************************************************************/
        case MB_READ_COILS:
            if(mb_listen_only == MB_TRUE)                                           //Take no action if in listen only mode.
            {
                mb_update_event_log(MB_TX_LISTEN_MODE);                             //Update event log.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_NO_ERROR;
            }
            
            if(mb_rx_struct->mb_slave_addr == 0)                                    //This function does not support broadcast requests.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_BRDCAST;
            }
            
            if(!(*(mb_callback_funcs + MB_RC)))                                     //Check if function is not supported.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                mb_rtu_exception(MB_READ_COILS, ILLEGAL_FUNCTION);                  //Setup exception response.
                mb_tx_frame();                                                      //Transmit exception and reset.
                return MB_ERR_UNSUPPORTED_CODE;
            }
            
            mb_rx_struct->mb_values = mb_rtu_get_quantity();                        //Get quantity of inputs.
            
            if(mb_rx_struct->mb_values < 1 || mb_rx_struct->mb_values > 0x07D0)     //Coils must be between 1 and 0x07D0.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                mb_rtu_exception(MB_READ_COILS, ILLEGAL_DATA_VALUE);                //Setup exception response.
                mb_tx_frame();                                                      //Transmit exception and reset.
                return MB_ERR_INVALID_QUANTITY;
            }
            
            mb_rx_struct->mb_address = mb_rtu_get_address();                        //Get starting address.
            
            (*(mb_callback_funcs + MB_RC))(mb_rx_struct, mb_tx_struct);             //Do callback function.
                    
            if(mb_tx_struct->mb_tx_exception)                                       //Check if callback function set an exception.
            {                   
                if(mb_tx_struct->mb_tx_exception == ILLEGAL_DATA_ADDRESS)           //Illegal range of addresses.
                {
                    mb_diagnostics->mb_slave_exc_err_count++;                       //Increment exception error counter.
                    mb_update_event_log(MB_TX_READ_EXCEPTION);                      //Update event log.
                    mb_rtu_exception(MB_READ_COILS, ILLEGAL_DATA_ADDRESS);          //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                    return MB_ERR_EXCEPTION;
                }
                    
                else if(mb_tx_struct->mb_tx_exception == MB_TX_ABORT_EXCEPTION)     //Device failure exception.
                {
                    mb_diagnostics->mb_slave_exc_err_count++;                       //Increment exception error counter.                    
                    mb_update_event_log(MB_TX_ABORT_EXCEPTION);                     //Update event log.
                    mb_rtu_exception(MB_READ_COILS, SERVER_DEVICE_FAILURE);         //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                    return MB_ERR_EXCEPTION;
                }
                    
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                mb_rtu_exception(MB_READ_COILS, ILLEGAL_FUNCTION);                  //Setup exception response.
                mb_tx_frame();                                                      //Transmit exception and reset.
                return MB_ERR_EXCEPTION;
            }
                
            //Transaction successful.  
            mb_diagnostics->mb_event_counter++;                                     //Increment event counter.
            mb_update_event_log(MB_TX_NORMAL_RESPONSE);                             //Update event log.
            mb_tx_frame();                                                          //Transmit response and reset.  
        break;
        
        /*************************************************************Write Single Coil*************************************************************/
        case MB_WRITE_SINGLE_COIL:
            if(mb_listen_only == MB_TRUE)                                           //Take no action if in listen only mode.
            {
                mb_update_event_log(MB_RX_LISTEN_MODE);                             //Update event log.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_NO_ERROR;
            }
            
            if(!(*(mb_callback_funcs + MB_WSC)))                                    //Check if function is not supported.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                
                if(mb_rx_struct->mb_slave_addr)                                     //Check if not broadcast.
                {
                    mb_rtu_exception(MB_WRITE_SINGLE_COIL, ILLEGAL_FUNCTION);       //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                }
                
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_UNSUPPORTED_CODE;
            }
            
            mb_rx_struct->mb_values = mb_rtu_get_quantity();                        //Get output value.
            
            if(mb_rx_struct->mb_values !=                                           //Value must be either 0x0000 or 0xFF00.
                             0x0000 && mb_rx_struct->mb_values != 0xFF00)
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                
                if(mb_rx_struct->mb_slave_addr)                                     //Check if not broadcast.
                {
                    mb_rtu_exception(MB_WRITE_SINGLE_COIL, ILLEGAL_DATA_VALUE);     //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                }
                
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_INVALID_QUANTITY;
            }
            
            mb_rx_struct->mb_address = mb_rtu_get_address();                        //Get address.
            
            (*(mb_callback_funcs + MB_WSC))(mb_rx_struct, mb_tx_struct);            //Do callback function.
            
            if(mb_tx_struct->mb_tx_exception)                                       //Check if callback function set an exception.
            {
                if(mb_tx_struct->mb_tx_exception == ILLEGAL_DATA_ADDRESS)           //Illegal address.
                {
                    mb_diagnostics->mb_slave_exc_err_count++;                       //Increment exception error counter.
                    mb_update_event_log(MB_TX_READ_EXCEPTION);                      //Update event log.
                    
                    if(mb_rx_struct->mb_slave_addr)                                 //Check if not broadcast.
                    {
                        mb_rtu_exception(MB_WRITE_SINGLE_COIL,                      //Setup exception response.
                                          ILLEGAL_DATA_ADDRESS);
                        mb_tx_frame();                                              //Transmit exception and reset.
                    }
                    mb_rx_tx_reset();                                               //Reset indexes for next frame.
                    return MB_ERR_EXCEPTION;
                    
                }
                
                else if(mb_tx_struct->mb_tx_exception == SERVER_DEVICE_FAILURE)     //Device failure exception.
                {
                    mb_diagnostics->mb_slave_exc_err_count++;                       //Increment exception error counter.
                    mb_update_event_log(MB_TX_ABORT_EXCEPTION);                     //Update event log.
                    
                    if(mb_rx_struct->mb_slave_addr)                                 //Check if not broadcast.
                    {
                        mb_rtu_exception(MB_WRITE_SINGLE_COIL,                      //Setup exception response.
                        SERVER_DEVICE_FAILURE);
                        mb_tx_frame();                                              //Transmit exception and reset.
                    }
                    mb_rx_tx_reset();                                               //Reset indexes for next frame.
                    return MB_ERR_EXCEPTION;
                }
                
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_READ_EXCEPTION);                          //Update event log.
                
                if(mb_rx_struct->mb_slave_addr)                                     //Check if not broadcast.
                {
                    mb_rtu_exception(MB_WRITE_SINGLE_COIL, ILLEGAL_FUNCTION);       //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                }
                
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_EXCEPTION;    
            }
            
            //Transaction successful.
            mb_diagnostics->mb_event_counter++;                                     //Increment event counter.
            mb_update_event_log(MB_TX_NORMAL_RESPONSE);                             //Update event log.
            
            if(mb_rx_struct->mb_slave_addr)                                         //Check if not broadcast.
            {
                mb_tx_frame();                                                      //Transmit response and reset.
            }
            
            mb_rx_tx_reset();                                                       //Reset indexes for next frame.         
        break;
        
        /********************************************************Get Communications Event Log*******************************************************/
        case MB_GET_COM_EVENT_LOG:          
            if(mb_listen_only == MB_TRUE)                                           //Take no action if in listen only mode.
            {
                mb_update_event_log(MB_RX_LISTEN_MODE);                             //Update event log.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_NO_ERROR;
            }
            
            if(mb_rx_struct->mb_slave_addr == 0)                                    //This function does not support broadcast requests.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_BRDCAST;
            }
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Set slave address in response.
                          mb_this_address;      
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Set request type in response.
                          MB_GET_COM_EVENT_LOG;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0x00;          //Place holder for byte count. Calculated later.
                          
            if (mb_is_programming)                                                  //Is controller currently in the middle of programming?
            {
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0xFF;      //If so, set all status bits.
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0xFF;  
            }
            else
            {
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0x00;      //Else clear all status bits.
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0x00;  
            }
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get upper byte of event counter.
                          mb_diagnostics->mb_event_counter >> 8;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get lower byte of event counter.
                          mb_diagnostics->mb_event_counter;
        
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get upper byte of message counter.
                          mb_diagnostics->mb_bus_msg_count >> 8;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get lower byte of message counter.
                          mb_diagnostics->mb_bus_msg_count;
        
            mb_get_event_log();                                                     //Add the event log to the frame.
            
            mb_tx_struct->mb_tx_frame[MB_EVNT_LOG_BYTE_COUNT] =                     //Recalculate the byte count.
                          mb_tx_struct->mb_tx_index - 3;
            
            crc = mb_crc_16(mb_tx_struct->mb_tx_frame, mb_tx_struct->mb_tx_index);  //Get CRC.
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc >> 8;
            
            //NOTE: This function does not increment the event counter.
            mb_update_event_log(MB_TX_NORMAL_RESPONSE);                             //Update event log.         
            mb_tx_frame();                                                          //Transmit response and reset.
        break;
            
        /******************************************************Get Communications Event Counter*****************************************************/
        case MB_GET_COM_EVENT_COUNTER:
            if(mb_listen_only == MB_TRUE)                                           //Take no action if in listen only mode.
            {
                mb_update_event_log(MB_RX_LISTEN_MODE);                             //Update event log.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_NO_ERROR;
            }
            
            if(mb_rx_struct->mb_slave_addr == 0)                                    //This function does not support broadcast requests.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_BRDCAST;
            }
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Set slave address in response.
                          mb_this_address;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Set request type in response.
                          MB_GET_COM_EVENT_COUNTER;
            
            if (mb_is_programming)                                                  //Is controller currently in the middle of programming?
            {
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0xFF;      //If so, set all status bits.
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0xFF;
            }
            else
            {
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0x00;      //Else clear all status bits.
                mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = 0x00;
            }
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get upper byte of event counter.
                          mb_diagnostics->mb_event_counter >> 8;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =                //Get lower byte of event counter.
                          mb_diagnostics->mb_event_counter;
            
            crc = mb_crc_16(mb_tx_struct->mb_tx_frame, mb_tx_struct->mb_tx_index);  //Get CRC.
            
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc;
            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] = crc >> 8;
            
            //NOTE: This function does not increment the event counter.
            mb_update_event_log(MB_TX_NORMAL_RESPONSE);                             //Update event log.
            mb_tx_frame();                                                          //Transmit response and reset.
        break;
        
        /*****************************************************************Diagnostics***************************************************************/
        case MB_DIAGNOSTIC:         
            mb_rx_struct->mb_sub_function =                                         //Get sub-function code.
                          mb_rx_struct->mb_rx_frame[MB_SUB_FUNCTION];
            mb_rx_struct->mb_sub_function <<= 8;
            mb_rx_struct->mb_sub_function |=
                          mb_rx_struct->mb_rx_frame[MB_SUB_FUNCTION + 1];
            
            switch(mb_rx_struct->mb_sub_function)                                   //determine sub-function and act accordingly.
            {
                /**********************************************************Return Query Data********************************************************/
                case MB_DIAG_RTN_QUERY_DATA:
                    if(mb_listen_only == MB_TRUE)                                   //Take no action if in listen only mode.
                    {
                        mb_update_event_log(MB_RX_LISTEN_MODE);                     //Update event log.
                        mb_rx_tx_reset();                                           //Reset indexes for next frame.
                        return MB_NO_ERROR;
                    }
                    
                    if(mb_rx_struct->mb_slave_addr == 0)                            //This function does not support broadcast requests.
                    {
                        mb_diagnostics->mb_slave_exc_err_count++;                   //Increment exception error counter.
                        mb_rx_tx_reset();                                           //Reset indexes for next frame.
                        return MB_ERR_BRDCAST;
                    }
                    
                    for(int i = 0; i < mb_rx_struct->mb_rx_index; i++)              //Echo frame back to sender.
                    {
                        mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =
                                      mb_rx_struct->mb_rx_frame[i];
                    }
                                        
                    //Transaction successful.
                    mb_diagnostics->mb_event_counter++;                             //Increment event counter.
                    mb_update_event_log(MB_TX_NORMAL_RESPONSE);                     //Update event log.
                    mb_tx_frame();                                                  //Transmit response and reset.
                break;
                
                /********************************************************Restart Communications*****************************************************/
                case MB_DIAG_RESTART_COMMS:
                    if(mb_tx_struct->mb_tx_frame[MB_DIAG_DATA] == 0xFF)             //Check if event log should be cleared.
                    {
                        void mb_clear_event_log();                                  //Clear event log.
                    }
                    else
                    {
                        mb_update_event_log(MB_COMM_RESET);                         //Update event log.
                    }
                    
                    mb_listen_only = MB_FALSE;                                      //Turn off listen only mode.
                    
                    mb_diagnostics->mb_bus_msg_count = 0;
                    mb_diagnostics->mb_bus_com_err_count = 0;
                    mb_diagnostics->mb_slave_exc_err_count = 0;
                    mb_diagnostics->mb_slave_message_count = 0;
                    mb_diagnostics->mb_no_response_count = 0;                       //Reset all counters.
                    mb_diagnostics->mb_slave_nak_count = 0;
                    mb_diagnostics->mb_slave_busy_count = 0;
                    mb_diagnostics->mb_char_overrun_count = 0;
                    mb_diagnostics->mb_event_counter = 0;
                    
                    //NOTE: This function does not increment the event counter.                 
                    if(mb_rx_struct->mb_slave_addr)                                 //Check if not broadcast.
                    {
                        for(int i = 0; i < mb_rx_struct->mb_rx_index; i++)          //Echo frame back to sender.
                        {
                            mb_tx_struct->mb_tx_frame[mb_tx_struct->mb_tx_index++] =
                            mb_rx_struct->mb_rx_frame[i];
                        }
                        
                        mb_tx_frame();                                              //Transmit response and reset.
                    }
                    
                    mb_rx_tx_reset();                                               //Reset indexes for next frame.
                break;
                
                /********************************************************Force Listen Only Mode*****************************************************/
                case MB_DIAG_LISTEN_MODE:
                
                mb_listen_only = MB_TRUE;                                           //Turn on listen only mode.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                break;
                
                /*********************************************************Set ASCII Delimiter*******************************************************/
                case MB_DIAG_ASCII_DELIM:
                break;
                case MB_DIAG_CLR_CNTR_AND_DIAG:
                break;
                case MB_DIAG_CLR_CHAR_OVRN_CNT:
                break;
                
                
                case MB_DIAG_RTN_DIAG_REG:
                break;
                case MB_DIAG_RTN_BUS_MSG_CNT:
                break;
                case MB_DIAG_RTN_COM_ERR_CNT:
                break;
                case MB_DIAG_RTN_EXCPTN_CNT:
                break;
                case MB_DIAG_RTN_MSG_CNT:
                break;
                case MB_DIAG_RTN_NO_RSP_CNT:
                break;
                case MB_DIAG_RTN_NAK_CNT:
                break;
                case MB_DIAG_RTN_BUSY_CNT:
                break;
                case MB_DIAG_RTN_CHAR_OVRN_CNT:
                break;
                
                
                /******************************************************Unknown Sub-function Code****************************************************/
                default:
                    //Unknown sub-function code.                    
                    mb_diagnostics->mb_slave_exc_err_count++;                       //Increment exception error counter.
                    
                    if(mb_listen_only == MB_TRUE)                                   //Take no action if in listen only mode.
                    {
                        mb_update_event_log(MB_RX_LISTEN_MODE);                     //Update event log.
                        mb_rx_tx_reset();                                           //Reset indexes for next frame.
                        return MB_ERR_UNKNOWN_SUB;  
                    }
                    
                    if(mb_rx_struct->mb_slave_addr == 0)                            //Check for broadcast.
                    {
                        mb_update_event_log(MB_TX_READ_EXCEPTION);                  //Update event log.
                        mb_rx_tx_reset();                                           //Reset indexes for next frame.
                        return MB_ERR_UNKNOWN_SUB;
                    }
                    
                    mb_update_event_log(MB_TX_READ_EXCEPTION);                      //Update event log.
                    mb_rtu_exception(MB_READ_COILS, ILLEGAL_FUNCTION);              //Setup exception response.
                    mb_tx_frame();                                                  //Transmit exception and reset.
                    return MB_ERR_UNKNOWN_SUB;                  
                break;
            }
        break;
        
        
        
        
        
        
        
        
        
        case MB_READ_DISCRETE_INPUTS:
        break;
        case MB_READ_HOLDING_REGISTERS:
        break;
        case MB_READ_INPUT_REGISTERS:
        break;      
        case MB_WRITE_MULTIPLE_REGISTERS:
        break;
        case MB_READ_FILE_RECORD:
        break;
        case MB_WRITE_FILE_RECORD:
        break;
        case MB_MASK_WRITE_REGISTER:
        break;
        case MB_WRITE_SINGLE_REGISTER:
        break;
        case MB_WRITE_MULTIPLE_COILS:
        break;
        case MB_READ_WRITE_MULTI_REGS:
        break;
        case MB_READ_FIFO_QUEUE:
        break;
        case MB_READ_DEVICE_ID:
        break;
        case MB_PROGRAM_CONTROLLER:
        break;
        case MB_POLL:
        break;      
        case MB_READ_EXCEPTION_STATUS:
        break;
        case MB_REPORT_SERVER_ID:
        break;

        /************************************************************Unknown Function Code**********************************************************/
        default:                                                                    //Function code not known.          
            if(mb_listen_only == MB_TRUE)                                           //Is device in listen only mode?
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_update_event_log(MB_TX_LISTEN_MODE);                             //Update event log.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_UNKNOWN_CODE;
            }
            
            if(mb_rx_struct->mb_slave_addr == 0)                                    //Check if broadcast message.
            {
                mb_diagnostics->mb_slave_exc_err_count++;                           //Increment exception error counter.
                mb_rx_tx_reset();                                                   //Reset indexes for next frame.
                return MB_ERR_UNKNOWN_CODE;
            }
             
            mb_diagnostics->mb_slave_exc_err_count++;                               //Increment exception error counter.
            mb_update_event_log(MB_TX_READ_EXCEPTION);                              //Update event log.
            mb_rtu_exception(mb_rx_struct->mb_function_code, ILLEGAL_FUNCTION);     //Setup exception response.
            mb_tx_frame();                                                          //Transmit exception and reset.
            return MB_ERR_UNKNOWN_CODE;
        break;
    }
    return MB_NO_ERROR;
}