#include "modbusTCP_HW.h"
#include "goose_HW.h"
#include "ccu_config.h"


static int modbusTCP_HW_init_SSL(modbusTCP_HW_t* dev)
{
    if (dev && dev->fd > 0)
    {
        // 创建 SSL 上下文
        dev->ctx = SSL_CTX_new(SSLv23_client_method());
        if (dev->ctx == NULL) {
            printe("ctx(%p)\n", dev->ctx);
            return -8;
        }

        // 双向验证
        // SSL_VERIFY_PEER---要求对证书进行认证，没有证书也会放行
        // SSL_VERIFY_FAIL_IF_NO_PEER_CERT---要求客户端需要提供证书，但验证发现单独使用没有证书也会放行
        SSL_CTX_set_verify(dev->ctx, SSL_VERIFY_PEER|SSL_VERIFY_FAIL_IF_NO_PEER_CERT, NULL);
        // 设置信任根证书
        if (SSL_CTX_load_verify_locations(dev->ctx, "/home/root/app_run/ca100year_pcuhw/ca.crt",NULL)<=0){
            printe("ctx(%p)\n", dev->ctx);
            return -7;
        }

        /* 载入用户的数字证书， 此证书用来发送给客户端。 证书里包含有公钥 */
        if (SSL_CTX_use_certificate_file(dev->ctx, "/home/root/app_run/ca100year_pcuhw/client.crt", SSL_FILETYPE_PEM) != 1) {
            printe("ctx(%p)\n", dev->ctx);
            SSL_CTX_free(dev->ctx);
            dev->ctx = NULL;
            return -6;
        }

        /* 载入用户私钥 */
        if (SSL_CTX_use_PrivateKey_file(dev->ctx, "/home/root/app_run/ca100year_pcuhw/client.keyp", SSL_FILETYPE_PEM) != 1) {
            printe("ctx(%p)\n", dev->ctx);
            SSL_CTX_free(dev->ctx);
            dev->ctx = NULL;
            return -5;
        }

        /* 检查用户私钥是否正确 */
        if (!SSL_CTX_check_private_key(dev->ctx)) {
            printe("ctx(%p)\n", dev->ctx);
            return -4;
        }

        // 创建 SSL 对象
        dev->ssl = SSL_new(dev->ctx);
        if (dev->ssl == NULL) {
            printe("SSL_new ctx(%p)\n", dev->ctx);
            SSL_CTX_free(dev->ctx);
            dev->ctx = NULL;
            return -3;
        }

        // 将 SSL 对象绑定到连接套接字
        SSL_set_fd(dev->ssl, dev->fd);

        // 建立 SSL 连接
        int ret = SSL_connect(dev->ssl);
        char buff[256];
        if (ret <= 0) {
            int ssl_errno = SSL_get_error(dev->ssl, ret);
            char* ssl_errstr = ERR_error_string(ssl_errno, buff);
            printe("ssl(%p), ret:%d, ssl_errno:%d, ssl_errstr:%s\n", dev->ssl, ret, ssl_errno, ssl_errstr);
            SSL_free(dev->ssl);
            SSL_CTX_free(dev->ctx);
            dev->ctx = NULL;
            dev->ssl = NULL;
            close(dev->fd);
            dev->fd = -1;
            return -2;
        }else{
            /* 基于 ctx 产生一个新的 SSL */
            printi("ssl(%p)\n", dev->ssl);
            X509 *cert = SSL_get_peer_certificate(dev->ssl);
            // SSL_get_verify_result()是重点，SSL_CTX_set_verify()只是配置启不启用并没有执行认证，调用该函数才会真证进行证书认证
            // 如果验证不通过，那么程序抛出异常中止连接
            if(SSL_get_verify_result(dev->ssl) == X509_V_OK){
                printi("ssl(%p), 证书验证通过\n", dev->ssl);
            }
            if (cert != NULL) {
                printi("ssl(%p), 数字证书信息:\n", dev->ssl);
                char *line = X509_NAME_oneline(X509_get_subject_name(cert), 0, 0);
                printi("ssl(%p), 证书: %s\n", dev->ssl, line);
                free(line);
                line = X509_NAME_oneline(X509_get_issuer_name(cert), 0, 0);
                printi("ssl(%p), 颁发者: %s\n", dev->ssl, line);
                free(line);
                X509_free(cert);
            } else {
                printi("ssl(%p), 无证书信息！\n", dev->ssl);
            }
        }
        return dev->fd;
    }
    return -1;
}

static int modbusTCP_HW_free_SSL(modbusTCP_HW_t* dev)
{
    if (dev)
    {
        if (dev->ssl)
        {
            SSL_shutdown(dev->ssl);
            SSL_free(dev->ssl);
            dev->ssl = NULL;
        }
        if (dev->ctx)
        {
            SSL_CTX_free(dev->ctx);
            dev->ctx = NULL;
        }
    }
    return 0;
}

static int modbusTCP_HW_connect(modbusTCP_HW_t* dev)
{
    int ret =0;
    if (dev && dev->fd <= 0)
    {
        pthread_mutex_lock(&dev->mutex);
        dev->fd = create_inet_stream_socket(dev->connect_ip_string, dev->connect_port, LIBSOCKET_IPv4, 0);
        // dev->fd = socket(AF_INET, SOCK_STREAM, 0);
        printd("dev->fd:%d\n", dev->fd);
        if (dev->enable_ssl)
        {
            ret = modbusTCP_HW_init_SSL(dev);
        }else{
            dev->ctx = NULL;
            dev->ssl = NULL;
            ret = dev->fd;
        }
        // if (dev->fd > 0)
        // {
        //     int flags = fcntl(dev->fd , F_GETFL);
        //     flags |= O_NONBLOCK;
        //     fcntl(dev->fd , F_SETFL, flags);
        // }
        pthread_mutex_unlock(&dev->mutex);
    }
    return ret;
}

static int modbusTCP_HW_recv_with_timeout(modbusTCP_HW_t* dev, int timeout)
{
    int ret, readb;
    fd_set rset;
    struct timeval tv;
    if (dev)
    {
        pthread_mutex_lock(&dev->mutex);
        if (dev->fd > 0)
        {
            FD_ZERO(&rset);
            FD_SET(dev->fd, &rset);
            pthread_mutex_unlock(&dev->mutex);
            tv.tv_sec = timeout/1000;
            tv.tv_usec = (timeout%1000) * 1000;
            switch(select(dev->fd + 1, &rset, NULL, NULL, &tv))
            {
                case 0:
                    // printi("timeout rx_bytes:%d, error:%d, %s\n",dev->rx_bytes, errno, strerror(errno));
                    break;
                case -1:
                    pthread_mutex_lock(&dev->mutex);
                    printe("recv(fd = %d) from(%s) error.\n", dev->fd, dev->connect_ip_string);
                    if (dev->enable_ssl) { modbusTCP_HW_free_SSL(dev); }
                    close(dev->fd);
                    dev->fd = -1;
                    dev->rx_bytes = 0;
                    pthread_mutex_unlock(&dev->mutex);
                    break;
                default:
                    pthread_mutex_lock(&dev->mutex);
                    if(FD_ISSET(dev->fd, &rset))
                    {
                        if (dev->enable_ssl)
                        {
                            readb = SSL_read(dev->ssl, &dev->rx_buff[dev->rx_bytes], (1024 - dev->rx_bytes));
                        }else{
                            readb = recv(dev->fd, &dev->rx_buff[dev->rx_bytes], (1024 - dev->rx_bytes), 0);
                            // printe("connect_ip_string:%s, ret:%d\n",dev->connect_ip_string, readb);
                        }
                        
                        if (readb>=0)
                        {
                            dev->rx_bytes += readb;
                        }else{
                            char buff[256];
                            int ssl_errno = SSL_get_error(dev->ssl, readb);
                            char* ssl_errstr = ERR_error_string(ssl_errno, buff);
                            printe("connect_ip_string:%s, fd:%d, rx_bytes:%d, ret:%d, ssl(%p), ssl_errno:%d, ssl_errstr:%s\n",dev->connect_ip_string, dev->fd, dev->rx_bytes, readb, dev->ssl, ssl_errno, ssl_errstr);
                            //  printe("connect_ip_string:%s, connect_port:%s\n",dev->connect_ip_string, dev->connect_port);
                            if (dev->enable_ssl) { modbusTCP_HW_free_SSL(dev); }
                            close(dev->fd);
                            dev->fd = -1;
                            dev->rx_bytes = 0;
                        }
                    }else{
                        printe("connect_ip_string:%s, fd:%d, rx_bytes:%d\n",dev->connect_ip_string, dev->fd, dev->rx_bytes);
                        if (dev->enable_ssl) { modbusTCP_HW_free_SSL(dev); }
                        close(dev->fd);
                        dev->fd = -1;
                        dev->rx_bytes = 0;
                    }
                    pthread_mutex_unlock(&dev->mutex);
            }
            // printline(dev->rx_buff, dev->rx_bytes, "modbusTCP(%d)->recv", dev->fd);
            return dev->rx_bytes;
        }
        pthread_mutex_unlock(&dev->mutex);
        dev->rx_bytes = 0;
        return -2;
    }
    return -1;
}

static void modbusTCP_HW_showRegs(modbusTCP_HW_t* dev)
{
    printv("\n.............\n");
    printv("GUN:2006, %04x\n", dev->regs_0x2000[6]);
    printv("GUN:212F, %04x\n", dev->regs_0x2100[0x2F]);
    printv("GUN:1105, %04x\n", dev->regs_0x1100[0x5]);
    printv("GUN:1106, %04x\n", dev->regs_0x1100[0x6]);
    printv("GUN:110B, %04x\n", dev->regs_0x1100[0xB]);
    printv("GUN:110D, %04x\n", dev->regs_0x1100[0xD]);
    printv("GUN:2115 :%04x %04x %04x %04x\n", dev->regs_0x2100[0x15], dev->regs_0x2100[0x16], dev->regs_0x2100[0x17], dev->regs_0x2100[0x18]);
    
    printv("\n.............\n");
    printv("GUN:2006, %04x\n", dev->regs_0x2000[6]);
    printv("GUN:2D2F, %04x\n", dev->regs_0x2D00[0x2F]);
    printv("GUN:1D05, %04x\n", dev->regs_0x1D00[0x5]);
    printv("GUN:1D06, %04x\n", dev->regs_0x1D00[0x6]);
    printv("GUN:1D0B, %04x\n", dev->regs_0x1D00[0xB]);
    printv("GUN:1D0D, %04x\n", dev->regs_0x1D00[0xD]);
    printv("GUN:2D15 :%04x %04x %04x %04x\n", dev->regs_0x2D00[0x15], dev->regs_0x2D00[0x16], dev->regs_0x2D00[0x17], dev->regs_0x2D00[0x18]);
    printv("\n");

    printv("ADDR:0x1015=%04x\n", dev->regs_0x1000[0x15]);
    printv("ADDR:0x1104=%04x, CCS1\n", dev->regs_0x1100[0x4]);
    printv("ADDR:0x110E=%04x\n", dev->regs_0x1100[0xE]);
    printv("ADDR:0x1D04=%04x, GBDC\n", dev->regs_0x1D00[0x4]);
    printv("ADDR:0x1D0E=%04x\n", dev->regs_0x1D00[0xE]);

    ccu_config_t *config = &ccu_config;
    printv("config->TCU_ID:%d\n", config->TCU_ID);
}

static int modbusTCP_HW_send(modbusTCP_HW_t* dev, uint8_t* buff, int bytes)
{
    int sendb;
    if (dev && dev->fd > 0)
    {
        if (bytes>0 && dev->fd > 0)
        {
            // printline(&buff[0], bytes, "modbusTCP(%d)->send", dev->fd);
            pthread_mutex_lock(&dev->mutex);
            if (dev->enable_ssl)
            {
                sendb = SSL_write(dev->ssl, &buff[0], bytes);
            }else{
                sendb = send(dev->fd, &buff[0], bytes, 0);
            }
            pthread_mutex_unlock(&dev->mutex);
            if (sendb>0)
            {
                return sendb;
            }
            printe("send(fd = %d)  error.\n", dev->fd);
            pthread_mutex_lock(&dev->mutex);
            if (dev->enable_ssl) { modbusTCP_HW_free_SSL(dev); }
            close(dev->fd);
            dev->fd = -1;
            pthread_mutex_unlock(&dev->mutex);
            return -3;
        }
        return -2;
    }
    return -1;
}

static int modbusTCP_HW_response_error(modbusTCP_HW_t* dev, uint8_t function, uint8_t errcode)
{
    if (dev)
    {
        dev->tx_bytes = 0;
        dev->tx_buff[0] = dev->rx_buff[0];//transaction
        dev->tx_buff[1] = dev->rx_buff[1];

        dev->tx_buff[2] = dev->rx_buff[2];//protocol
        dev->tx_buff[3] = dev->rx_buff[3];

        dev->tx_buff[4] = 0x0;//length
        dev->tx_buff[5] = 0x3;

        dev->tx_buff[6] = dev->rx_buff[6];//unit_id;
        dev->tx_buff[7] = function|0x80;//function;
        dev->tx_buff[8] = errcode;//errcode;
        dev->tx_bytes = 9;
    }
}


int modbusTCP_HW_report_registers(modbusTCP_HW_t* dev, uint16_t regaddr, uint16_t* regvalue, int N)
{
    int i, sendb;
    if (dev && dev->fd > 0)
    {
        dev->xx_bytes = 0;
        dev->seqnumb++;
        dev->xx_buff[dev->xx_bytes++] = (dev->seqnumb>>8)&0xFF;
        dev->xx_buff[dev->xx_bytes++] = (dev->seqnumb>>0)&0xFF;

        dev->xx_buff[dev->xx_bytes++] = 0x00;//protocol
        dev->xx_buff[dev->xx_bytes++] = 0x00;

        dev->xx_buff[dev->xx_bytes++] = 0x0;
        dev->xx_buff[dev->xx_bytes++] = 0xD;

        dev->xx_buff[dev->xx_bytes++] = 0x01; //unit_id;
        dev->xx_buff[dev->xx_bytes++] = 0x41; //function;
        dev->xx_buff[dev->xx_bytes++] = 0x91; //sub function;

        dev->xx_buff[dev->xx_bytes++] = 0x0;
        dev->xx_buff[dev->xx_bytes++] = 0x9; //length

        dev->xx_buff[dev->xx_bytes++] = 0x0; //no need ack

        dev->xx_buff[dev->xx_bytes++] = 0;
        dev->xx_buff[dev->xx_bytes++] = 1;  //numDev

        dev->xx_buff[dev->xx_bytes++] = 0;
        dev->xx_buff[dev->xx_bytes++] = 1;  //devAddr

        dev->xx_buff[dev->xx_bytes++] = 0;
        dev->xx_buff[dev->xx_bytes++] = 1;  //segments = 1

        dev->xx_buff[dev->xx_bytes++] = (regaddr>>8)&0xFF;
        dev->xx_buff[dev->xx_bytes++] = (regaddr>>0)&0xFF;

        dev->xx_buff[dev->xx_bytes++] = 0;
        dev->xx_buff[dev->xx_bytes++] = N;  //regnumb

        for (i=0; i<N; i++)
        {
            dev->xx_buff[dev->xx_bytes++] = (regvalue[i]>>8)&0xFF;
            dev->xx_buff[dev->xx_bytes++] = (regvalue[i]>>0)&0xFF;
            printi("ADDR[%04X]:%04X\n", regaddr+i, regvalue[i]);
        }

        dev->xx_buff[0x5] = dev->xx_bytes - 0x6;
        dev->xx_buff[0xA] = dev->xx_bytes - 0xB;

        

        modbusTCP_HW_send(dev, dev->xx_buff, dev->xx_bytes);
        return 0;
    }
    return -1;
}


static int modbusTCP_HW_read_holding_registers(modbusTCP_HW_t* dev)
{
    int i;
    uint16_t value;
    uint16_t regs_addr = (dev->rx_buff[8]<<8)|dev->rx_buff[9];
    uint16_t regs_numb = (dev->rx_buff[10]<<8)|dev->rx_buff[11];

    // printi("read-addr[%04x --- %04x] %04x regs.\n", regs_addr, regs_addr+regs_numb-1, regs_numb);

    dev->tx_buff[0] = dev->rx_buff[0];
    dev->tx_buff[1] = dev->rx_buff[1];// trans_id

    dev->tx_buff[2] = dev->rx_buff[2];
    dev->tx_buff[3] = dev->rx_buff[3];// protocol_id

    dev->tx_buff[4] = dev->rx_buff[4];
    dev->tx_buff[5] = dev->rx_buff[5];// length

    dev->tx_buff[6] = dev->rx_buff[6];// unit_id
    dev->tx_buff[7] = dev->rx_buff[7];// function

    dev->tx_buff[8] = dev->rx_buff[0];// 2*N bytes

    dev->tx_bytes = 9;

    for (i=0; i<regs_numb; i++)
    {
        uint16_t addr = regs_addr+i;
        uint16_t value = 0;

        switch(addr)
        {
            case 0x0000 ... 0x002A: value = dev->regs_0x0000[addr - 0x0000]; break;
            case 0x0100 ... 0x012A: value = dev->regs_0x0100[addr - 0x0100]; break;

            case 0x1000 ... 0x1025: value = dev->regs_0x1000[addr - 0x1000]; break;
            case 0x1100 ... 0x117F: value = dev->regs_0x1100[addr - 0x1100]; break;
            case 0x1D00 ... 0x1D7F: value = dev->regs_0x1D00[addr - 0x1D00]; break;
  
            case 0x2000 ... 0x2016: value = dev->regs_0x2000[addr - 0x2000]; break;
            case 0x2100 ... 0x212F: value = dev->regs_0x2100[addr - 0x2100]; break;
            case 0x2D00 ... 0x2D2F: value = dev->regs_0x2D00[addr - 0x2D00]; break;
            
            case 0x3000 ... 0x3004: value = dev->regs_0x3000[addr - 0x3000]; break;
            case 0x3100 ... 0x3128: value = dev->regs_0x3100[addr - 0x3100]; break;
            case 0x3D00 ... 0x3D28: value = dev->regs_0x3D00[addr - 0x3D00]; break;

            case 0x4000 ... 0x40FF: value = dev->regs_0x4000[addr - 0x4000]; break;
            //----------------


            case 0x1200 ... 0x129F: value = dev->regs_0x1100[addr - 0x1200]; break;
            case 0x1400 ... 0x147F: value = dev->regs_0x1100[addr - 0x1400]; break;
            case 0x1500 ... 0x157F: value = dev->regs_0x1100[addr - 0x1500]; break;
            case 0x2500 ... 0x252F: value = dev->regs_0x2100[addr - 0x2500]; break;
            case 0x2E00 ... 0x2E2F: value = dev->regs_0x2D00[addr - 0x2E00]; break;
            case 0x3200 ... 0x3215: value = dev->regs_0x3100[addr - 0x3200]; break;

            default:
            modbusTCP_HW_response_error(dev, dev->rx_buff[7], 0x2);//非法地址
            return -1;
        }
        dev->tx_buff[dev->tx_bytes++] = (value>>8)&0xFF;
        dev->tx_buff[dev->tx_bytes++] = (value>>0)&0xFF;
        // printi("read-addr[%04x] = %04x\n", addr, value);

        switch(addr)
        {
            // case 0x1113:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1114:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1105:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1106:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x110B:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x110D:printi("read-addr[%04x] = %04x\n", addr, value);break;

            // case 0x114D:printi("---read macA[%04x]-%04x %04x %04x\n", addr, dev->regs_0x1100[0x4D], dev->regs_0x1100[0x4E], dev->regs_0x1100[0x4F]);break;
            // case 0x1D4D:printi("---read macB[%04x]-%04x %04x %04x\n", addr, dev->regs_0x1D00[0x4D], dev->regs_0x1D00[0x4E], dev->regs_0x1D00[0x4F]);break;

            // case 0x1D13:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1D14:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1D05:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1D06:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1D0B:printi("read-addr[%04x] = %04x\n", addr, value);break;
            // case 0x1D0D:printi("read-addr[%04x] = %04x\n", addr, value);break;
        }
    }
    
    uint16_t length = dev->tx_bytes - 6;
    uint16_t nbytes = 2*regs_numb;
    dev->tx_buff[4] = (length>>8)&0xFF;
    dev->tx_buff[5] = (length>>0)&0xFF;
    dev->tx_buff[8] = (nbytes>>0)&0xFF;
    return 0;
}

extern void pcuhw_Update_GUNA_mac_addr(void);
extern void pcuhw_Update_GUNB_mac_addr(void);
extern void pcuhw_Update_GUNA_HMAC(void);
extern void pcuhw_Update_GUNB_HMAC(void);

static int modbusTCP_HW_write_single_register(modbusTCP_HW_t* dev)
{
    int i;
    uint16_t addri = (dev->rx_buff[8]<<8)|dev->rx_buff[9];
    uint16_t value = (dev->rx_buff[10]<<8)|dev->rx_buff[11];
    // printi("fill-addr[%04x] = %04x\n", addri, value);
    switch(addri)
    {
        case 0x0100 ... 0x012A: dev->regs_0x0100[addri - 0x0100] = value; break;     
        case 0x2006 ... 0x2015: dev->regs_0x2000[addri - 0x2000] = value; break;
        case 0x2100 ... 0x212F: dev->regs_0x2100[addri - 0x2100] = value; break;
        case 0x2D00 ... 0x2D2F: dev->regs_0x2D00[addri - 0x2D00] = value; break;
        case 0x4000 ... 0x40FF: dev->regs_0x4000[addri - 0x4000] = value; break;

        default: 
        modbusTCP_HW_response_error(dev, dev->rx_buff[7], 0x2);//非法地址
        return -1;
    }
    switch(addri)
    {
        // case 0x2006: printi("---fill-addr[%04x] = %04x\n", addri, value);break;
        // case 0x212F: printi("---fill-addr[%04x] = %04x\n", addri, value);break;
        // case 0x2D2F: printi("---fill-addr[%04x] = %04x\n", addri, value);break;

        case 0x2101: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[0]<<16)|dev->regs_0x2100[1]; };break;
        case 0x2103: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[2]<<16)|dev->regs_0x2100[3]; };break;
        case 0x2106: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[5]<<16)|dev->regs_0x2100[6]; };break;
        case 0x2108: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[7]<<16)|dev->regs_0x2100[8]; };break;
        case 0x210A: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[9]<<16)|dev->regs_0x2100[10]; };break;

        case 0x2D01: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[0]<<16)|dev->regs_0x2D00[1]; };break;
        case 0x2D03: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[2]<<16)|dev->regs_0x2D00[3]; };break;
        case 0x2D06: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[5]<<16)|dev->regs_0x2D00[6]; };break;
        case 0x2D08: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[7]<<16)|dev->regs_0x2D00[8]; };break;
        case 0x2D0A: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[9]<<16)|dev->regs_0x2D00[10]; };break;

        case 0x2113: pcuhw_Update_GUNA_mac_addr();break;
        case 0x212E: pcuhw_Update_GUNA_HMAC();break;

        case 0x2D13: pcuhw_Update_GUNB_mac_addr();break;
        case 0x2D2E: pcuhw_Update_GUNB_HMAC();break;
    }

    dev->tx_buff[0] = dev->rx_buff[0];
    dev->tx_buff[1] = dev->rx_buff[1];// trans_id

    dev->tx_buff[2] = dev->rx_buff[2];
    dev->tx_buff[3] = dev->rx_buff[3];// protocol_id

    dev->tx_buff[4] = 0;
    dev->tx_buff[5] = 6;// length

    dev->tx_buff[6] = dev->rx_buff[6];// unit_id
    dev->tx_buff[7] = dev->rx_buff[7];// function

    dev->tx_buff[8] = dev->rx_buff[8];
    dev->tx_buff[9] = dev->rx_buff[9];// regs_addr

    dev->tx_buff[10] = dev->rx_buff[10];
    dev->tx_buff[11] = dev->rx_buff[11];// regs_value

    dev->tx_bytes = 12;

    return 0;

}

static int modbusTCP_HW_write_multiple_registers(modbusTCP_HW_t* dev)
{
    int i, n=13;
    uint16_t addr = (dev->rx_buff[8]<<8)|dev->rx_buff[9];
    uint16_t numb = (dev->rx_buff[10]<<8)|dev->rx_buff[11];
    int N2 = dev->rx_buff[12];

    // printi("fill-addr[%04x --- %04x] %04x regs.\n", addr, addr + numb -1, numb);

    for (i=0; i<numb; i++)
    {
        uint16_t addri = addr + i;
        uint16_t value = (dev->rx_buff[n++]<<8)|dev->rx_buff[n++];
        // printi("fill-addr[%04x] = %04x\n", addri, value);
        switch(addri)
        {
            case 0x0100 ... 0x012A: dev->regs_0x0100[addri - 0x0100] = value; break;     
            case 0x2006 ... 0x2015: dev->regs_0x2000[addri - 0x2000] = value; break;
            case 0x2100 ... 0x212F: dev->regs_0x2100[addri - 0x2100] = value; break;
            case 0x2D00 ... 0x2D2F: dev->regs_0x2D00[addri - 0x2D00] = value; break;
            case 0x4000 ... 0x40FF: dev->regs_0x4000[addri - 0x4000] = value; break;

            default:
            modbusTCP_HW_response_error(dev, dev->rx_buff[7], 0x2);//非法地址
            return -1;
        }
        switch(addri)
        {
            // case 0x2006: printi("---fill-addr[%04x] = %04x\n", addri, value);break;
            // case 0x2115: printi("--------------HMAC\n");
            // case 0x2D15: printi("--------------HMAC\n");
            // case 0x212F: printi("---fill-addr[%04x] = %04x\n", addri, value);break;
            // case 0x2D2F: printi("---fill-addr[%04x] = %04x\n", addri, value);break;

            case 0x2101: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[0]<<16)|dev->regs_0x2100[1]; 
                // printi("--GUNA.maxV:%f\n", pvalue[0]);
            };break;
            case 0x2103: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[2]<<16)|dev->regs_0x2100[3]; 
                // printi("--GUNA.maxI:%f\n", pvalue[0]);
            };break;
            case 0x2106: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[5]<<16)|dev->regs_0x2100[6]; 
                // printi("--GUNA.minV:%f\n", pvalue[0]);
            };break;
            case 0x2108: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[7]<<16)|dev->regs_0x2100[8]; 
                // printi("--GUNA.minI:%f\n", pvalue[0]);
            };break;
            case 0x210A: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2100[9]<<16)|dev->regs_0x2100[10]; 
                // printi("--GUNA.fixedP.KW:%f\n", pvalue);
            };break;

            case 0x2D01: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[0]<<16)|dev->regs_0x2D00[1]; 
                // printi("--GUNB.maxV:%f\n", pvalue[0]);
            };break;
            case 0x2D03: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[2]<<16)|dev->regs_0x2D00[3]; 
                // printi("--GUNB.maxI:%f\n", pvalue[0]);
            };break;
            case 0x2D06: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[5]<<16)|dev->regs_0x2D00[6];
                // printi("--GUNB.minV:%f\n", pvalue[0]);
            };break;
            case 0x2D08: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[7]<<16)|dev->regs_0x2D00[8]; 
                // printi("--GUNB.minI:%f\n", pvalue[0]);
            };break;
            case 0x2D0A: { uint32_t value[2]; float* pvalue = (float*)&value[0]; value[0] = (dev->regs_0x2D00[9]<<16)|dev->regs_0x2D00[10]; 
                // printi("--GUNB.fixedP.KW:%f\n", pvalue[0]);
            };break;

            case 0x2113: pcuhw_Update_GUNA_mac_addr();break;
            case 0x212C: pcuhw_Update_GUNA_HMAC();break;

            case 0x2D13: pcuhw_Update_GUNB_mac_addr();break;
            case 0x2D2C: pcuhw_Update_GUNB_HMAC();break;
        }
    }
    

    dev->tx_buff[0] = dev->rx_buff[0];
    dev->tx_buff[1] = dev->rx_buff[1];// trans_id

    dev->tx_buff[2] = dev->rx_buff[2];
    dev->tx_buff[3] = dev->rx_buff[3];// protocol_id

    dev->tx_buff[4] = 0;
    dev->tx_buff[5] = 6;// length

    dev->tx_buff[6] = dev->rx_buff[6];// unit_id
    dev->tx_buff[7] = dev->rx_buff[7];// function

    dev->tx_buff[8] = dev->rx_buff[8];
    dev->tx_buff[9] = dev->rx_buff[9];// regs_addr

    dev->tx_buff[10] = (numb>>8)&0xFF;
    dev->tx_buff[11] = (numb>>0)&0xFF;

    dev->tx_bytes = 12;

    return 0;
}


void* modbusTCP_HW_loop(void* argv)
{
    modbusTCP_HW_t* dev = (modbusTCP_HW_t*)argv;

    if (dev)
    {
        printd("step, dev:%p\n", dev);
        modbusTCP_HW_report_registers(dev, 0x0000, &dev->regs_0x0000[0x00], 3);
        modbusTCP_HW_report_registers(dev, 0x114D, &dev->regs_0x1100[0x4D], 3);
        modbusTCP_HW_report_registers(dev, 0x1D4D, &dev->regs_0x1D00[0x4D], 3);
        while(dev->quit == 0)
        {
            goose_HW_open_socket(&dev->gooseA);
            goose_HW_open_socket(&dev->gooseB);

            goose_HW_loop(&dev->gooseA);
            goose_HW_loop(&dev->gooseB);

            modbusTCP_HW_connect(dev);
            modbusTCP_HW_recv_with_timeout(dev, 1000);

            while(dev->rx_bytes >= 11)
            {
                uint16_t transaction_id =  (dev->rx_buff[0]<<8)|dev->rx_buff[1];
                uint16_t protocol_id = (dev->rx_buff[2]<<8)|dev->rx_buff[3];
                uint16_t length = (dev->rx_buff[4]<<8)|dev->rx_buff[5];
                uint16_t addri = (dev->rx_buff[8]<<8)|dev->rx_buff[9];
                uint16_t value = (dev->rx_buff[10]<<8)|dev->rx_buff[11];


                if ((dev->trans_ids[0]==0) && (dev->trans_ids[1]==0))
                {
                    dev->trans_ids[0] = transaction_id - 1;
                    dev->trans_ids[1] = transaction_id;
                }else{
                    dev->trans_ids[0] = dev->trans_ids[1];
                    dev->trans_ids[1] = transaction_id;
                }

                if (protocol_id == 0x0000 && (dev->rx_buff[7]==0x03||dev->rx_buff[7]==0x06||dev->rx_buff[7]==0x10||dev->rx_buff[7]==0x41||dev->rx_buff[7]==0x67))
                {
                    // dev->seqnumb = transaction_id;
                    length += 6;
                    if (length <= dev->rx_bytes)
                    {
                        // pthread_mutex_lock(&dev->mutex);
                        switch(dev->rx_buff[7])
                        {
                            case 0x03:modbusTCP_HW_read_holding_registers(dev); break;
                            case 0x06:modbusTCP_HW_write_single_register(dev); break;
                            case 0x10:modbusTCP_HW_write_multiple_registers(dev); break;
                            default:
                            modbusTCP_HW_response_error(dev, dev->rx_buff[7], 0x1);//非法功能
                            // printe("unknow function:%02x, unit_id:%02x, length:%d, rx_btyes:%d\n",dev->rx_buff[7], dev->rx_buff[6], length, dev->rx_bytes);
                            break;
                        }
                        memmove(&dev->rx_buff[0], &dev->rx_buff[length], dev->rx_bytes - length);
                        dev->rx_bytes -= length;
                        modbusTCP_HW_send(dev, dev->tx_buff, dev->tx_bytes);
                        // pthread_mutex_unlock(&dev->mutex);
                    }
                    //printi("TID:%04x, PID:%04x, ID:%04x %04x, length:%d, addr:%04x, value:%04x\n", transaction_id, protocol_id, dev->trans_ids[0], dev->trans_ids[1], length, addri, value);
                }else{
                    // printi("TID:%4x, PID:%04x\n", transaction_id, protocol_id);
                    memmove(&dev->rx_buff[0], &dev->rx_buff[1], dev->rx_bytes-1);
                    dev->rx_bytes--;
                }
            }
        }
    }
    return NULL;
}

int modbusTCP_HW_init(modbusTCP_HW_t* dev)
{
    if (dev)
    {
        uint8_t buff[128];
        ccu_config_t *config = &ccu_config;
        memset(dev, 0, sizeof(modbusTCP_HW_t));

        dev->enable_ssl = config->pcuhw.enable_ssl;
        sprintf(dev->connect_ip_string, "%s", config->pcuhw.connect_ip_string);// "192.168.11.1";
        sprintf(dev->connect_port, "%d", config->pcuhw.connect_port);//"502";
        dev->fd = -1;

        dev->trans_ids[0] = 0;
        dev->trans_ids[1] = 0;
        pthread_mutex_init(&dev->mutex, NULL);
        dev->quit = 0;

        dev->regs_0x0000[0] = 0x2;    //生产厂家编码
        dev->regs_0x0000[1] = 0x80;   //充电终端
        dev->regs_0x0000[2] = 0x100;  //协议版本
        dev->regs_0x0000[4] = 0x4142; //硬件版本
        dump_U8String_To_U16String(&dev->regs_0x0000[0x13], "SW20241128", 10);//软件版本

        dev->regs_0x1000[0x15] = 2;   //充电控制板可管理的充电枪数量
        sprintf(buff, "TC20241120-ASTXH%2d", config->TCU_ID);//终端唯一标识ID
        dump_U8String_To_U16String(&dev->regs_0x1000[0x16], buff, 18);

        dev->regs_0x1100[0x4] = 0x4; //CCS1 GUN
        dev->regs_0x1100[0xE] = config->TCU_ID*2 + 1;
        dev->regs_0x1100[0xB] = 0;
        dev->regs_0x1100[0xD] = 0;
        dev->regs_0x2100[0x2F] = 1;//充电端口可用状态
        dev->regs_0x2100[0x2F] = 1000;

        dev->regs_0x1D00[0x4] = 0x4; //GBDC GUN
        dev->regs_0x1D00[0xE] = config->TCU_ID*2 + 2;
        dev->regs_0x1D00[0xB] = 0;
        dev->regs_0x1D00[0xD] = 0;
        dev->regs_0x2D00[0x2F] = 1;//充电端口可用状态

        modbusTCP_HW_showRegs(dev);

        // SSL_library_init();
        // OpenSSL_add_all_algorithms();
        // SSL_load_error_strings();
        // CRYPTO_thread_setup();
        
        goose_HW_init(&dev->gooseA, "eth0", config->TCU_ID*2+1);
        goose_HW_init(&dev->gooseB, "eth0", config->TCU_ID*2+2);

        return modbusTCP_HW_connect(dev);

    }
    return -1;
}

void modbusTCP_HW_exit(modbusTCP_HW_t* dev)
{
    if (dev)
    {
        goose_HW_exit(&dev->gooseA);
        goose_HW_exit(&dev->gooseB);
        modbusTCP_HW_free_SSL(dev);
        close(dev->fd);
        dev->fd = -1;
        pthread_mutex_destroy(&dev->mutex);
        // CRYPTO_thread_cleanup();
    }
}


