#include "lip_config.h"

#ifdef LIP_USE_MB_TCP_SERVER
    #include "lip_mb_server_private.h"
    #include "lip_mb_defs.h"
    #include <stddef.h>
    #include <string.h>

    #ifndef LIP_MB_SERVER_INPUT_REGS_MAX
        #define LIP_MB_SERVER_INPUT_REGS_MAX      65536
    #endif
    #ifndef LIP_MB_SERVER_HOLDING_REGS_MAX
        #define LIP_MB_SERVER_HOLDING_REGS_MAX    65536
    #endif
    #ifndef LIP_MB_SERVER_DISCRETE_INPUTS_MAX
        #define LIP_MB_SERVER_DISCRETE_INPUTS_MAX 65536
    #endif
    #ifndef LIP_MB_SERVER_COILS_MAX
        #define LIP_MB_SERVER_COILS_MAX           65536
    #endif

    #define LIP_MB_ITEM_ADDR(ref_addr) ((uint32_t)(ref_addr) + 1)


    typedef t_lip_mb_exception_code (*t_mb_func_cb)(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_read_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_write_reg(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_write_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_read_write_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_mask_write_reg(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_read_bits(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_write_coil(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);
    static t_lip_mb_exception_code f_write_coils(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t func_code, uint8_t *data, uint16_t data_size, uint16_t *resp_size);

    static t_lip_mb_server_cbs f_mb_cbs;

    static const t_mb_func_cb func_table[] = {
    #if LIP_MB_SERVER_DISCRETE_INPUTS_MAX > 0
        [LIP_MB_FUNC_READ_INPUTS] = f_read_bits,
    #endif
    #if LIP_MB_SERVER_COILS_MAX > 0
        [LIP_MB_FUNC_READ_COILS] = f_read_bits,
        [LIP_MB_FUNC_WRITE_SINGLE_COIL] = f_write_coil,
        [LIP_MB_FUNC_WRITE_MULTIPLE_COILS] = f_write_coils,
    #endif
    #if LIP_MB_SERVER_INPUT_REGS_MAX > 0
        [LIP_MB_FUNC_READ_INPUT_REGS] = f_read_regs,
    #endif
    #if LIP_MB_SERVER_HOLDING_REGS_MAX > 0
        [LIP_MB_FUNC_READ_HOLDING_REGS] = f_read_regs,
        [LIP_MB_FUNC_WRITE_SINGLE_REG] = f_write_reg,
        [LIP_MB_FUNC_WRITE_MULTIPLE_REGS] = f_write_regs,
        [LIP_MB_FUNC_MASK_WRITE_REG]  = f_mask_write_reg,
        [LIP_MB_FUNC_RW_MULTIPLE_REGS] = f_read_write_regs,
    #endif
    };

    void lip_mb_exec_func(const t_lip_mb_con_info *coninfo, uint8_t devaddr, uint8_t *data, uint16_t data_size, uint16_t *resp_size) {
        t_lip_mb_exception_code ret;        
        if (data_size == 0) {
            ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            *resp_size = 0;
        } else {
            uint8_t func_code = data[0];
            /** @todo check devaddr */

            if ((func_code >= sizeof(func_table)/sizeof(func_table[0])) ||
                    (func_table[func_code] == NULL)) {
                ret = LIP_MB_EXC_ILLEGAL_FUNCTION;
            } else {
                ret = func_table[func_code](coninfo, devaddr, func_code, &data[1], data_size-1, resp_size);
            }

            if (ret != LIP_MB_EXC_OK) {
                data[0] |= 0x80;
                data[1] = ret;
                *resp_size = 2;
            } else {
                *resp_size = *resp_size + 1;
            }
        }
    }

    void lip_mb_server_cb_init(t_lip_mb_server_cbs *cbs) {
        cbs->read_bits = NULL;
        cbs->read_regs = NULL;
        cbs->write_bits = NULL;
        cbs->read_bits = NULL;
    }

    void lip_mb_server_cb_set(const t_lip_mb_server_cbs *cbs) {
        f_mb_cbs = *cbs;
    }

#if (LIP_MB_SERVER_INPUT_REGS_MAX > 0) || (LIP_MB_SERVER_HOLDING_REGS_MAX > 0)
    static t_lip_mb_exception_code f_read_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                               uint8_t func_code, uint8_t *data,
                                               uint16_t data_size, uint16_t *resp_size) {

        t_lip_mb_exception_code ret = f_mb_cbs.read_regs == NULL ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < 4 ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);        
        if (ret == LIP_MB_EXC_OK) {
            int inputs = func_code == LIP_MB_FUNC_READ_INPUT_REGS ;
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
            uint16_t regcnt = LIP_MB_GET_WORD(&data[2]);
            if ((regcnt < 1) || (regcnt > LIP_MB_READ_REGS_MAX_CNT)) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            } else {
                uint32_t last_addr =  (uint32_t)addr + regcnt;
                if ((inputs && (last_addr > LIP_MB_SERVER_INPUT_REGS_MAX)) ||
                    (!inputs && (last_addr > LIP_MB_SERVER_HOLDING_REGS_MAX))) {
                    ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
                } else {
                    uint8_t bytes_cnt = 2*regcnt;
                    data[0] = bytes_cnt;
                    memset(&data[1], 0, bytes_cnt);
                    ret = f_mb_cbs.read_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), regcnt,
                                             inputs ? LIP_MB_REGTYPE_INPUT : LIP_MB_REGTYPE_HOLDING, &data[1]);
                    if (ret == LIP_MB_EXC_OK) {
                        *resp_size = 1 + bytes_cnt;
                    }
                }
            }
        }

        return ret;
    }
#endif
#if  (LIP_MB_SERVER_HOLDING_REGS_MAX > 0)
    static t_lip_mb_exception_code f_write_reg(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                               uint8_t func_code, uint8_t *data,
                                               uint16_t data_size, uint16_t *resp_size) {

        t_lip_mb_exception_code ret = f_mb_cbs.write_regs == NULL ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < 4 ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
#if LIP_MB_SERVER_HOLDING_REGS_MAX < 65536
            if (addr >= LIP_MB_SERVER_HOLDING_REGS_MAX) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
            } else {
#endif
                ret = f_mb_cbs.write_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), 1, &data[2]);
                if (ret == LIP_MB_EXC_OK) {
                    *resp_size = 4;
                }
#if LIP_MB_SERVER_HOLDING_REGS_MAX < 65536
            }
#endif
        }
        return ret;
    }

    static t_lip_mb_exception_code f_write_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                                uint8_t func_code, uint8_t *data,
                                                uint16_t data_size, uint16_t *resp_size) {

        static const unsigned hdr_size = 5;
        t_lip_mb_exception_code ret = f_mb_cbs.write_regs == NULL ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < hdr_size ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
            uint16_t regcnt = LIP_MB_GET_WORD(&data[2]);
            uint8_t  byte_cnt = data[4];
            if ((regcnt < 1) || (regcnt > LIP_MB_WRITE_REGS_MAX_CNT) || (data_size < (hdr_size + byte_cnt))
                    || (byte_cnt != 2*regcnt)) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            } else {
                uint32_t last_addr =  (uint32_t)addr + regcnt;
                if (last_addr > LIP_MB_SERVER_HOLDING_REGS_MAX) {
                    ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
                } else {
                    ret = f_mb_cbs.write_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), regcnt, &data[hdr_size]);
                    if (ret == LIP_MB_EXC_OK) {
                        *resp_size = 4;
                    }
                }
            }

        }
        return ret;
    }

    static t_lip_mb_exception_code f_mask_write_reg(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                                    uint8_t func_code, uint8_t *data,
                                                    uint16_t data_size, uint16_t *resp_size) {

        t_lip_mb_exception_code ret = ((f_mb_cbs.write_regs == NULL) || (f_mb_cbs.read_regs == NULL)) ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < 6 ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
#if LIP_MB_SERVER_HOLDING_REGS_MAX < 65536
            if (addr >= LIP_MB_SERVER_HOLDING_REGS_MAX) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
            } else {
#endif
                uint8_t reg_val[2] = {0, 0};
                ret = f_mb_cbs.read_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), 1, LIP_MB_REGTYPE_HOLDING, reg_val);
                if (ret == LIP_MB_EXC_OK) {
                    unsigned i;
                    for (i = 0; i < sizeof(reg_val); i++) {
                        uint8_t and_mask = data[2+i];
                        uint8_t or_mask = data[4+i];
                        reg_val[i] = (reg_val[i] & and_mask) | (or_mask & ~and_mask);
                    }
                    ret = f_mb_cbs.write_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), 1, reg_val);
                }
                if (ret == LIP_MB_EXC_OK) {
                    *resp_size = 6;
                }
#if LIP_MB_SERVER_HOLDING_REGS_MAX < 65536
            }
#endif
        }
        return ret;
    }

    static t_lip_mb_exception_code f_read_write_regs(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                                     uint8_t func_code, uint8_t *data,
                                                     uint16_t data_size, uint16_t *resp_size) {

        static const unsigned hdr_size = 9;
        t_lip_mb_exception_code ret = ((f_mb_cbs.write_regs == NULL) || (f_mb_cbs.read_regs == NULL)) ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < hdr_size ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            const uint16_t read_addr = LIP_MB_GET_WORD(&data[0]);
            const uint16_t read_cnt = LIP_MB_GET_WORD(&data[2]);
            const uint16_t write_addr = LIP_MB_GET_WORD(&data[4]);
            const uint16_t write_cnt = LIP_MB_GET_WORD(&data[6]);
            const uint8_t  write_byte_cnt  = data[8];
            if ((read_cnt < 1) || (read_cnt > LIP_MB_READ_REGS_MAX_CNT) ||
                (write_cnt < 1) || (write_cnt > LIP_MB_RW_WRITE_REGS_MAX_CNT) ||
                    (data_size < (hdr_size + write_byte_cnt)) || (write_byte_cnt != 2*write_cnt)) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            } else {
                const uint32_t last_read_addr  =  (uint32_t)read_addr + read_cnt;
                const uint32_t last_write_addr =  (uint32_t)write_addr + write_cnt;

                if ((last_read_addr > LIP_MB_SERVER_HOLDING_REGS_MAX) ||
                    (last_write_addr > LIP_MB_SERVER_HOLDING_REGS_MAX)) {
                    ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
                } else {
                    const uint8_t read_byte_cnt = 2 * read_cnt;
                    ret = f_mb_cbs.write_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(write_addr), write_cnt, &data[hdr_size]);
                    if (ret == LIP_MB_EXC_OK) {
                        data[0] = read_byte_cnt;
                        memset(&data[1], 0, read_byte_cnt);
                        ret = f_mb_cbs.read_regs(coninfo, devaddr, LIP_MB_ITEM_ADDR(read_addr), read_cnt, LIP_MB_REGTYPE_HOLDING, &data[1]);
                    }
                    if (ret == LIP_MB_EXC_OK) {
                        *resp_size = 1 + read_byte_cnt;
                    }
                }
            }
        }
        return ret;
    }
#endif

#if (LIP_MB_SERVER_DISCRETE_INPUTS_MAX > 0) || (LIP_MB_SERVER_COILS_MAX > 0)
    static t_lip_mb_exception_code f_read_bits(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                               uint8_t func_code, uint8_t *data,
                                               uint16_t data_size, uint16_t *resp_size) {

        t_lip_mb_exception_code ret = (f_mb_cbs.read_bits == NULL) ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < 4 ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
            uint16_t bit_cnt = LIP_MB_GET_WORD(&data[2]);
            if ((bit_cnt < 1) || (bit_cnt > LIP_MB_READ_BITS_MAX_CNT)) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            } else {
                int inputs = func_code == LIP_MB_FUNC_READ_INPUTS;
                 uint32_t last_addr =  (uint32_t)addr + bit_cnt;
                 if ((inputs && (last_addr > LIP_MB_SERVER_DISCRETE_INPUTS_MAX)) ||
                     (!inputs && (last_addr > LIP_MB_SERVER_COILS_MAX))) {
                     ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
                 } else {
                     uint8_t bytes_cnt = (bit_cnt + 7)/8;
                     memset(&data[1], 0, bytes_cnt);
                     ret = f_mb_cbs.read_bits(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), bit_cnt,
                                              inputs ? LIP_MB_BITTYPE_INPUT : LIP_MB_BITTYPE_COIL,
                                              &data[1]);
                     if (ret == LIP_MB_EXC_OK) {
                         data[0] = bytes_cnt;
                         *resp_size = 1 + bytes_cnt;
                     }
                }
            }
        }
        return ret;
    }
#endif

#if LIP_MB_SERVER_COILS_MAX > 0
    static t_lip_mb_exception_code f_write_coil(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                                uint8_t func_code, uint8_t *data,
                                                uint16_t data_size, uint16_t *resp_size) {
        t_lip_mb_exception_code ret = (f_mb_cbs.write_bits == NULL) ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < 4 ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
            uint16_t value = LIP_MB_GET_WORD(&data[2]);
#if LIP_MB_SERVER_COILS_MAX < 65536
            if (addr >= LIP_MB_SERVER_COILS_MAX) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
            } else {
#endif
                uint8_t bit = 0;
                if (value == 0xFF00) {
                    bit = 1;
                } else if (value != 0) {
                    ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
                }

                if (ret == LIP_MB_EXC_OK) {
                    ret = f_mb_cbs.write_bits(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), 1, &bit);
                }

                if (ret == LIP_MB_EXC_OK) {
                    *resp_size = 4;
                }
#if LIP_MB_SERVER_COILS_MAX < 65536
            }
#endif
        }
        return ret;
    }

    static t_lip_mb_exception_code f_write_coils(const t_lip_mb_con_info *coninfo, uint8_t devaddr,
                                                 uint8_t func_code, uint8_t *data,
                                                 uint16_t data_size, uint16_t *resp_size) {

        static const unsigned hdr_size = 5;
        t_lip_mb_exception_code ret = (f_mb_cbs.write_bits == NULL) ? LIP_MB_EXC_ILLEGAL_FUNCTION :
                                      (data_size < hdr_size ? LIP_MB_EXC_ILLEGAL_DATA_VALUE : LIP_MB_EXC_OK);
        if (ret == LIP_MB_EXC_OK) {
            uint16_t addr = LIP_MB_GET_WORD(&data[0]);
            uint16_t bit_cnt = LIP_MB_GET_WORD(&data[2]);
            uint16_t byte_cnt = data[4];

            if ((bit_cnt < 1) || (bit_cnt > LIP_MB_WRITE_BITS_MAX_CNT) || (data_size < (hdr_size + byte_cnt))
                    || (byte_cnt != (bit_cnt + 7)/8)) {
                ret = LIP_MB_EXC_ILLEGAL_DATA_VALUE;
            } else {
                uint32_t last_addr =  (uint32_t)addr + bit_cnt;
                if (last_addr > LIP_MB_SERVER_COILS_MAX) {
                    ret = LIP_MB_EXC_ILLEGAL_DATA_ADDRESS;
                } else {
                    ret = f_mb_cbs.write_bits(coninfo, devaddr, LIP_MB_ITEM_ADDR(addr), bit_cnt, &data[hdr_size]);
                    if (ret == LIP_MB_EXC_OK) {
                        *resp_size = 4;
                    }
                }
            }
        }
        return ret;
    }
#endif


#endif







