#include"modbus_slave.h"
#include <string.h>
//数据区域表
//从站数据读取域总缓冲池
static modbus_dataarea_t data_area_table[MODBUS_SLAVE_DATAAREA_POOL_SIZE];
static int data_area_count = 0;

/**
 * 查找有效的数据域
 * 
 * @author lxz 
 * 
 * @param group 
 * @param id 
 * @param cmd 
 * 
 * @return modbus_data_area_t* 
 */
static modbus_dataarea_t* modbus_slave_find_dataarea(modbus_slave_t * slave,
                                                     unsigned char data_type,
                                                     unsigned short address)
{
    modbus_dataarea_t *data = slave->data_area_list;
    while (data != 0)
    {
        if (data->data_type == data_type)
        {
            if (data->address <= address && (data->address + data->number) > address)
            {
                return data;
            }
        }
        data = (modbus_dataarea_t *)data->next;
    }
    return 0;
}


/**
 * 读取线圈
 * 
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_read_coils(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short number = (req[4] << 8) + req[5];
    char shift = 0;
    int len = 0;
    int index = 0;
    unsigned char *coils;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x01, address);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制
    if (number > 0x7F8 || number == 0 || (data->address + data->number) < (number + address))
    {
        return -3;
    }

    //生成响应命令
    ack[2] = (number + 7) >> 3;
    address -= data->address;
    shift = address & 0x07;
    index = 0;
    len = ack[2];

    coils = &data->buffer[address >> 3];
    //如果字节对齐，可以直接拷贝
    if (shift == 0)
    {
        while (index < len)
        {
            ack[index + 3] = coils[index];
            index++;
        }
    } else
    {
        //字节不对齐需要执行移位处理
        unsigned char dat = 0;
        while (index < len)
        {
            dat = coils[index] >> shift;
            dat |= coils[index + 1] << (8 - shift);
            ack[index + 3] = dat;
            index++;
        }
    }

    return index + 3;
}


/**
 * 读取数据
 * 
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_read_registers(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short number = (req[4] << 8) + req[5];

    int index = 0;
    unsigned char *reg;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x03, address);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制
    if (number > 0x7F || number == 0 || (data->address + data->number) < (number + address))
    {
        return -3;
    }
    //生成响应命令
    address -= data->address;
    index = 0;
    reg = &data->buffer[address * 2];
    number *= 2;
    ack[2] = number;
    while (index < number)
    {
        ack[index + 3] = reg[index + 1];
        ack[index + 4] = reg[index];
        index += 2;
    }
    return index + 3;
}

/**
 * 写单个线圈
 * 
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_write_coil(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short value = (req[4] << 8) + req[5];


    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x01, address);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    address -= data->address;

    if (value != 0)
    {
        data->buffer[address >> 3] |= 1 << (address & 0x07);
    } else
    {
        data->buffer[address >> 3] &= ~(1 << (address & 0x07));
    }

    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], address, 1);
    }

    //生成响应命令
    ack[2] = req[2];
    ack[3] = req[3];
    ack[4] = req[4];
    ack[5] = req[5];

    return 6;
}

/**
 * 写多个线圈
 * 
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_write_coils(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short number = (req[4] << 8) + req[5];
    unsigned short number_old = number;
    char shift = 0;

    int index = 0;
    unsigned char *coils;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x01, address);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制
    if (number > 0x7F8 || number == 0 || (data->address + data->number) < (number + address))
    {
        return -3;
    }

    //执行数据搬运
    ack[2] = (number + 7) >> 3;
    address -= data->address;
    shift = address & 0x07;
    address >>= 3;
    index = 0;

    coils = &data->buffer[address >> 3];
    if (shift == 0)
    {
        //整个字节部分可以直接处理
        while (number > 7)
        {
            coils[index] = req[index + 7];
            index++;
            number -= 8;
        }
        //非完整部分需要进行处理
        if (number != 0)
        {
            unsigned char dat = req[index + 7] & (0xFF >> (8 - number));
            coils[index] &= 0xFF << number;
            coils[index] |= dat;
        }
    } else
    {
        unsigned char dat  = 0;
        unsigned char i = 0;
        //整个字节部分独立处理
        while (number > 7)
        {
            dat = coils[index] & (0xFF >> (8 - shift));
            coils[index] = dat | (req[index + 7] << shift);

            dat = coils[index + 1] & (0xFF << shift);
            coils[index + 1] = dat | (req[index + 7] >> (8 - shift));

            index++;
            number -= 8;
        }

        //最后一个字节不满，按位处理
        while (i < number)
        {
            if (req[index + 7] & (1 << i))
            {
                coils[index] |= 1 << shift;
            } else
            {
                coils[index] &= ~(1 << shift);
            }
            i++;
            shift++;
            if (shift > 7)
            {
                shift = 0;
                coils++;
            }
        }
    }

    //回调函数
    if (slave->on_comm !=0)
    {
        slave->on_comm(slave->id, req[1], address, number_old);
    }

    //生成响应命令
    ack[2] = req[2];
    ack[3] = req[3];
    ack[4] = req[4];
    ack[5] = req[5];

    return 6;
}


/**
 * 写单个寄存器
 * 
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_write_register(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];

    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x03, address);
    unsigned char *reg;
    //没有找到
    if (data == 0)
    {
        return -2;
    }

    address -= data->address;
    reg = &data->buffer[address * 2];
    reg[0] = req[5];
    reg[1] = req[4];
    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], address, 1);
    }

    //生成响应命令
    ack[2] = req[2];
    ack[3] = req[3];
    ack[4] = req[4];
    ack[5] = req[5];

    return 6;
}

/**
 * 进入bootloader 
 *  
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_exit_app(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制

    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFEE, 0);
    }

    //生成响应命令
    if (data->buffer != (void *)0)
    {
        ack[2] = req[2];
        ack[3] = req[3];
        ack[4] = req[4];
        ack[5] = req[5];
        return 6;
    } else
    {
        return -1;
    }


}
/**
 * 离开bootloader 
 *  
 * @author lxz 
 * 
 * @param group 组号
 * @param ack 响应命令缓冲
 * @param req 请求命令缓冲
 * 
 * @return int 返回长度，小于0表示是错误码
 */
static int modbus_slave_enter_app(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制

    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFEF, 0);
    }

    //生成响应命令
    if (data->buffer == 0)
    {
        ack[2] = req[2];
        ack[3] = req[3];
        ack[4] = req[4];
        ack[5] = req[5];
        return 6;
    }           else
    {
        return -1;
    }
}

/**
 * 擦除用户程序
 * 
 * @author lxz (2019/6/12/周三)
 * 
 * @param group 
 * @param ack 
 * @param req 
 * 
 * @return int 
 */
static int modbus_slave_erase_app(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制

    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFEC, 0);
    }

    //生成响应命令
    if (data->buffer != 0)
    {
        ack[2] = 0x00;
        ack[3] = 0x00;
        ack[4] = 0xFF;
        ack[5] = 0x00;
        return 6;
    }           else
    {
        return -1;
    }
}

/**
 * 接收用户程序
 * 
 * @author lxz (2019/6/12/周三)
 * 
 * @param group 
 * @param ack 
 * @param req 
 * 
 * @return int 
 */
static int modbus_slave_receive_app(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short number = (req[4] << 8) + req[5];
    unsigned char *reg;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制
    if (data->buffer != (void *)0)
    {
        //超过数量或者地址限制
        if (number == 0 || (data->address + data->number) < (number + address))
        {
            return -3;
        }
        address -= data->address;
        reg = &data->buffer[address];
        memcpy(reg, &req[6], number);
    }
    
    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFE0, 0);
    }

    //生成响应命令
    if (data->buffer != 0)
    {
        ack[2] = 0x00;
        ack[3] = 0x00;
        ack[4] = 0xFF;
        ack[5] = 0x00;
        return 6;
    } else
    {
        return -1;
    }
}

/**
 * 将app缓冲数据写入指定的位置
 * 
 * @author lxz (2019/6/12/周三)
 * 
 * @param group 
 * @param ack 
 * @param req 
 * 
 * @return int 
 */
static int modbus_slave_copy_to_flash(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short number = (req[6] << 8) + req[7];
    unsigned char *reg;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    if (data->buffer != (void *)0)
    {
        //将写入地址与写入长度写在缓冲最后面
        reg = &data->buffer[number];
        reg[0] = req[2];
        reg[1] = req[3];
        reg[2] = req[4];
        reg[3] = req[5];

    }
    
    
    //回调函数,该函数会开始写入
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFE1, number);
    }

    //生成响应命令
    if (data->buffer != 0)
    {
        ack[2] = req[2];
        ack[3] = req[3];
        ack[4] = req[4];
        ack[5] = req[5];
        return 6;
    }           
    else
    {
        return -1;
    }
}

/**
 * 将后面附带的数据直接写入FLASH 
 * 该指令用于指定直接写入
 * 
 * @author lxz 
 * 
 * @param group 
 * @param ack 
 * @param req 
 * 
 * @return int 
 */
static int modbus_slave_write_to_flash(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short number = (req[4] << 8) + req[5];
    unsigned char *reg = 0;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0xFE, 0);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    if (data->buffer != (void *)0)
    {
        //将写入地址与写入长度写在缓冲最后面
        memcpy(data->buffer, &req[6], number);
        reg = &data->buffer[number];
        reg[0] = 0x00;
        reg[1] = req[2];    
        reg[2] = req[3];
        reg[3] = 0x00;
    }
    
    
    //回调函数,该函数会开始写入
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], 0xFFE2, number);
    }

    //生成响应命令
    if (data->buffer != 0)
    {
        ack[2] = req[2];
        ack[3] = req[3];
        ack[4] = req[4];
        ack[5] = req[5];
        return 6;
    }           
    else
    {
        return -1;
    }
}


/**
 * 写多个寄存器
 * 
 * @author lxz (2019/6/12/周三)
 * 
 * @param group 
 * @param ack 
 * @param req 
 * 
 * @return int 
 */
static int modbus_slave_write_registers(modbus_slave_t * slave, unsigned char *ack, unsigned char *req)
{
    unsigned short address = (req[2] << 8) + req[3];
    unsigned short number = (req[4] << 8) + req[5];

    int index = 0;
    unsigned char *reg;
    //读取符合条件的数据域信息
    modbus_dataarea_t *data = modbus_slave_find_dataarea(slave, 0x03, address);
    //没有找到
    if (data == 0)
    {
        return -2;
    }
    //超过数量或者地址限制
    if (number > 0x7F || number == 0 || (data->address + data->number) < (number + address))
    {
        return -3;
    }
    address -= data->address;
    //执行数据搬运
    reg = &data->buffer[address * 2];
    number *= 2;
    while (index < number)
    {
        reg[index] = req[8 + index];
        reg[index + 1] = req[7 + index];
        index += 2;
    }

    //回调函数
    if (slave->on_comm != 0)
    {
        slave->on_comm(slave->id, req[1], address, number);
    }

    //生成响应命令
    ack[2] = req[2];
    ack[3] = req[3];
    ack[4] = req[4];
    ack[5] = req[5];

    return 6;
}


/**
 * 从站协议接口
 * 
 * @author lxz 
 * 
 * @param slave 从站对象
 * @param ack 回应缓冲
 * @param req 命令缓冲
 * 
 * @return int 返回需要发送的长度
 */
int modbus_slave_run(modbus_slave_t *slave, unsigned char *ack, unsigned char *req)
{
    int ack_len = 0;
    if (req[0] != slave->id) return 0;

    switch (req[1])
    {
    case 0x01:
    case 0x02:
        ack_len = modbus_slave_read_coils(slave, ack, req);
        break;
    case 0x03:
    case 0x04:
        ack_len = modbus_slave_read_registers(slave, ack, req);
        break;
    case 0x05:
        ack_len = modbus_slave_write_coil(slave, ack, req);
        break;
    case 0x06:
        ack_len = modbus_slave_write_register(slave, ack, req);
        break;
    case 0x0F:
        ack_len = modbus_slave_write_coils(slave, ack, req);
        break;
    case 0x10:
        ack_len = modbus_slave_write_registers(slave, ack, req);
        break;
    case 0x23:
        ack_len = modbus_slave_exit_app(slave, ack, req);
        break;
    case 0x24:
        ack_len = modbus_slave_enter_app(slave, ack, req);
        break;
    case 0x20:
        ack_len = modbus_slave_erase_app(slave, ack, req);
        break;
    case 0x21:
        ack_len = modbus_slave_receive_app(slave, ack, req);
        break;
    case 0x22:
        ack_len = modbus_slave_copy_to_flash(slave, ack, req);
        break;
    case 0x29:
        ack_len = modbus_slave_write_to_flash(slave, ack, req);
        break;
    default:
        ack_len = -1;
        break;
    }
    ack[0] = req[0];

    if (ack_len < 0)
    {
        ack[1] = req[1] |0x80;
        ack[2] = -ack_len;
        ack_len = 3;
    } else
    {
        ack[1] = req[1];
    }
    return ack_len;

}

/**
 * 向从站注册一条读取域信息
 * 
 * @author lxz 
 * 
 * @param slave         从站对象
 * @param data_type 
 *                      数据类型，0x01表示位，0x03表示数值，0xFE表示系统占用
 * @param address       数据开始地址
 * @param number        数据区域长度
 * @param data          数据所在指针
 */
modbus_dataarea_t * modbus_slave_add_dataarea(
    modbus_slave_t *slave,
    unsigned char data_type,
    unsigned short address,
    unsigned short number,
    unsigned char *data)
{
   
    if (data_area_count < MODBUS_SLAVE_DATAAREA_POOL_SIZE)
    {
        modbus_dataarea_t *data_area = &data_area_table[data_area_count];
        modbus_dataarea_t *next = slave->data_area_list;

        //信息初始化
        data_area->data_type = data_type;
        data_area->address = address;
        data_area->number = number;
        data_area->buffer = data;
        data_area->next = 0;
    
        //插入从站对象的数据域链表中
        if (next == 0)
        {
            slave->data_area_list = data_area;
        }
        else
        {
            while (next->next != 0)
            {
                next = (modbus_dataarea_t *)next->next;
            }
            next->next = data_area;
        }

        //已经使用+1
        data_area_count++;

        return data_area;
    }
    return 0;
}