csu3_am335x/EVSE/Projects/DD360Audi/Apps/ModuleDoComm/DoComm.c

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdarg.h>
#include <string.h>
#include <stdbool.h>

#include <sys/time.h>
#include <sys/timeb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>

#include <arpa/inet.h>
#include <netinet/in.h>

#include <assert.h>
#include <time.h>
#include <errno.h>
#include <unistd.h>
#include <signal.h>

#include "../Log/log.h"
#include "../ShareMemory/shmMem.h"
#include "../Define/define.h"
#include "../Config.h"
#include "../SelectGun/SelectGun.h"
#include "../timeout.h"
#include "DoComm.h"

//------------------------------------------------------------------------------
//#define log_info(format, args...) StoreLogMsg("[%s:%d][%s][Info] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
//#define log_warn(format, args...) StoreLogMsg("[%s:%d][%s][Warn] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
//#define log_error(format, args...) StoreLogMsg("[%s:%d][%s][Error] "format, __FILE__, __LINE__, __FUNCTION__, ##args)

//------------------------------------------------------------------------------
static DoCommGblData gDoCommGblData             = {0};

static struct SysConfigData *pSysConfig         = NULL;
static struct SysInfoData *pSysInfo             = NULL;
static struct WARNING_CODE_INFO *pSysWarning    = NULL;
static struct AlarmCodeData *pAlarmCode         = NULL;
static struct PsuData *ShmPsuData               = NULL;
static struct OCPP16Data *ShmOCPP16Data         = NULL;
static struct PrimaryMcuData *ShmPrimaryMcuData = NULL;
static SelectGunInfo *ShmSelectGunInfo          = NULL;
static DcCommonInfo *ShmDcCommonData            = NULL;

//static struct ChargingInfoData  *ChargingData[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY]
static struct ChargingInfoData *pDcChargingInfo = NULL;
static struct timeb             gRegTimeUp[2][MAX_REGISTER_NUM] = {0};
static struct WARNING_CODE_INFO gPreSysWarningInfo = {0};

//------------------------------------------------------------------------------
static void removeFaultCodeToBuf(uint8_t *Code);
static void addFaultCodeToBuf(uint8_t *Code);
static int readMiscCommand(int fd, uint8_t id);
static int writeCsuModuleVersion(int fd);
static int writeGroundFaultDetection(int fd, uint8_t status, uint8_t id);

//------------------------------------------------------------------------------
//--- Common function ---
//------------------------------------------------------------------------------
void GetClockTime(struct timespec *_now_time, void *null)
{
    clock_gettime(CLOCK_MONOTONIC, _now_time);
}

/*static int StoreLogMsg(const char *fmt, ...)
{
    char Buf[4096 + 256];
    char buffer[4096];
    va_list args;
    struct timeb  SeqEndTime;
    struct tm *tm;

    va_start(args, fmt);
    int rc = vsnprintf(buffer, sizeof(buffer), fmt, args);
    va_end(args);

    memset(Buf, 0, sizeof(Buf));
    ftime(&SeqEndTime);
    SeqEndTime.time = time(NULL);
    tm = localtime(&SeqEndTime.time);

    if (pSysConfig->SwitchDebugFlag == 1) {
        sprintf(Buf, "%02d:%02d:%02d:%03d - %s",
                tm->tm_hour, tm->tm_min, tm->tm_sec, SeqEndTime.millitm, buffer);
        printf("%s \n", Buf);
    } else {
        sprintf(Buf, "echo \"%04d-%02d-%02d %02d:%02d:%02d:%03d - %s\" >> /Storage/SystemLog/[%04d.%02d]SystemLog",
                tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, SeqEndTime.millitm,
                buffer,
                tm->tm_year + 1900, tm->tm_mon + 1);
        system(Buf);
    }

    return rc;
}
*/

static int DiffTimeb(struct timeb ST, struct timeb ET)
{
    //return milli-second
    unsigned int StartTime, StopTime;

    StartTime = (unsigned int)ST.time;
    StopTime = (unsigned int)ET.time;

    return (StopTime - StartTime) * 1000 + ET.millitm - ST.millitm;
}

/**
 * [hexdump : check data]
 * @Author   Jerry
 * @DateTime 2018-12-21
 * @param    p          [description]
 * @param    size       [description]
 */
static void Hexdump(const void *p, size_t size)
{
    const uint8_t *c = p;
    char message[10240] = {0};
    uint32_t i = 0;
    uint32_t message_len = 0;

    assert(p);

    //printf("Dumping %u bytes from %p:\r\n", size, p);
    message_len += sprintf(&message[message_len], "\r\nDumping %u bytes from %p:\r\n",  (unsigned int)size, p);

    while (size > 0) {
        for (i = 0; i < 16; i++) {
            if (i < size) {
                //printf("%02x ", c[i]);
                message_len += sprintf(&message[message_len], "%02x ", c[i]);
            } else {
                //printf("   ");
                message_len += sprintf(&message[message_len], "   ");
            }
        }

        for (i = 0; i < 16; i++) {
            if (i < size) {
                //printf("%c", c[i] >= 32 && c[i] < 127 ? c[i] : '.');
                message_len += sprintf(&message[message_len], "%c", c[i] >= 32 && c[i] < 127 ? c[i] : '.');
            } else {
                //printf(" ");
                message_len += sprintf(&message[message_len], " ");
            }
        }

        //printf("\n");
        message_len += sprintf(&message[message_len], "\r\n");
        c += 16;

        if (size <= 16) {
            break;
        }

        size -= 16;
    }

    message_len += sprintf(&message[message_len], "\r\n");

    log_info("%s", message);
}

static int string2ByteArray(char *input, uint8_t *output)
{
    int loop = 0;
    int i = 0;

    while (input[loop] != '\0') {
        output[i++] = input[loop++];
    }
    output[loop] = '\0';

    return loop + 1;
}

static void unixSocketSigPipeHandle(int sig)
{
    log_error("socket packet error %x\r\n", sig);
}

static void InitSocketSigPipe(void)
{
    struct sigaction action;

    action.sa_handler = unixSocketSigPipeHandle;
    sigemptyset(&action.sa_mask);
    action.sa_flags = 0;
    sigaction(SIGPIPE, &action, NULL);
}

//------------------------------------------------------------------------------
static void setTcpStatus(uint8_t setValue)
{
    pAlarmCode->AlarmEvents.bits.DisconnectedFromDo = setValue;
}

//------------------------------------------------------------------------------
//--- TCP socket function ---
//------------------------------------------------------------------------------
static int sendTcpSocket(int fd, uint8_t *data, uint16_t dataLen)
{
    int size = -1;

    size = send(fd, data, dataLen , 0);
    if ((size < 0) || (errno == EAGAIN)) {
        if (size < 0) {
            log_error("Size = %d, EAGAIN error %d:%s\r\n", size, errno, strerror(errno));
        }
    }

    return size;
}

static int recvTcpSocket(int fd, uint8_t *data, uint16_t dataLen)
{
    int size = -1;
    uint8_t *pdata = (uint8_t *)data;

    size = recv(fd, pdata, dataLen, MSG_WAITALL);
    if ((errno == EAGAIN) || (size < 0)) {
        log_error("Size = %d, EAGAIN error %d:%s\r\n", size, errno, strerror(errno));
    }

    return size;
}

static int getSO_ERROR(int fd)
{
    int err = 1;
    socklen_t len = sizeof err;

    if (-1 == getsockopt(fd, SOL_SOCKET, SO_ERROR, (char *)&err, &len)) {
        log_error("getSO_ERROR\r\n");
    }

    if (err) {
        errno = err;        // set errno to the socket SO_ERROR
    }

    return err;
}

static void closeSocket(int fd)        // *not* the Windows closesocket()
{
    if (fd < 0) {
        return;
    }

    getSO_ERROR(fd); // first clear any errors, which can cause close to fail
    if (shutdown(fd, SHUT_RDWR) < 0) { // secondly, terminate the 'reliable' delivery
        if (errno != ENOTCONN && errno != EINVAL) { // SGI causes EINVAL
            log_info("shutdown\r\n");
        }
    }

    if (close(fd) < 0) { // finally call close()
        log_info("client socket close\r\n");
    }
}

static int doCommConnToServer(void)
{
    struct sockaddr_in dest;
    struct timeval tv;
    int flag;
    int TcpSock = 0;

    //if (TcpSock > 0) {
    //    close(TcpSock);
    //}

    if ((TcpSock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
        log_error("Open TCP socket NG");
        return -1;
    }

    //flag = fcntl (TcpSock, F_GETFL, 0);
    //if (flag >= 0) {
    //    flag |= O_NONBLOCK;
    //    fcntl(TcpSock, F_SETFL, flag );
    //}

    tv.tv_sec = 3;
    tv.tv_usec = 0;
    setsockopt(TcpSock, SOL_SOCKET,  SO_RCVTIMEO, &tv, sizeof(struct timeval)); //設定等待3s
    setsockopt(TcpSock, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(struct timeval));  //設定傳送3s
    flag = 1;
    setsockopt(TcpSock, SOL_SOCKET, MSG_NOSIGNAL, &flag, sizeof(flag));

    memset(&dest, 0, sizeof(dest));
    dest.sin_family = AF_INET;
    dest.sin_port = htons(DoTcpPort);
    inet_aton(DoIPAddress, (struct in_addr *) &dest.sin_addr.s_addr);

    if (connect(TcpSock, (struct sockaddr *) &dest, sizeof(dest)) != 0) {
        close(TcpSock);
        return -1;
    }

    return TcpSock;
}

//------------------------------------------------------------------------------
//--- Audi select gun ---
//------------------------------------------------------------------------------
static void clearPricesInfo(uint8_t id)
{
    memset(&ShmSelectGunInfo->PricesInfo[id], 0, sizeof(PricesInfo));
    ShmSelectGunInfo->PricesInfo[id].Balance = FAIL_BALANCE_PRICES;
}

static void ClearAuthorizedFlag(void)
{
    ShmOCPP16Data->SpMsg.bits.AuthorizeConf = NO;
    pSysInfo->AuthorizeFlag = NO;
}

static void ClearDetectPluginFlag(void)
{
    pSysInfo->WaitForPlugit = NO;
}

static bool isDetectPlugin(void)
{
    if (pSysInfo->WaitForPlugit == YES) {
        return YES;
    }

    return NO;
}

static void destroySelectGun(uint8_t curGun)
{
    uint8_t i = 0;
    uint8_t totalGun = pSysConfig->TotalConnectorCount;

    if (curGun == DESTROY_ALL_SEL) {
        ShmSelectGunInfo->SelGunInfo.RightGun = SEL_GUN_RELEASE;
        ShmSelectGunInfo->SelGunInfo.LeftGun = SEL_GUN_RELEASE;

        for (i = 0; i < totalGun; i++) {
            pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(i);

            pDcChargingInfo->TimeoutFlag = Timeout_None;
            clearPricesInfo(i);
        }
        pSysInfo->CurGunSelected = 0;
        strcpy((char *)pSysConfig->UserId, "");
    }

    //for charging timeout or complete
    if ((curGun == LEFT_GUN_NUM) && (ShmSelectGunInfo->SelGunInfo.LeftGun != SEL_GUN_RELEASE)) {
        ShmSelectGunInfo->SelGunInfo.LeftGun = SEL_GUN_RELEASE;
        if (ShmOCPP16Data->SpMsg.bits.AuthorizeConf != NO) {
            ClearAuthorizedFlag();
        }
        clearPricesInfo(curGun);
    }

    if ((curGun == RIGHT_GUN_NUM) && (ShmSelectGunInfo->SelGunInfo.RightGun != SEL_GUN_RELEASE)) {
        if (ShmOCPP16Data->SpMsg.bits.AuthorizeConf != NO) {
            ClearAuthorizedFlag();
        }

        clearPricesInfo(curGun);
    }
}

static int getConfirmSelectedGun(uint8_t curSel)
{
    if (((curSel == LEFT_GUN_NUM) && (ShmSelectGunInfo->SelGunInfo.LeftGun >= SEL_GUN_CONFIRM)) ||
            ((curSel == RIGHT_GUN_NUM) && (ShmSelectGunInfo->SelGunInfo.RightGun >= SEL_GUN_CONFIRM))) {
        return PASS;
    }

    return FAIL;
}

static int getSelGunWaitToAuthor(uint8_t curGun)
{
    if (curGun == LEFT_GUN_NUM && ShmSelectGunInfo->SelGunInfo.LeftGun == SEL_GUN_ATHOR) {
        return FAIL;
    }

    if (curGun == RIGHT_GUN_NUM && ShmSelectGunInfo->SelGunInfo.RightGun == SEL_GUN_ATHOR) {
        return FAIL;
    }

    return PASS;
}

static void setConfirmSelGun(uint8_t selGun)
{
    if (selGun == LEFT_GUN_NUM && ShmSelectGunInfo->SelGunInfo.LeftGun == SEL_GUN_RELEASE) {
        ShmSelectGunInfo->SelGunInfo.LeftGun = SEL_GUN_CONFIRM;
        //printf("confirmSelGun left\r\n");
    } else if (selGun == RIGHT_GUN_NUM && ShmSelectGunInfo->SelGunInfo.RightGun == SEL_GUN_RELEASE) {
        ShmSelectGunInfo->SelGunInfo.RightGun = SEL_GUN_CONFIRM;
        //printf("confirmSelGun right\r\n");
    }
}

//------------------------------------------------------------------------------
//--- DoComm function ---
//------------------------------------------------------------------------------
static int compareOpcode(uint8_t opCode)
{
    if (opCode != OP_WAIT_RESPONSE) {
        //log_error("response operative code fail\r\n");
        return FAIL;
    }

    return PASS;
}

static int compareResult(uint8_t result)
{
    if (result != COMMAND_RESULT_OK) {
        //log_error("response result fail\r\n");
        return FAIL;
    }

    return PASS;
}

static int compareRegister(uint8_t srcReg, uint8_t destReg)
{
    if (srcReg != destReg) {
        //log_error("response register fail\r\n");
        return FAIL;
    }

    return PASS;
}

static float transPricesUnit(int prices)
{
    //printf("prices = %.2f\r\n", prices * PRICES_UNIT);
    return (prices * PRICES_UNIT);
}

static void clearMiscCommand(void)
{
    gDoCommGblData.MiscCmd = 0;
}

static int qrCodeUrlInfoHandle(uint8_t *data)
{
    //int len = 0;
    //char cmdBuf[128] = {0};
    char localTime[128] = {0};
    uint16_t timeLen = 0;
    struct timeb SeqEndTime;
    struct tm *tm;

    if ((char *)&data[0] == '\0') {
        log_error("QR code date error\r\n");
        return FAIL;
    }

    //get local system time
    ftime(&SeqEndTime);
    SeqEndTime.time = time(NULL);
    tm = localtime(&SeqEndTime.time);
    timeLen = sprintf(localTime, "%04d-%02d-%02d %02d:%02d",
                      tm->tm_year + 1900,
                      tm->tm_mon + 1,
                      tm->tm_mday,
                      tm->tm_hour,
                      tm->tm_min);

    //copy QR code string
    if (strncmp((char *)localTime, (char *)&data[0], timeLen - 2) != 0) {
        memset(pSysConfig->SystemId, '\0', sizeof(pSysConfig->SystemId));
        //printf("data =  %s\r\n", data);
        //len =
        string2ByteArray((char *)data, (uint8_t *)pSysConfig->SystemId);
        //printf("SystemId =  %s\r\n", pSysConfig->SystemId);
    }

    //if ((char *)&data[len] == '\0') {
    //    log_error("power cabinet system date error\r\n");
    //    return FAIL;
    //}

    //set system date
    //if (strncmp((char *)localTime, (char *)&data[len], timeLen) != 0) {
    //    sprintf(cmdBuf, "date -s \"%s\" >> /dev/null", (char *)&data[len]);
    //    system(cmdBuf);
    //    log_info("local time = %s, data time = %s\r\n", localTime, (char *)&data[len]);
    //    ShmOCPP16Data->MsMsg.bits.ResetReq = YES;
    //    strcpy((char *)ShmOCPP16Data->Reset.Type, "Soft");
    //    sleep(3);
    //    //gDoCommGblData.DisConnCount = CHECK_NETWORK_FAIL_COUNT;
    //}

    return PASS;
}

static int updateFirmwareHandle(uint8_t *imgName)
{
    int ret = PASS;
    char cmdBuf[1024] = {0};

    sprintf(cmdBuf, "/mnt/%s", imgName);
    log_info("Program ready to check file %s\n", cmdBuf);
    if ( access(cmdBuf, F_OK) != -1) {
        log_info("File '%s' exist.\n", cmdBuf);
        pSysInfo->FirmwareUpdate = YES;
    } else {
        log_info("File '%s' doesn't exist.\n", cmdBuf);
        ret = FAIL;
    }

#if 0
    char cmdBuf[1024] = {0};
    int status = 0;

    system("touch ./tftpUpdate.sh"); //創建shell
    sprintf(cmdBuf, "echo tftp -gr %s -l %s/%s %s; > ./tftpUpdate.sh",
            imgName,
            IMAGE_FILE_PATH,
            imgName,
            DoIPAddress);
    system("chmod +x ./tftpUpdate.sh"); //修改權限

    status = system("sh tftpUpdate.sh"); //執行shell
    if (-1 == status) {
        printf("system error!");
    } else {
        printf("exit status value = [0x%x]\n", status);

        if (WIFEXITED(status)) {
            if (0 == WEXITSTATUS(status)) {
                printf("run shell script successfully.\n");
                pSysInfo->FirmwareUpdate = YES;
            } else {
                printf("run shell script fail, script exit code: %d\n", WEXITSTATUS(status));
            }
        } else {
            printf("exit status = [%d]\n", WEXITSTATUS(status));
        }
    }

    system("rm -rf ./tftpUpdate.sh"); //刪除shell
#endif //0

    return ret;
}

static int miscCommandHandle(uint8_t dataLen, uint8_t plugNum, uint8_t *data)
{
    int ret = PASS;
    uint8_t i = 0, j = 0;
    uint8_t cmdCount = (dataLen / 6);
    uint16_t cmd = 0;
    uint32_t value = 0;
    float prices = 0;
    MiscCommand *pMiscCmd = NULL;

    if (cmdCount < 1) {
        gDoCommGblData.MiscCmd = 0;
        //printf("cmdCount fail = %d, data len = %d\r\n", cmdCount, dataLen);
        return FAIL;
    }

    for (i = 0; i < cmdCount; i++) {
        pMiscCmd = (MiscCommand *)data + i;
        cmd = ntohs(pMiscCmd->CMD);
        value = ntohl(*((uint32_t *)&pMiscCmd->Value[0]));
        //log_info("misc command = %x, value = %d\r\n", cmd, value);

        switch (cmd) {
        //--- Execute parameter ---
        case MISC_CMD_CONNECOTOR_TIMEOUT:
            ShmSelectGunInfo->RemoteSetup.ConnectionTimeout = value;
            log_info("connection timeout = %d\r\n", ShmSelectGunInfo->RemoteSetup.ConnectionTimeout);
            clearMiscCommand();
            break;

        case MISC_CMD_OPERATIVE_STATE:
            if ((value > YES) || (value < NO)) {
                if (plugNum == 1) {
                    clearMiscCommand();
                }
                break;
            }

            log_info("change availability plugNum = %d, value = %d\r\n", plugNum, value);
            ShmOCPP16Data->CsMsg.bits[plugNum].ChangeAvailabilityReq = YES;
            if (value == YES) {
                strcpy((char *)ShmOCPP16Data->ChangeAvailability[plugNum].Type, "Operative");
            } else if (value == NO) {
                strcpy((char *)ShmOCPP16Data->ChangeAvailability[plugNum].Type, "Inoperative");
            }

            if (plugNum == 1) {
                clearMiscCommand();
            }
            break;

        case MISC_CMD_DEFAULT_PRICES:
            prices = transPricesUnit((int)value);
            log_info("default prices = %.2f\r\n", prices);

            //pSysConfig->BillingData.isBilling = YES;
            for (j = 0; j < 24; j++) {
                pSysConfig->BillingData.Fee[j] = prices;
            }

            clearMiscCommand();
            break;

        case MISC_CMD_DEFAULT_CURRENCY:
            if (value < 0) {
                clearMiscCommand();
                return FAIL;
            }

            //log_info("default currency = %s%c\r\n", (uint8_t *)Currency[value]);
            log_info("default currency = %s\r\n", (uint8_t *)GetCurrency(value));
            pSysConfig->BillingData.Currency = value;
            clearMiscCommand();
            break;

        case MISC_CMD_ACCOUNT_BALANCE:
            if (pSysInfo->CurGunSelected != (plugNum)) {
                clearMiscCommand();
                break;
            }

#if defined DD360Audi
            if (getConfirmSelectedGun(plugNum) == FAIL) {
                log_info("Remote start not select gun\r\n");
                break;
            }
#endif //DD360Audi

            ShmSelectGunInfo->PricesInfo[plugNum].Balance = transPricesUnit((int)value);
            log_info("%d misc balance = %.2f\r\n", plugNum, ShmSelectGunInfo->PricesInfo[plugNum].Balance);
            clearMiscCommand();
            break;

        case MISC_CMD_BACKEND_STATUS :
            ShmSelectGunInfo->EthDevStatus.Backend = value;
            break;

        case MISC_CMD_ETHERNET_STATUS :
            ShmSelectGunInfo->EthDevStatus.Ethernet = value;
            break;

        case MISC_CMD_WIFI_STATUS :
            ShmSelectGunInfo->EthDevStatus.Wifi = value;
            break;

        case MISC_CMD_4G_STATUS :
            ShmSelectGunInfo->EthDevStatus.FourG = value;
            break;

        case MISC_CMD_BILLING_INFO:
            pSysConfig->BillingData.isBilling = value;
            break;

        case MISC_CMD_WEB_STOP_CHARGING:
            pSysConfig->StopChargingByButton = value;
            break;

        //--- Control Dispenser ---
        case MISC_CMD_HARDWARE_REBOOT:
            if (value != YES) {
                clearMiscCommand();
                break;
            }

            log_info("Hardware reboot\r\n");
            ShmOCPP16Data->MsMsg.bits.ResetReq = YES;
            strcpy((char *)ShmOCPP16Data->Reset.Type, "Hard");
            clearMiscCommand();
            break;

        case MISC_CMD_SOFTWARE_RESTART:
            if (value != YES) {
                clearMiscCommand();
                break;
            }

            clearMiscCommand();
            log_info("Software reboot\r\n");
            ShmOCPP16Data->MsMsg.bits.ResetReq = YES;
            strcpy((char *)ShmOCPP16Data->Reset.Type, "Soft");
            break;

        case MISC_CMD_REMOTE_START_CHARGING:
            if (value != YES) {
                if (plugNum == 1) {
                    clearMiscCommand();
                }
                break;
            }
            log_info("Remote start charging plugNum = %d\r\n", plugNum);

#if defined DD360Audi
            if (getSelGunWaitToAuthor(plugNum) == FAIL) {
                log_error("Remote start gun already charging\r\n");
                break;
            }

            if (pSysInfo->CurGunSelected != (plugNum)) {
                if (plugNum == LEFT_GUN_NUM &&
                        (ShmSelectGunInfo->SelGunInfo.LeftGun == SEL_GUN_CONFIRM ||
                         ShmSelectGunInfo->SelGunInfo.LeftGun == SEL_GUN_ATHOR)) {
                    ShmSelectGunInfo->SelGunInfo.LeftGun = SEL_GUN_RELEASE;
                    strcpy((char *)pSysConfig->UserId, "");
                    pSysInfo->WaitForPlugit = NO;
                    sleep(1); //Jerry add
                    pSysInfo->SystemPage = _LCM_SELECT_GUN;
                    GetClockTime(&pSysInfo->SystemTimeoutTimer, NULL);
                    pSysInfo->SystemTimeoutFlag = Timeout_None;
                }

                if (plugNum == RIGHT_GUN_NUM &&
                        (ShmSelectGunInfo->SelGunInfo.RightGun == SEL_GUN_CONFIRM ||
                         ShmSelectGunInfo->SelGunInfo.RightGun == SEL_GUN_ATHOR)) {
                    ShmSelectGunInfo->SelGunInfo.RightGun = SEL_GUN_RELEASE;
                    strcpy((char *)pSysConfig->UserId, "");
                    pSysInfo->WaitForPlugit = NO;
                    sleep(1); //Jerry add
                    pSysInfo->SystemPage = _LCM_SELECT_GUN;
                    GetClockTime(&pSysInfo->SystemTimeoutTimer, NULL);
                    pSysInfo->SystemTimeoutFlag = Timeout_None;
                }

                pSysInfo->CurGunSelected = (plugNum);
                pSysInfo->CurGunSelectedByAc = NO_DEFINE;
            }
#else
            pSysInfo->CurGunSelected = (plugNum);
            pSysInfo->CurGunSelectedByAc = NO_DEFINE;
#endif //DD360Audi
            setConfirmSelGun(pSysInfo->CurGunSelected);

            ShmOCPP16Data->CsMsg.bits[plugNum].RemoteStartTransactionReq = YES;
            ShmSelectGunInfo->PricesInfo[plugNum].Balance = 0.00;
            clearMiscCommand();
            break;

        case MISC_CMD_REMOTE_STOP_CHARGING:
            if (value != YES) {
                if (plugNum == 1) {
                    clearMiscCommand();
                }
                break;
            }

            strcpy((char *)pSysConfig->UserId, "");
            pSysInfo->WaitForPlugit = NO;
            pSysInfo->SystemPage = _LCM_SELECT_GUN;
            GetClockTime(&pSysInfo->SystemTimeoutTimer, NULL);
            pSysInfo->SystemTimeoutFlag = Timeout_None;
            destroySelectGun(plugNum);

            clearMiscCommand();
            break;

        case MISC_CMD_REMOTE_UNLOCK:
            if (value != YES) {
                if (plugNum == 1) {
                    clearMiscCommand();
                }
                break;
            }

            if (isDetectPlugin() == YES) {
                ClearDetectPluginFlag();
                strcpy((char *)pSysConfig->UserId, "");
                pSysInfo->WaitForPlugit = NO;
                pSysInfo->SystemPage = _LCM_SELECT_GUN;
                GetClockTime(&pSysInfo->SystemTimeoutTimer, NULL);
                pSysInfo->SystemTimeoutFlag = Timeout_None;
                destroySelectGun(plugNum);
            } else {
                pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

                pDcChargingInfo->StopChargeFlag = POWER_CABINET_STOP_CHARGING;
            }
            break;

        default:
            clearMiscCommand();
            break;
        }
        usleep(128);
    }

    return ret;
}

static int chargingcapabilityHandle(uint8_t *data, uint8_t plugNum)
{
    uint8_t addr = 0;
    float MaxVolt, MaxCurrent, MaxPower;
    static PricesInfo pricesInfo[2] = {0};
    CapabilityInfo *pCapabilityInfo = NULL;
    AccountInfo *pAccountInfo = NULL;
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    //--- charging capability information --------------------------------------
    pCapabilityInfo = (CapabilityInfo *)&data[0];

    MaxVolt = (float)(ntohs(pCapabilityInfo->OutputVoltage));
    if (MaxVolt >= 0) {
        if (MaxVolt > MAX_VOLTAGE) {
            MaxVolt = MAX_VOLTAGE;
        }
        pDcChargingInfo->MaximumChargingVoltage = MaxVolt;
    }

    MaxCurrent = (float)(ntohs(pCapabilityInfo->OutputCurrent));
    if (MaxCurrent >= 0) {
        if (MaxCurrent > MAX_CURRENCY) {
            MaxCurrent = MAX_CURRENCY;
        }
        pDcChargingInfo->AvailableChargingCurrent = MaxCurrent;
    }

    MaxPower = (float)(ntohs(pCapabilityInfo->OutputPower));
    if (MaxPower >= 0) {
        if (MaxPower > MAX_POWER) {
            MaxPower = MAX_POWER;
        }
        pDcChargingInfo->AvailableChargingPower = MaxPower;
    }
    //MaxVolt = (float)(data[0] << 8 |data[1]);
    //MaxCurrent = (float)(data[2] << 8 | data[3]);
    //MaxPower = (float)(data[4] << 8 | data[5]);
    //printf("MaxVolt=%f, MaxCurrent=%f, MaxPower=%f,\n", MaxVolt, MaxCurrent, MaxPower);

    //--- user prices information ----------------------------------------------
    addr = (sizeof(CapabilityInfo) - 2); //2 byte reserved
    pAccountInfo = (AccountInfo *)&data[addr];

    pSysConfig->BillingData.Currency = pAccountInfo->Currency;
    ShmSelectGunInfo->PricesInfo[plugNum].UserPrices = transPricesUnit(ntohl(pAccountInfo->UserPrices));
    pDcChargingInfo->ChargingFee                     = transPricesUnit(ntohl(pAccountInfo->TotalCost));
    ShmSelectGunInfo->PricesInfo[plugNum].Balance    = transPricesUnit(ntohl(pAccountInfo->Balance));

    if ((pricesInfo[plugNum].UserPrices != ShmSelectGunInfo->PricesInfo[plugNum].UserPrices) ||
            (pricesInfo[plugNum].Balance != ShmSelectGunInfo->PricesInfo[plugNum].Balance)) {
        pricesInfo[plugNum].UserPrices = ShmSelectGunInfo->PricesInfo[plugNum].UserPrices;
        pricesInfo[plugNum].Balance = ShmSelectGunInfo->PricesInfo[plugNum].Balance;

        log_info("id = %d, user prices = %.2f, Total cost = %.2f, Account balances = %.2f, currency = %s\r\n",
                 plugNum,
                 ShmSelectGunInfo->PricesInfo[plugNum].UserPrices,
                 pDcChargingInfo->ChargingFee,
                 ShmSelectGunInfo->PricesInfo[plugNum].Balance,
                 (uint8_t *)GetCurrency(pSysConfig->BillingData.Currency)
                );
    }

    return PASS;
}

static void addFaultCodeToBuf(uint8_t *Code)
{
    uint8_t warningCount = pSysWarning->WarningCount;

    if (warningCount < 10) {
        memcpy(&pSysWarning->WarningCode[warningCount][0],
               Code,
               strlen((char *)Code));
        pSysWarning->WarningCount++;
    }
}

static void removeFaultCodeToBuf(uint8_t *Code)
{
    uint8_t find = 0x01;
    uint8_t i = 0;
    char _code[7] = {0};

    sprintf(_code, "%s", Code);

    // 把相關的錯誤碼一次移除，避免重複顯示
    while (find) {
        usleep(128);
        find = 0x00;
        for (i = 0; i < pSysWarning->WarningCount; i++) {
            usleep(128);
            if (find == 0x00) {
                if (memcmp(&pSysWarning->WarningCode[i][0], _code, 7) == 0) {
                    find = 0x01;
                }
            } else {
                memcpy(&pSysWarning->WarningCode[i - 1][0],
                       &pSysWarning->WarningCode[i][0], 7);
            }
        }

        if (find) {
            pSysWarning->WarningCount--;
        }
    }
}

bool CompareArrayIsZero(uint8_t *array, unsigned int array_size)
{
    uint8_t i;

    if (array != NULL) {
        for (i = 0; i < array_size; i++) {
            if (array[i] != 0) {
                return false;
            }
        }
    }

    return true;
}
static int powerCabinetStatusProcess(uint8_t dataLen, uint8_t *data)
{
    uint8_t ret = 0;
    uint8_t i = 0;
    uint8_t count = 0;
    uint8_t EventCodeTmp[7] = {0};
    uint8_t StatusArray[MAX_REGISTER_NUM][WARNING_CODE_SIZE] = {0};
    uint8_t remaindLen = 0;
    //uint8_t statusCodeError = 0;

    if (dataLen > 0) {
        Hexdump((uint8_t *)data, dataLen);

        remaindLen = dataLen % WARNING_CODE_SIZE;

        if (remaindLen != 0) {
            log_info("fail status code length =  %d\r\n", dataLen);
            dataLen -= remaindLen;
        }

        if (dataLen < WARNING_CODE_SIZE) {
            log_error("fail status code length = %d\r\n", dataLen);
            Hexdump(data, dataLen);
            if (pSysWarning->WarningCount > 0) {
                for (i = 0; i < pSysWarning->WarningCount; i++) {
                    usleep(128);
                    if (pSysWarning->WarningCode[i][1] >= '3' ||  //from backend or power cabinet
                            (pSysWarning->WarningCode[i][0] >= 'B' &&
                             pSysWarning->WarningCode[i][1] >= '4')) {
                        memset(EventCodeTmp, 0, sizeof(EventCodeTmp));
                        memcpy(EventCodeTmp,
                               pSysWarning->WarningCode[i],
                               sizeof(EventCodeTmp));
                        removeFaultCodeToBuf(EventCodeTmp);
                    }
                }
            }
            return FAIL;
        }

        for (count = 0; count < dataLen; count += WARNING_CODE_SIZE) {
            usleep(128);

            if (strncmp((char *)&data[count + 1], "1", 1) != 0 &&
                    strncmp((char *)&data[count + 1], "2", 1) != 0 &&
                    strncmp((char *)&data[count + 1], "3", 1) != 0 &&
                    (strncmp((char *)&data[count], "B", 1) != 0 &&
                     strncmp((char *)&data[count + 1], "4", 1) != 0)
               ) {
                log_error("error status code = %s", (char *)&data[count]);
                continue;
            }

            // misc command status code handle
            if (strncmp((char *)&data[count], MISC_ST_MISC_CMD, WARNING_CODE_SIZE) == 0) {
                gDoCommGblData.MiscCmd = REG_MISC_CONTROL;
                memset((char *)&data[count], 0, WARNING_CODE_SIZE);
                continue;
            } else if (strncmp((char *)&data[count], MISC_ST_VERSION, WARNING_CODE_SIZE) == 0) {
                gDoCommGblData.MiscCmd = REG_REPORT_CSU_VERSION;
                memset((char *)&data[count], 0, WARNING_CODE_SIZE);
                continue;
            }

            if (CompareArrayIsZero((uint8_t *)&data[count], WARNING_CODE_SIZE) == true) {
                memset(EventCodeTmp, 0, sizeof(EventCodeTmp));
                memcpy(EventCodeTmp, (char *)&data[count], sizeof(EventCodeTmp));
                log_error("error status = %s\r\n", EventCodeTmp);
                continue;
            }

            strncpy((char *)&StatusArray[count / WARNING_CODE_SIZE], (char *)&data[count], WARNING_CODE_SIZE);

            ret = 0;
            for (i = 0; i < pSysWarning->WarningCount; i++) {
                usleep(128);
                if (memcmp(&pSysWarning->WarningCode[i][0],
                           StatusArray[count / WARNING_CODE_SIZE],
                           WARNING_CODE_SIZE) == 0) {
                    ret = 1;
                    break;
                }
            }

            if (ret == 0) {
                addFaultCodeToBuf(StatusArray[count / WARNING_CODE_SIZE]);
            }
            //Rtn=StatusCodeProcessing(StatusArray[count/6]);
        }
    } else {
        if (CompareArrayIsZero(data, WARNING_CODE_SIZE) == false) {
            log_error("power cabinet status code data length is zero, but have data\r\n");
            Hexdump((uint8_t *)data, WARNING_CODE_SIZE);
        }
    }

    for (i = 0; i < pSysWarning->WarningCount; i++) {
        usleep(128);

        if (pSysWarning->WarningCode[i][1] >= '3' || //from backend or power cabinet 0x33
                (pSysWarning->WarningCode[i][0] >= 'B' &&
                 pSysWarning->WarningCode[i][1] >= '4')) {
            ret = 0;

            for (count = 0; count < (dataLen / WARNING_CODE_SIZE); count++) {
                usleep(128);
                if (memcmp(&pSysWarning->WarningCode[i][0],
                           StatusArray[count],
                           WARNING_CODE_SIZE) == 0) {
                    ret = 1;
                    break;
                }
            }

            if (ret == 0) {
                memset(EventCodeTmp, 0, sizeof(EventCodeTmp));
                memcpy(EventCodeTmp,
                       pSysWarning->WarningCode[i],
                       sizeof(EventCodeTmp));
                removeFaultCodeToBuf(EventCodeTmp);
            }
        }
    }

    return PASS;
}

static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t plugNum, uint8_t reg)
{
    int ret = PASS;
    uint8_t rawDataLen = 0;
    CsuResultPkt *pCsuResult = (CsuResultPkt *)pResult;
    SoftwareUpdInfo *pSoftwareUpd = NULL;
    PcPsuOutput *pPcPsuOutput = NULL;

    //Hexdump((uint8_t *)pCsuResult, sizeof(CmdHead) + pCsuResult->Head.DataLen);
    if (compareOpcode(pCsuResult->Head.OP) == FAIL ||
            compareResult(pCsuResult->Data.Result) == FAIL ||
            compareRegister(pCsuResult->Data.Register, reg) == FAIL) {
        if (reg != REG_CHARGING_PERMISSION) {
            return FAIL;
        }
    }

    rawDataLen = (pCsuResult->Head.DataLen - 2); //2 byte = register and result byte

    switch (pCsuResult->Data.Register) {
    case REG_MODEL_NAME:
        break;

    case REG_CONNECTOR_ID:
        gDoCommGblData.ConnectorID[0] = pCsuResult->Data.Data[0];
        gDoCommGblData.ConnectorID[1] = pCsuResult->Data.Data[1];
        log_info("OK (Left connector ID = %d, Right connector ID = %d)\n",
                 pCsuResult->Data.Data[0],
                 pCsuResult->Data.Data[1]);
        break;

    case REG_POWER_CABINET_STATUS:
        ret = powerCabinetStatusProcess(rawDataLen, pCsuResult->Data.Data);
        break;

    case REG_DISPENSER_STATUS:
        memset(&gPreSysWarningInfo, 0, sizeof(struct WARNING_CODE_INFO));
        memcpy((uint8_t *)&gPreSysWarningInfo,
               pSysWarning,
               sizeof(struct WARNING_CODE_INFO));
        break;

    case REG_CHARGING_CAP:
        chargingcapabilityHandle(pCsuResult->Data.Data, plugNum);
        break;

    case REG_CHARGING_TARGET:
        break;

    case REG_SOFTWARE_UPDATE:
        pSoftwareUpd = (SoftwareUpdInfo *)&pCsuResult->Data.Data[0];

        if ((strcmp((char *)pSoftwareUpd->ImgName, "") == 0) ||
                (rawDataLen == 0) ||
                pSoftwareUpd->UpdateState != 1) {
            ret = FAIL;
            break;
        }

        ret = FAIL;
        ret = updateFirmwareHandle(pSoftwareUpd->ImgName);
        break;

    case REG_PLUG_IN_STATE:
        break;

    case REG_CONNECTOR_STATE:
        break;

    case REG_USER_ID:
        //log_info("%s\n", pCsuResult->Data.Data[0] == 1 ? "Accept" : "Reject" );
        //if (pCsuResult->Data.Data[0] <= 0) {
        //    return FAIL;
        //}
        if (pCsuResult->Data.Result == COMMAND_RESULT_NG) {
            return COMMAND_RESULT_NG;
        }

        ShmSelectGunInfo->AuthorStateFromCabinet[plugNum] = pCsuResult->Data.Data[0];

        return pCsuResult->Data.Data[0];
        break;

    case REG_CHARGING_PERMISSION:
        //log_info("id = %d, result = %s, action = %s\r\n",
        //           pCsuResult->Head.ID,
        //           pCsuResult->Data.Result == 1 ? "OK" : "NG",
        //           pCsuResult->Data.Data[0] == 1 ? "Permitted" : "NOT permitted");
        if (pCsuResult->Data.Result == COMMAND_RESULT_NG) {
            return COMMAND_RESULT_NG;
        }

        ShmSelectGunInfo->WaitDoCommPermission[plugNum] = pCsuResult->Data.Data[0];
        return pCsuResult->Data.Data[0];
        break;

    case REG_MISC_CONTROL:
        miscCommandHandle(rawDataLen, plugNum, pCsuResult->Data.Data);
        break;

    case REG_REPORT_CSU_VERSION:
    case REG_REPORT_OTHER_VERSION:
        break;

    case REG_PRESENT_CHARGING_INFO:
        pPcPsuOutput = (PcPsuOutput *)&pCsuResult->Data.Data[0];

        ShmDcCommonData->PcPsuOutput[plugNum].Voltage = ntohs((uint16_t)pPcPsuOutput->Voltage);
        ShmDcCommonData->PcPsuOutput[plugNum].Current = ntohs((uint16_t)pPcPsuOutput->Current);
        //log_info("%d power cabinet PSU output vol = %f, cur = %f\r\n",
        //         plugNum,
        //         (float)ShmDcCommonData->PcPsuOutput[plugNum].Voltage,
        //         (float)ShmDcCommonData->PcPsuOutput[plugNum].Current);
        break;

    case REG_QRCODE_URL_INFO:
        ret = qrCodeUrlInfoHandle(pCsuResult->Data.Data);
        break;

    case REG_WAIT_PLUG_IT_STATE:
        break;

    case REG_Ground_Fault_Detection:
        if (pCsuResult->Data.Result == COMMAND_RESULT_NG) {
            return COMMAND_RESULT_NG;
        }

        //集電弓relay 不打開才能進入動作
        pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);
        if(pDcChargingInfo->PantographFlag == YES)
        {
            GroundFaultDetection *gfd = (GroundFaultDetection *)&pCsuResult->Data.Data[0];
            if(pDcChargingInfo->GroundFaultStatus != gfd->Status)
            {
                log_info("id = %d, GFD Result = %d\r\n",
                           pCsuResult->Head.ID,
                           gfd->Status);
            }
            pDcChargingInfo->GroundFaultStatus = gfd->Status;
        }
        break;

    default:
        break;
    }

    return ret;
}

static int composeSocketData(int fd,
                             uint8_t id,
                             uint8_t opCode,
                             uint8_t reg,
                             uint8_t dataLen,
                             uint8_t *data)
{
    int ret = PASS;
    int size = 0;
    int sendPktLen = 0;
    uint8_t i = 0;
    uint8_t plugNum = 0;
    CsuCmdPkt csuCmdPkt = {0};
    CsuResultPkt csuResult = {0};

    csuCmdPkt.Head.SeqNum = gDoCommGblData.SeqNum++;
    csuCmdPkt.Head.ID = id;
    csuCmdPkt.Head.OP = opCode;
    csuCmdPkt.Head.DataLen = dataLen + 1; //+1 for register byte

    csuCmdPkt.Data.Register = reg;
    if ((data != NULL) && (dataLen > 0)) {
        memcpy(csuCmdPkt.Data.Data, data, dataLen);
    }

    sendPktLen = csuCmdPkt.Head.DataLen + sizeof(csuCmdPkt.Head);

    //Hexdump((uint8_t *)&csuCmdPkt, sendPktLen);

    //send command packet
    if ((size = sendTcpSocket(fd, (uint8_t *)&csuCmdPkt, sendPktLen)) < 0) {
        log_error("TCP socket send packet fail = %d\r\n", size);
        gDoCommGblData.DisConnCount++;
        return FAIL;
    }

    //receive result head
    if ((size = recvTcpSocket(fd, (uint8_t *)&csuResult.Head, sizeof(csuResult.Head))) < 0) {
        log_error("TCP socket RX head fail = %d\r\n", size);
        gDoCommGblData.DisConnCount++;
        return FAIL;
    }

    //receive result raw data
    if ((size = recvTcpSocket(fd, (uint8_t *)&csuResult.Data, csuResult.Head.DataLen)) < 0) {
        log_error("TCP socket RX data fail = %d\r\n", size);
        gDoCommGblData.DisConnCount++;
        return FAIL;
    }

    for (i = 0; i < sizeof(gDoCommGblData.ConnectorID); i++) {
        if (id == gDoCommGblData.ConnectorID[i]) {
            plugNum = i;
            break;
        }
    }

    ret = responsePackeHandle(fd, (uint8_t *)&csuResult, plugNum, csuCmdPkt.Data.Register);

    gDoCommGblData.DisConnCount = 0; //送收資料沒有問題, 清除中斷網路計數

    return ret;
}

static int writeWaitPlugItState(int fd, uint8_t id)
{
    int ret = PASS;
    uint8_t data[2] = {pSysInfo->WaitForPlugit, 0};

    //printf("WaitForPlugit = %d\r\n", pSysInfo->WaitForPlugit);

    if ((ret = composeSocketData(fd,
                                 id,
                                 OP_WRITE_DATA,
                                 REG_WAIT_PLUG_IT_STATE,
                                 1,
                                 &data[0])) == FAIL) {
        return ret;
    }

    return ret;
}

static int readQRcodeURLAndSystemDate(int fd)
{
    int ret = PASS;

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_READ_DATA,
                            REG_QRCODE_URL_INFO,
                            0,
                            NULL);
    return ret;
}

static int writePresentChargingInfo(int fd, uint8_t plugNum, uint8_t id)
{
    int ret = PASS;
    uint8_t dataBuf[16] = {0};
    PreChargingInfo *pPreChargingInfo = (PreChargingInfo *)dataBuf;
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    if(pDcChargingInfo->PantographFlag == NO)
    {
        pPreChargingInfo->PresentChargingVoltage = htons((uint16_t)(pDcChargingInfo->PresentChargingVoltage * 10));
        pPreChargingInfo->PresentChargingCurrent = htons((uint16_t)(pDcChargingInfo->PresentChargingCurrent * 10));
    }
    else
    {
        pPreChargingInfo->PresentChargingVoltage = htons((uint16_t)(ShmDcCommonData->PcPsuOutput[plugNum].Voltage));
        pPreChargingInfo->PresentChargingCurrent = htons((uint16_t)(ShmDcCommonData->PcPsuOutput[plugNum].Current));
    }
    pPreChargingInfo->RemainChargingDuration = htonl(pDcChargingInfo->RemainChargingDuration);
    pPreChargingInfo->EvBatterySoc = pDcChargingInfo->EvBatterySoc;

    ret = composeSocketData(fd,
                            id,
                            OP_WRITE_DATA,
                            REG_PRESENT_CHARGING_INFO,
                            sizeof(PreChargingInfo),
                            (uint8_t *)pPreChargingInfo);
    return ret;
}

static int writeOtherModuleVersion(int fd)
{
    int ret = PASS;
    uint8_t data[255] = {0};
    uint8_t dataLen = 0;

    //report other module version message
    memcpy(&data[dataLen], pSysInfo->FanModuleFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->RelayModuleFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->Connector1FwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->Connector2FwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->LedModuleFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_WRITE_DATA,
                            REG_REPORT_OTHER_VERSION,
                            dataLen,
                            &data[0]);

    return ret;
}

static int writeCsuModuleVersion(int fd)
{
    int ret = PASS;
    uint8_t data[255] = {0};
    uint8_t dataLen = 0;

    //report CSU platform version message
    memcpy(&data[dataLen], pSysInfo->CsuBootLoadFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->CsuKernelFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->CsuRootFsFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], pSysInfo->CsuPrimFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_WRITE_DATA,
                            REG_REPORT_CSU_VERSION,
                            dataLen,
                            &data[0]);

    return ret;
}

static int readMiscCommand(int fd, uint8_t id)
{
    int ret = PASS;

    ret = composeSocketData(fd,
                            id,
                            OP_READ_DATA,
                            REG_MISC_CONTROL,
                            0,
                            NULL);

    return ret;
}

static int readChargePermission(int fd, uint8_t id)
{
    return composeSocketData(fd,
                             id,
                             OP_READ_DATA,
                             REG_CHARGING_PERMISSION,
                             0,
                             NULL);
}

static int writeGroundFaultDetection(int fd, uint8_t status, uint8_t id)
{
    int ret = PASS;
    uint8_t dataBuf[1] = {status};
    ret = composeSocketData(fd,
                            id,
                            OP_WRITE_DATA,
                            REG_Ground_Fault_Detection,
                            1,
                            &dataBuf[0]);
    return ret;
}

static int writeUserID(int fd, uint8_t id, uint8_t *pUserID)
{
    if ((strlen((char *)pUserID) <= 0) || (pUserID == NULL)) {
        return FAIL;
    }

    return composeSocketData(fd,
                             id,
                             OP_WRITE_DATA,
                             REG_USER_ID,
                             strlen((char *)pUserID),
                             pUserID);
}

static int writeConnectorState(int fd, uint8_t plugNum, uint8_t id)
{
    uint8_t dataBuf[10] = {'\0'};
    ConnectorState *pConnState = (ConnectorState *)dataBuf;
    static char vendorErrorCodeTmp[2][WARNING_CODE_SIZE] = {0};
    int ret = PASS;
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    pConnState->ConnectorTemp = pDcChargingInfo->ConnectorTemp;
    pConnState->ChillerTemp = pDcChargingInfo->ChillerTemp;

    if (pDcChargingInfo->SystemStatus <= S_AUTHORIZING) {
        pConnState->State = CONN_ST_IDLE;    //idle
        strncpy((char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode,
                "",
                sizeof(ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode));

        strncpy(&vendorErrorCodeTmp[plugNum][0], "", WARNING_CODE_SIZE);
    } else if ((pDcChargingInfo->SystemStatus <= S_PREPARING_FOR_EVSE) ||
               (pDcChargingInfo->SystemStatus == S_CCS_PRECHARGE_ST0) ||
               (pDcChargingInfo->SystemStatus == S_CCS_PRECHARGE_ST1)) {
        pConnState->State = CONN_ST_PREPARING;    //preparing
    } else if (pDcChargingInfo->SystemStatus == S_CHARGING) {
        pConnState->State = CONN_ST_CHARGING;    //charging
    } else if (pDcChargingInfo->SystemStatus == S_TERMINATING) {
        pConnState->State = CONN_ST_TERMINATING;    //terminating
    } else if ((pDcChargingInfo->SystemStatus == S_ALARM) ||
               (pDcChargingInfo->SystemStatus == S_FAULT)) {
        pConnState->State = CONN_ST_ALARM;
        strncpy((char *)pConnState->WarningCode,
                (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode,
                WARNING_CODE_SIZE);

        if (strncmp(&vendorErrorCodeTmp[plugNum][0], "", WARNING_CODE_SIZE) == 0) {
            strncpy(&vendorErrorCodeTmp[plugNum][0],
                    (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode,
                    WARNING_CODE_SIZE);
            vendorErrorCodeTmp[plugNum][6] = '\0';
            log_info("1 ID = %d, VendorErrorCode = %s\r\n",
                     plugNum,
                     (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode);
        } else {
            if (strncmp(&vendorErrorCodeTmp[plugNum][0],
                        (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode,
                        WARNING_CODE_SIZE) != 0
               ) {
                strncpy(&vendorErrorCodeTmp[plugNum][0],
                        (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode,
                        WARNING_CODE_SIZE);
                vendorErrorCodeTmp[plugNum][6] = '\0';
                log_info("2 ID = %d, VendorErrorCode = %s\r\n",
                         plugNum,
                         (char *)ShmOCPP16Data->StatusNotification[plugNum].VendorErrorCode);
            }
        }
    }

    ret = composeSocketData(fd,
                            id,
                            OP_WRITE_DATA,
                            REG_CONNECTOR_STATE,
                            sizeof(dataBuf) - 1,
                            &dataBuf[0]);
    return ret;
}

static int writePlugInStatus(int fd, uint8_t plugNum, uint8_t id)
{
    int ret = PASS;
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);
    uint8_t dataBuf[2] = {pDcChargingInfo->ConnectorPlugIn, 0};

    ret = composeSocketData(fd,
                            id,
                            OP_WRITE_DATA,
                            REG_PLUG_IN_STATE,
                            1,
                            &dataBuf[0]);
    return ret;
}

static int readSoftwareUpdate(int fd, uint8_t gunID)
{
    int ret = PASS;
    uint8_t dataBuf[2] = {pSysInfo->FirmwareUpdate, 0};

    ret = composeSocketData(fd,
                            gunID,
                            OP_READ_DATA,
                            REG_SOFTWARE_UPDATE,
                            1,
                            &dataBuf[0]);

    return ret;
}

static int writeChargingTarget(int fd, uint8_t plugNum, uint8_t id)
{
    int ret = PASS;
    float ChargingVolt, ChargingAmp;
    uint8_t dataBuf[4] = {0};
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    ChargingVolt = pDcChargingInfo->EvBatterytargetVoltage;
    ChargingAmp = pDcChargingInfo->EvBatterytargetCurrent;
    ChargingVolt *= 10;
    ChargingAmp *= 10;

    dataBuf[0] = ((uint16_t)ChargingVolt >> 8) & 0xFF;
    dataBuf[1] = ((uint16_t)ChargingVolt) & 0xFF;
    dataBuf[2] = ((uint16_t)ChargingAmp >> 8) & 0xFF;
    dataBuf[3] = ((uint16_t)ChargingAmp) & 0xFF;

    ret = composeSocketData(fd,
                            id,
                            OP_WRITE_DATA,
                            REG_CHARGING_TARGET,
                            sizeof(dataBuf),
                            &dataBuf[0]);

    return ret;
}

static int readChargingCapability(int fd, uint8_t id)
{
    int ret = PASS;

    ret = composeSocketData(fd,
                            id,
                            OP_READ_DATA,
                            REG_CHARGING_CAP,
                            0,
                            NULL);

    return ret;
}

static int writeDispenserStatus(int fd, uint8_t gunID)
{
    uint8_t warningCount = 0;
    uint8_t count = 0;
    uint8_t CurWarnCodeTmp[10][6] = {0};
    uint8_t PreWarnCodeTmp[10][6] = {0};
    int ret = PASS;

    if ((pSysWarning->WarningCount <= 0) &&
            (gPreSysWarningInfo.WarningCount <= 0)) {
        return FAIL;
    }

    warningCount = pSysWarning->WarningCount;
    for (count = 0; count < warningCount; count++) {
        memcpy(CurWarnCodeTmp[count], pSysWarning->WarningCode[count], WARNING_CODE_SIZE);
    }

    warningCount = gPreSysWarningInfo.WarningCount;
    for (count = 0; count < warningCount; count++) {
        memcpy(PreWarnCodeTmp[count], gPreSysWarningInfo.WarningCode[count], WARNING_CODE_SIZE);
    }

    if (strcmp((char *)&CurWarnCodeTmp[0], (char *)&PreWarnCodeTmp[0]) == 0) {
        return FAIL;
    }

    ret = composeSocketData(fd,
                            gunID,
                            OP_WRITE_DATA,
                            REG_DISPENSER_STATUS,
                            strlen((char *)CurWarnCodeTmp),
                            &CurWarnCodeTmp[0][0]);

    return ret;
}

static int readPowerCabinetStatus(int fd, uint8_t gunID)
{
    int ret = PASS;

    ret = composeSocketData(fd,
                            gunID,
                            OP_READ_DATA,
                            REG_POWER_CABINET_STATUS,
                            0,
                            NULL);

    return ret;
}

static int readConnectorID(int fd)
{
    int ret = PASS;

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_READ_DATA,
                            REG_CONNECTOR_ID,
                            0,
                            NULL);

    return ret;
}

static int WriteModelName(int fd)
{
    int ret = PASS;
    uint8_t Tmp = 0;
    uint8_t TmpBuf[255] = {0};

    memcpy(TmpBuf,
           pSysConfig->ModelName,
           strlen((char *)pSysConfig->ModelName));

    Tmp = (uint8_t)(ShmPrimaryMcuData->InputDet.bits.Key2 << 2 |
                    ShmPrimaryMcuData->InputDet.bits.Key1 << 1 |
                    ShmPrimaryMcuData->InputDet.bits.Key0);

    TmpBuf[strlen((char *)pSysConfig->ModelName)] = Tmp; //Dispenser switch value

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_WRITE_DATA,
                            REG_MODEL_NAME,
                            strlen((char *)pSysConfig->ModelName) + 1,
                            &TmpBuf[0]);

    return ret;
}

static void calDisconnectCount(uint8_t *pValue, uint8_t maxCount)
{
    sleep(3); //wait 1 second

    if ((*pValue) >= maxCount) {
        setTcpStatus(ABNORMAL);
    } else {
        (*pValue)++;
    }
}

static int CheckNetworkStatus(void)
{
    char *ipAddr = (char *)&pSysConfig->Eth0Interface.EthIpAddress;

    //printf("Check network IP Header = %s\n", ipAddr);
    if (strstr(ipAddr, CMP_ETH_IP_HEAD) != NULL) {
        return 1;
    }

    return 0;
}

static void updateFirmwareProcess(int fd, uint8_t gunID, uint8_t totalConnCount)
{
    bool canUpdateFirmware = true;
    uint8_t plugNum = 0;
    uint8_t ackCount = 5;
    struct timeb updateTime;
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    if (pSysWarning->Level != 2) {
        for (plugNum = 0; plugNum < totalConnCount; plugNum++) {
            if (pDcChargingInfo->SystemStatus != S_IDLE &&
                    pDcChargingInfo->SystemStatus != S_RESERVATION &&
                    pDcChargingInfo->SystemStatus != S_MAINTAIN) {
                canUpdateFirmware = false;
            }
        }
    }

    if (canUpdateFirmware) {
        ftime(&updateTime);
        if (DiffTimeb(gRegTimeUp[0][REG_SOFTWARE_UPDATE], updateTime) > LOOP_RETRY_TIME * 5) {
            readSoftwareUpdate(fd, gunID);
            ftime(&gRegTimeUp[0][REG_SOFTWARE_UPDATE]);
        }

        if (pSysInfo->FirmwareUpdate == YES) {
            while (ackCount != 0) {
                ftime(&updateTime);
                if (DiffTimeb(gRegTimeUp[0][REG_SOFTWARE_UPDATE], updateTime) > LOOP_RETRY_TIME / 2) {
                    readSoftwareUpdate(fd, gunID);
                    ftime(&gRegTimeUp[0][REG_SOFTWARE_UPDATE]);
                    if (pSysInfo->FirmwareUpdate == NO) {
                        ackCount--;
                    }
                }
                usleep(128);
            }
        }
    }
}

static void checkAuthorProcess(int fd, uint8_t plugNum)
{
    int ret = 0;
    uint8_t gunID = 0;
    struct timeb AuthNowTime;

#if defined DD360Audi
    gunID = gDoCommGblData.ConnectorID[pSysInfo->CurGunSelected];
    //gunID = gDoCommGblData.ConnectorID[plugNum];
#else
    gunID = ID_REGISTER;

    ShmSelectGunInfo->PricesInfo[pSysInfo->CurGunSelected].Balance = 0.0; //非Audi 不需要等待主櫃回報餘額
#endif // DD360Audi

    if ((ShmOCPP16Data->SpMsg.bits.AuthorizeReq == YES) ||
            (pSysInfo->AuthorizeFlag == YES)) {
        ftime(&AuthNowTime);

        if (DiffTimeb(gRegTimeUp[plugNum][REG_USER_ID], AuthNowTime) > LOOP_RETRY_TIME) {
            ret = writeUserID(fd,
                              gunID,
                              pSysConfig->UserId);
            if (ret >= 0) {
                memset(ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status,
                       0,
                       sizeof(ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status));
                if (ret == 0) {
                    strcpy((char *)ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status, "Invalid");
                } else {
                    strcpy((char *)ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status, "Accepted");
                }

                //printf("%d Balance = %.2f, %.2f\r\n",
                //       plugNum,
                //       ShmSelectGunInfo->PricesInfo[plugNum].Balance,
                //       FAIL_BALANCE_PRICES);

                if (ShmSelectGunInfo->PricesInfo[plugNum].Balance != FAIL_BALANCE_PRICES) {
                    ShmOCPP16Data->SpMsg.bits.AuthorizeConf = YES; //isAuthorizedComplete
                    ShmOCPP16Data->SpMsg.bits.AuthorizeReq  = NO;
                    pSysInfo->AuthorizeFlag                 = NO;
                    ShmPsuData->SystemAvailablePower        = NO;
                    ShmPsuData->SystemPresentPsuQuantity    = NO;
                }
            }
            ftime(&gRegTimeUp[plugNum][REG_USER_ID]);
        }
    }
}

static int messageTrigger(int fd, uint8_t plugNum, uint8_t gunID, uint8_t curReg)
{
    int ret = DISPENSER_INIT_SUCC;
    int isContinue = 1;
    struct timeb NowTime;

    while (isContinue) {
        usleep(128);
        ftime(&NowTime);

        switch (curReg) {
        case REG_MODEL_NAME:
            if (WriteModelName(fd) == PASS) {
                gDoCommGblData.DisConnCount = 0;
                curReg = REG_CONNECTOR_ID;
            } else {
                sleep(1);
            }
            break;

        case REG_CONNECTOR_ID:
            if (readConnectorID(fd) == PASS) {
                gDoCommGblData.DisConnCount = 0;
                curReg = REG_REPORT_CSU_VERSION;
            } else {
                sleep(1);
            }
            break;

        case REG_POWER_CABINET_STATUS:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME ||
                    DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0) {
                readPowerCabinetStatus(fd, gunID);
                ftime(&gRegTimeUp[plugNum][curReg]);
            }
            curReg = REG_DISPENSER_STATUS;
            break;

        case REG_DISPENSER_STATUS:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME ||
                    DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0) {
                writeDispenserStatus(fd, gunID);
                ftime(&gRegTimeUp[plugNum][curReg]);
            }
            curReg = REG_PLUG_IN_STATE;
            break;

        //case REG_CHARGING_CAP:
        //    readChargingCapability(fd, plugNum, gunID);
        //    break;

        //case REG_CHARGING_TARGET:
        //    writeChargingTarget(fd, plugNum, gunID);
        //    break;

        //case REG_SOFTWARE_UPDATE:
        //    if (ChargingData[plugNum]->SystemStatus == S_IDLE) {
        //        if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > L OOP_RETRY_TIME||
        //DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0  * 1) {
        //            readSoftwareUpdate(fd);
        //            ftime(&gRegTimeUp[plugNum][curReg]);
        //        }
        //        while (pSysInfo->FirmwareUpdate == YES) {
        //            ftime(&NowTime);
        //            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowT ime) > LOOP_RETRY_TIME||
        //DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0) {
        //                readSoftwareUpdate(fd);
        //            }
        //            usleep(128);
        //        }
        //    }
        //    isContinue = 0;
        //    break;

        case REG_PLUG_IN_STATE:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME ||
                    DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0) {
                writePlugInStatus(fd, plugNum, gunID);
                ftime(&gRegTimeUp[plugNum][curReg]);
            }
            curReg = REG_CONNECTOR_STATE;
            break;

        case REG_CONNECTOR_STATE:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME ||
                    DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0) {
                writeConnectorState(fd, plugNum, gunID);
                ftime(&gRegTimeUp[plugNum][curReg]);
            }

            //check misc command from power cabinet
            if (gDoCommGblData.MiscCmd != 0) {
                if (gDoCommGblData.MiscCmd != REG_MISC_CONTROL) {
                    log_error("misc command failed = %x\r\n", gDoCommGblData.MiscCmd);
                }
                curReg = gDoCommGblData.MiscCmd;
            } else {
                curReg = REG_QRCODE_URL_INFO;
            }
            break;

        //case REG_USER_ID:
        //    writeUserID(fd, gunID, uint8_t *pUserID);
        //    break;

        //case REG_CHARGING_PERMISSION:
        //    readChargePermission(fd, gunID);
        //    break;

        case REG_MISC_CONTROL:
            readMiscCommand(fd, gunID);
            isContinue = 0;
            break;

        case REG_REPORT_CSU_VERSION:
        case REG_REPORT_OTHER_VERSION:
            if (gDoCommGblData.MiscCmd != 0) {
                writeCsuModuleVersion(fd);
                writeOtherModuleVersion(fd);
                clearMiscCommand();
                isContinue = 0;
            } else {
                //power up write dispenser version and get power cabinet system ID
                if ((writeCsuModuleVersion(fd) == PASS) &&
                        (writeOtherModuleVersion(fd) == PASS) &&
                        (readQRcodeURLAndSystemDate(fd) == PASS)
                   ) {
                    //gDoCommGblData.DisConnCount = 0;
                    isContinue = 0;
                }
            }
            break;

        //case REG_PRESENT_CHARGING_INFO:
        //    break;

        case REG_QRCODE_URL_INFO:
            if (gunID == 1) {
                if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > (LOOP_RETRY_TIME * 10) ||
                        DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) < 0
                   ) {
                    readQRcodeURLAndSystemDate(fd);
                    ftime(&gRegTimeUp[plugNum][curReg]);
                }
            }
            isContinue = 0;
            //curReg = REG_SOFTWARE_UPDATE;
            break;

        //case REG_WAIT_PLUG_IT_STATE:
        //    if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > (LOOP_RETRY_TIME / 10)) {
        //        writeWaitPlugItState(fd, gunID);
        //        ftime(&gRegTimeUp[plugNum][curReg]);
        //    }
        //    isContinue = 0;
        //    break;

        default:
            log_error("error curReg = %x\r\n", curReg);
            gDoCommGblData.MiscCmd = 0;
            gDoCommGblData.DisConnCount = CHECK_NETWORK_FAIL_COUNT;
            isContinue = 0;
            break;
        }

        if (gDoCommGblData.DisConnCount >= CHECK_NETWORK_FAIL_COUNT) {
            return DISPENSER_SOCKET_RECONN;
        }
    }//for

    return ret;
}

//static bool isDetectPlugin()
//{
//    if (pSysInfo->WaitForPlugit == YES) {
//        return YES;
//    }
//
//    return NO;
//}

static void systemStatusProcess(int fd, uint8_t totalGun, uint8_t plugNum, uint8_t gunID)
{
    uint8_t i = 0;
    struct timeb AuthNowTime = {0};
    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(plugNum);

    switch (pDcChargingInfo->SystemStatus) {
    case S_IDLE:
    case S_RESERVATION:
    case S_MAINTAIN:
    case S_ALARM:
    case S_AUTHORIZING:
        checkAuthorProcess(fd, plugNum);

        //authorization complete, write wait plug it state
        if (ShmSelectGunInfo->PricesInfo[plugNum].Balance == FAIL_BALANCE_PRICES) {
            break;
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_WAIT_PLUG_IT_STATE], AuthNowTime) > (LOOP_RETRY_TIME / 10) ||
                DiffTimeb(gRegTimeUp[plugNum][REG_WAIT_PLUG_IT_STATE], AuthNowTime) < 0
           ) {
            writeWaitPlugItState(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_WAIT_PLUG_IT_STATE]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) < 0
           ) {
            readChargingCapability(fd, gunID);
            writeGroundFaultDetection(fd, 0, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }
        break;

    case S_PREPARNING: //get permission
        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CONNECTOR_STATE], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CONNECTOR_STATE], AuthNowTime) < 0
           ) {
            writeConnectorState(fd, plugNum, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CONNECTOR_STATE]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) < 0
           ) {
            if (readChargePermission(fd, gunID) && readChargingCapability(fd, gunID)) {
                for (i = 0; i < totalGun; i++) {
                    pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(i);

                    ShmPsuData->SystemAvailablePower += pDcChargingInfo->AvailableChargingPower;
                }
                ShmPsuData->SystemPresentPsuQuantity = (ShmPsuData->SystemAvailablePower / 30);
            }
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_PERMISSION]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) < 0
           ) {
            writePresentChargingInfo(fd, plugNum, gunID);
            writeGroundFaultDetection(fd, 0, gunID);
            ftime(&gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO]);
        }
        break;

    case S_PREPARING_FOR_EV://wait connector lock
        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) < 0
           ) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) < 0
           ) {
            if (readChargePermission(fd, gunID) == 0) {
                log_info("S_PREPARING_FOR_EV Stop charging by power cabinet's permission = %d, %d\r\n",
                         plugNum,
                         REG_CHARGING_PERMISSION);
                pDcChargingInfo->StopChargeFlag = POWER_CABINET_STOP_CHARGING;
            }
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_PERMISSION]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) < 0
           ) {
            writePresentChargingInfo(fd, plugNum, gunID);
            writeGroundFaultDetection(fd, 0, gunID);
            ftime(&gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO]);
        }
        break;

    case S_PREPARING_FOR_EVSE: //insaulation test
    case S_CCS_PRECHARGE_ST0:
    case S_CCS_PRECHARGE_ST1:
        writeChargingTarget(fd, plugNum, gunID);
        writeGroundFaultDetection(fd, 1, gunID);

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) < 0
           ) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) < 0
           ) {
            if (readChargePermission(fd, gunID) == 0) {
                log_info("S_PREPARING_FOR_EVSE Stop charging by power cabinet's permission = %d, %d\r\n",
                         plugNum,
                         REG_CHARGING_PERMISSION);
                pDcChargingInfo->StopChargeFlag = POWER_CABINET_STOP_CHARGING;
            }
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_PERMISSION]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) < 0
           ) {
            writePresentChargingInfo(fd, plugNum, gunID);
            ftime(&gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO]);
        }
        break;

    case S_CHARGING: //charging
    case S_TERMINATING:
        if(pDcChargingInfo->Type == _Type_GB || pDcChargingInfo->Type == _Type_Chademo)
        {
            writeGroundFaultDetection(fd, 0, gunID);
        }
        else
        {
            writeGroundFaultDetection(fd, 1, gunID);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) < 0
           ) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) < 0
           ) {
            if (readChargePermission(fd, gunID) == 0) {
                log_info("Stop charging by power cabinet's permission = %d, %d\r\n",
                         plugNum,
                         REG_CHARGING_PERMISSION);
                pDcChargingInfo->StopChargeFlag = POWER_CABINET_STOP_CHARGING;
                //printf("%d StopChargeFlag = %d\r\n", plugNum, pDcChargingInfo->StopChargeFlag);
            }
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_PERMISSION]);
        }
        writeChargingTarget(fd, plugNum, gunID);

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) < 0
           ) {
            writePresentChargingInfo(fd, plugNum, gunID);
            ftime(&gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO]);
        }
        break;

    case S_COMPLETE:
        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME ||
                DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) < 0
           ) {
            readChargingCapability(fd, gunID);
            writeGroundFaultDetection(fd, 0, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }
        break;

    default:
        break;
    }
}

static int networkCreatePorcess(void)
{
    int fd = 0;

    while (pSysInfo->SelfTestSeq != _STEST_COMPLETE) {
        usleep(128);
    }

    setTcpStatus(ABNORMAL);

    /**************** Check Network **********/
    log_info("Check Dispenser Network Information");
    while (pSysInfo->SelfTestSeq != _STEST_COMPLETE) {
        usleep(125);
    }

    while (!CheckNetworkStatus()) {
        calDisconnectCount(&gDoCommGblData.DisConnCount, CHECK_NETWORK_FAIL_COUNT);
        log_error("disconnect count = % d\r\n", gDoCommGblData.DisConnCount);
    }
    log_info("Dispenser Network Information checked: IP = % s Netmask = % s, Gateway = % s",
             pSysConfig->Eth0Interface.EthIpAddress,
             pSysConfig->Eth0Interface.EthSubmaskAddress,
             pSysConfig->Eth0Interface.EthGatewayAddress);

    gDoCommGblData.DisConnCount = 0;

    /**************** Power cabinet connection **********/
    log_info("Connect to Power Cabinet");
    while ((fd = doCommConnToServer()) <= 0) {
        calDisconnectCount(&gDoCommGblData.DisConnCount, CONNECT_SERVER_FAIL_COUNT);
        log_error("disconnect count = %d, fd = %d\r\n", gDoCommGblData.DisConnCount, fd);
    }
    log_info("Power cabinet connected(fd = % d): IP = % s Netmask = % s, Gateway = % s",
             fd,
             pSysConfig->Eth0Interface.EthIpAddress,
             pSysConfig->Eth0Interface.EthSubmaskAddress,
             pSysConfig->Eth0Interface.EthGatewayAddress);

    gDoCommGblData.DisConnCount = 0;
    destroySelectGun(DESTROY_ALL_SEL);

    sleep(3);

    setTcpStatus(NORMAL);

    return fd;
}

int main(int argc, char *argv[])
{
    uint8_t plugNum = 0, gunID = 0;
    uint8_t index = 0;
    uint8_t totalConnCount = 0;
    uint8_t initDone = DISPENER_INIT_FAIL;
    char tmpbuf[256] = {0};
    int fd = 0;
    int isContinue = 1;

    InitSocketSigPipe();

    /**************** Initialization **********/
    if (CreateAllCsuShareMemory() == FAIL) {
        log_error("create share memory error\r\n");
        return FAIL;
    }

    MappingGunChargingInfo("DoComm Task");

    pSysConfig = (struct SysConfigData *)GetShmSysConfigData();
    pSysInfo = (struct SysInfoData *)GetShmSysInfoData();
    pSysWarning = (struct WARNING_CODE_INFO *)GetShmSysWarningInfo();

    pAlarmCode = (struct AlarmCodeData *)GetShmAlarmCodeData();
    ShmPsuData = (struct PsuData *)GetShmPsuData();
    ShmPrimaryMcuData = (struct PrimaryMcuData *)GetShmPrimaryMcuData();
    ShmOCPP16Data = (struct OCPP16Data *)GetShmOCPP16Data();
    ShmSelectGunInfo = (SelectGunInfo *)GetShmSelectGunInfo();
    ShmDcCommonData = (DcCommonInfo *)GetShmDcCommonData();

    totalConnCount = pSysConfig->TotalConnectorCount;

    for (index = 0; index < totalConnCount; index++) {
        clearPricesInfo(index);
    }

    //initial trigger message timer
    for (index = 0; index < MAX_REGISTER_NUM; index++) {
        ftime(&gRegTimeUp[0][index]);
        usleep(128);
        ftime(&gRegTimeUp[1][index]);
        usleep(50000);
    }

    setTcpStatus(NORMAL);

    while (isContinue) {
        if (initDone == DISPENER_INIT_FAIL) {
            fd = networkCreatePorcess();

            initDone = messageTrigger(fd, 0, 0, REG_MODEL_NAME); //first trigger model name and connector id
        } else {
            plugNum = 0;
            gunID = 0;
            for (plugNum = 0; plugNum < totalConnCount; plugNum++) {
                //checkAuthorProcess(fd, plugNum);

                //plugNum : setup chargingData value for bottom layer
                //gunID : connector Id from power cabinet, 1 = left gun, 2 = right gun,
                gunID = gDoCommGblData.ConnectorID[plugNum];
                systemStatusProcess(fd, totalConnCount, plugNum, gunID);

                initDone = messageTrigger(fd,
                                          plugNum,
                                          gunID,
                                          REG_POWER_CABINET_STATUS);

                if (initDone == DISPENSER_SOCKET_RECONN) {
                    break;
                }
            }
        }

        if (initDone != DISPENSER_SOCKET_RECONN) {
            //update dispenser firmware
            updateFirmwareProcess(fd, gDoCommGblData.ConnectorID[0], totalConnCount);

            usleep(100000);
            continue;
        }

        //Ethernet error recovery handle
        log_info("Disconnected from power cabinet...re-connecting");
        setTcpStatus(ABNORMAL);


        if (pSysConfig->Eth0Interface.EthDhcpClient == 0) {
            sprintf(tmpbuf,
                    "/sbin/udhcpc -i eth0 -x hostname:CSU3_%s -s /root/dhcp_script/eth0.script > /dev/null &",
                    pSysConfig->SystemId);
            system(tmpbuf);
        }

        gDoCommGblData.DisConnCount = 0;
        gDoCommGblData.SeqNum = 0;

        closeSocket(fd);
        fd = 0;

        initDone = DISPENER_INIT_FAIL;
    }
}