csu3_am335x/EVSE/Projects/DD360Audi/Apps/ReDoComm.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 "../../define.h"
#include "Config.h"
#include "ReDoComm.h"
#include "AudiCust.h"

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

//------------------------------------------------------------------------------
//static int TcpSock = 0;
//static uint8_t PacketSe;
static DoCommGblData gDoCommGblData = {0};

static struct SysConfigAndInfo  *ShmSysConfigAndInfo;
static struct StatusCodeData    *ShmStatusCodeData;
static struct PsuData           *ShmPsuData;
static struct OCPP16Data        *ShmOCPP16Data;
static struct PrimaryMcuData    *ShmPrimaryMcuData;
static struct ChargingInfoData  *ChargingData[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY];
static struct timeb             gRegTimeUp[2][MAX_REGISTER_NUM] = {0};
static struct WARNING_CODE_INFO PreSysWarningInfo = {0};
static AudiCustInfo             *gAudiCustInfo = NULL;

//------------------------------------------------------------------------------
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);

//------------------------------------------------------------------------------
//--- Common function ---
//------------------------------------------------------------------------------
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 (ShmSysConfigAndInfo->SysConfig.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");

    printf("%s", message);
}

static int string2ByteArray(char *input, byte *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)
{
    DEBUG_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)
{
    ShmStatusCodeData->AlarmCode.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) {
            DEBUG_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)) {
        DEBUG_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)) {
        DEBUG_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
            DEBUG_INFO("shutdown\r\n");
        }
    }

    if (close(fd) < 0) { // finally call close()
        DEBUG_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) {
        DEBUG_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 = 0;
    tv.tv_usec = 100000;
    setsockopt(TcpSock, SOL_SOCKET,  SO_RCVTIMEO, &tv, sizeof(struct timeval));
    setsockopt(TcpSock, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(struct timeval));
    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;
}

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

    return PASS;
}

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

    return PASS;
}

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

    return PASS;
}

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

    return FAIL;
}

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

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

    return PASS;
}

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

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') {
        DEBUG_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(ShmSysConfigAndInfo->SysConfig.SystemId, '\0', sizeof(ShmSysConfigAndInfo->SysConfig.SystemId));
        //printf("data =  %s\r\n", data);
        len = string2ByteArray((char *)data, (uint8_t *)ShmSysConfigAndInfo->SysConfig.SystemId);
        //printf("SystemId =  %s\r\n", ShmSysConfigAndInfo->SysConfig.SystemId);
    }

    //if ((char *)&data[len] == '\0') {
    //    DEBUG_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);
    //    DEBUG_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);
    DEBUG_INFO("Program ready to check file %s\n", cmdBuf);
    if ( access(cmdBuf, F_OK) != -1) {
        DEBUG_INFO("File '%s' exist.\n", cmdBuf);
        ShmSysConfigAndInfo->SysInfo.FirmwareUpdate = YES;
    } else {
        DEBUG_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");
                ShmSysConfigAndInfo->SysInfo.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 id, 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]));
        //DEBUG_INFO("misc command = %x, value = %d\r\n", cmd, value);

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

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

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

            if (id == 2) {
                clearMiscCommand();
            }
            break;

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

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

            clearMiscCommand();
            break;

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

            DEBUG_INFO("default currency = %s%c\r\n", (uint8_t *)Currency[value]);
            ShmSysConfigAndInfo->SysConfig.BillingData.Currency = value;
            clearMiscCommand();
            break;

        case MISC_CMD_ACCOUNT_BALANCE:
            if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (id - 1)) {
                clearMiscCommand();
                break;
            }

            if (getConfirmSelectedGun(id - 1) == FAIL) {
                DEBUG_INFO("Remote start not select gun\r\n");
                break;
            }

            gAudiCustInfo->PricesInfo[id - 1].Balance = transPricesUnit((int)value);
            DEBUG_INFO("%d misc balance = %.2f\r\n", id, gAudiCustInfo->PricesInfo[id - 1].Balance);
            clearMiscCommand();
            break;

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

            DEBUG_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();
            DEBUG_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 (id == 2) {
                    clearMiscCommand();
                }
                break;
            }
            DEBUG_INFO("Remote start charging id = %d\r\n", id);

            if (getConfirmSelectedGun(id - 1) == FAIL) {
                DEBUG_INFO("Remote start not select gun\r\n");
                break;
            }

            //if (getSelGunWaitToAuthor(id - 1) == FAIL) {
            //    DEBUG_ERROR("Remote start gun already charging\r\n");
            //    break;
            //}

            //if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (id - 1)) {
            //    ShmSysConfigAndInfo->SysInfo.CurGunSelected = (id - 1);
            //    ShmSysConfigAndInfo->SysInfo.CurGunSelectedByAc = NO_DEFINE;
            //}

            //setConfirmSelGun(ShmSysConfigAndInfo->SysInfo.CurGunSelected);

            ShmOCPP16Data->CsMsg.bits[id - 1].RemoteStartTransactionReq = YES;
            gAudiCustInfo->PricesInfo[id - 1].Balance = 0.00;
            if (id == 2) {
                clearMiscCommand();
            }
            break;

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

    return ret;
}

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

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

    MaxVolt = (float)(ntohs(pCapabilityInfo->OutputVoltage));
    if (MaxVolt >= 0) {
        if (MaxVolt > MAX_VOLTAGE) {
            MaxVolt = MAX_VOLTAGE;
        }
        ChargingData[gunID]->MaximumChargingVoltage = MaxVolt;
    }

    MaxCurrent = (float)(ntohs(pCapabilityInfo->OutputCurrent));
    if (MaxCurrent >= 0) {
        if (MaxCurrent > MAX_CURRENCY) {
            MaxCurrent = MAX_CURRENCY;
        }
        ChargingData[gunID]->AvailableChargingCurrent = MaxCurrent;
    }

    MaxPower = (float)(ntohs(pCapabilityInfo->OutputPower));
    if (MaxPower >= 0) {
        if (MaxPower > MAX_POWER) {
            MaxPower = MAX_POWER;
        }
        ChargingData[gunID]->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];

    ShmSysConfigAndInfo->SysConfig.BillingData.Currency = pAccountInfo->Currency;
    gAudiCustInfo->PricesInfo[gunID].UserPrices         = transPricesUnit(ntohl(pAccountInfo->UserPrices));
    ChargingData[gunID]->ChargingFee                    = transPricesUnit(ntohl(pAccountInfo->TotalCost));
    gAudiCustInfo->PricesInfo[gunID].Balance            = transPricesUnit(ntohl(pAccountInfo->Balance));

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

        DEBUG_INFO("id = %d, user prices = %.2f, Total cost = %.2f, Account balances = %.2f, currency = %s\r\n",
                   gunID,
                   gAudiCustInfo->PricesInfo[gunID].UserPrices,
                   ChargingData[gunID]->ChargingFee,
                   gAudiCustInfo->PricesInfo[gunID].Balance,
                   (uint8_t *)Currency[ShmSysConfigAndInfo->SysConfig.BillingData.Currency]);
    }

    return PASS;
}

static void addFaultCodeToBuf(uint8_t *Code)
{
    uint8_t warningCount = ShmSysConfigAndInfo->SysWarningInfo.WarningCount;

    if (warningCount < 10) {
        memcpy(&ShmSysConfigAndInfo->SysWarningInfo.WarningCode[warningCount][0],
               Code,
               strlen((char *)Code));
        ShmSysConfigAndInfo->SysWarningInfo.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 < ShmSysConfigAndInfo->SysWarningInfo.WarningCount; i++) {
            if (find == 0x00) {
                if (memcmp(&ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i][0], _code, 7) == 0) {
                    find = 0x01;
                }
            } else {
                memcpy(&ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i - 1][0],
                       &ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i][0], 7);
            }
        }

        if (find) {
            ShmSysConfigAndInfo->SysWarningInfo.WarningCount--;
        }
    }
}

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};

    if (dataLen < WARNING_CODE_SIZE) {
        DEBUG_ERROR("raw data len < 6 = %d\r\n", dataLen);
        if (ShmSysConfigAndInfo->SysWarningInfo.WarningCount > 0) {
            for (i = 0; i < ShmSysConfigAndInfo->SysWarningInfo.WarningCount; i++) {
                usleep(128);
                if (ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i][1] >= 0x33) { //from backend or power cabinet
                    memset(EventCodeTmp, 0, sizeof(EventCodeTmp));
                    memcpy(EventCodeTmp,
                           ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i],
                           sizeof(EventCodeTmp));
                    removeFaultCodeToBuf(EventCodeTmp);
                }
            }
        }
        return FAIL;
    }

    for (count = 0; count < dataLen; count += WARNING_CODE_SIZE) {
        usleep(128);
        // misc command status code handle
        if (strcmp((char *)&data[count], MISC_ST_MISC_CMD) == 0) {
            if (gDoCommGblData.MiscCmd != REG_MISC_CONTROL) {
                gDoCommGblData.MiscCmd = REG_MISC_CONTROL;
            }
            memset((char *)&data[count], 0, WARNING_CODE_SIZE);
            continue;
        } else if (strcmp((char *)&data[count], MISC_ST_VERSION) == 0) {
            if (gDoCommGblData.MiscCmd != REG_REPORT_CSU_VERSION) {
                gDoCommGblData.MiscCmd = REG_REPORT_CSU_VERSION;
            }
            memset((char *)&data[count], 0, WARNING_CODE_SIZE);
            continue;
        }

        strncpy((char *)&StatusArray[count / WARNING_CODE_SIZE], (char *)&data[count], WARNING_CODE_SIZE);
        ret = 0;
        for (i = 0; i < ShmSysConfigAndInfo->SysWarningInfo.WarningCount; i++) {
            usleep(128);
            if (memcmp(&ShmSysConfigAndInfo->SysWarningInfo.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]);
    }

    for (i = 0; i < ShmSysConfigAndInfo->SysWarningInfo.WarningCount; i++) {
        usleep(128);
        if (ShmSysConfigAndInfo->SysWarningInfo.WarningCode[i][1] >= '3') { //from backend or power cabinet 0x33
            ret = 0;

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

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

    return PASS;
}

static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t id, uint8_t reg)
{
    int ret = PASS;
    uint8_t rawDataLen = 0;
    CsuResultPkt *pCsuResult = (CsuResultPkt *)pResult;
    SoftwareUpdInfo *pSoftwareUpd = 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];
        DEBUG_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(&PreSysWarningInfo, 0, sizeof(struct WARNING_CODE_INFO));
        memcpy(&PreSysWarningInfo,
               &(ShmSysConfigAndInfo->SysWarningInfo),
               sizeof(struct WARNING_CODE_INFO));
        break;

    case REG_CHARGING_CAP:
        chargingcapabilityHandle(pCsuResult->Data.Data, id - 1);
        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:
        //DEBUG_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;
        }
        return pCsuResult->Data.Data[0];
        break;

    case REG_CHARGING_PERMISSION:
        //DEBUG_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;
        }
        return pCsuResult->Data.Data[0];
        break;

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

    case REG_REPORT_CSU_VERSION:
    case REG_REPORT_OTHER_VERSION:
        break;

    case REG_PRESENT_CHARGING_INFO:
        break;

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

    case REG_WAIT_PLUG_IT_STATE:
        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;
    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) {
        DEBUG_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) {
        DEBUG_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) {
        DEBUG_ERROR("TCP socket RX data fail = %d\r\n", size);
        gDoCommGblData.DisConnCount++;
        return FAIL;
    }

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

    return ret;
}

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

    //printf("WaitForPlugit = %d\r\n", ShmSysConfigAndInfo->SysInfo.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;

    pPreChargingInfo->EvBatterySoc = htonl(ChargingData[plugNum]->EvBatterySoc);
    pPreChargingInfo->RemainChargingDuration = htonl(ChargingData[plugNum]->RemainChargingDuration);
    pPreChargingInfo->PresentChargingVoltage = htonl(ChargingData[plugNum]->PresentChargingVoltage);
    pPreChargingInfo->PresentChargingCurrent = htonl(ChargingData[plugNum]->PresentChargingCurrent);

    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], ShmSysConfigAndInfo->SysInfo.FanModuleFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.RelayModuleFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.Connector1FwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.Connector2FwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.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], ShmSysConfigAndInfo->SysInfo.CsuBootLoadFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.CsuKernelFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev, VERSION_BUF_SIZE);
    dataLen += VERSION_BUF_SIZE;
    memcpy(&data[dataLen], ShmSysConfigAndInfo->SysInfo.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)
{
    //int ret = PASS;

    //if ((ret = composeSocketData(fd,
    //                             id,
    //                             OP_READ_DATA,
    //                             REG_CHARGING_PERMISSION,
    //                             0,
    //                             NULL)) == FAIL) {
    //    return ret;
    //}

    return composeSocketData(fd,
                             id,
                             OP_READ_DATA,
                             REG_CHARGING_PERMISSION,
                             0,
                             NULL);
}

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[2] = {0};
    int ret = PASS;

    if (ChargingData[plugNum]->SystemStatus <= 2) {
        dataBuf[0] = 0;    //idle
    } else if ((ChargingData[plugNum]->SystemStatus <= 7) ||
               (ChargingData[plugNum]->SystemStatus == 17) ||
               (ChargingData[plugNum]->SystemStatus == 18)) {
        dataBuf[0] = 1;    //preparing
    } else if (ChargingData[plugNum]->SystemStatus == 8) {
        dataBuf[0] = 2;    //charging
    } else if (ChargingData[plugNum]->SystemStatus == 9) {
        dataBuf[0] = 3;    //terminating
    }

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

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

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

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

    ret = composeSocketData(fd,
                            0x01,
                            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};

    ChargingVolt = ChargingData[plugNum]->EvBatterytargetVoltage;
    ChargingAmp = ChargingData[plugNum]->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 warningCount = 0;
    uint8_t count = 0;
    uint8_t CurWarnCodeTmp[10][6] = {0};
    uint8_t PreWarnCodeTmp[10][6] = {0};
    int ret = PASS;

    if ((ShmSysConfigAndInfo->SysWarningInfo.WarningCount <= 0) &&
            (PreSysWarningInfo.WarningCount <= 0)) {
        return FAIL;
    }

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

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

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

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

    return ret;
}

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

    ret = composeSocketData(fd,
                            ID_OF_DISPENSER,
                            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,
           ShmSysConfigAndInfo->SysConfig.ModelName,
           strlen((char *)ShmSysConfigAndInfo->SysConfig.ModelName));

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

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

    ret = composeSocketData(fd,
                            ID_REGISTER,
                            OP_WRITE_DATA,
                            REG_MODEL_NAME,
                            strlen((char *)ShmSysConfigAndInfo->SysConfig.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 *)&ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress;

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

    return 0;
}

static int FindChargingInfoData(byte target, struct ChargingInfoData **chargingData)
{
    uint8_t index = 0;

    for (index = 0; index < CHAdeMO_QUANTITY; index++) {
        if (ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index].Index == target) {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index];
            return 1;
        }
    }

    for (index = 0; index < CCS_QUANTITY; index++) {
        if (ShmSysConfigAndInfo->SysInfo.CcsChargingData[index].Index == target) {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.CcsChargingData[index];
            return 1;
        }
    }

    for (index = 0; index < GB_QUANTITY; index++) {
        if (ShmSysConfigAndInfo->SysInfo.GbChargingData[index].Index == target) {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.GbChargingData[index];
            return 1;
        }
    }

    return 0;
}

static int InitShareMemory(void)
{
    int result = 1;
    int MeterSMId = 0;

    if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0) {
        DEBUG_ERROR("shmget ShmSysConfigAndInfo NG\n");
        result = 0;
    } else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("[shmat ShmSysConfigAndInfo NG\n");
        result = 0;
    }

    if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData),  0777)) < 0) {
        DEBUG_ERROR("shmget ShmStatusCodeData NG\n");
        result = 0;
    } else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("shmat ShmStatusCodeData NG\n");
        result = 0;
    }

    //creat ShmPsuData
    if ((MeterSMId = shmget(ShmPsuKey, sizeof(struct PsuData),  0777)) < 0) {
        DEBUG_ERROR("shmget ShmPsuData NG \n");
        result = 0;
    } else if ((ShmPsuData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("shmat ShmPsuData NG \n");
        result = 0;
    }

    //creat ShmOCPP16Data
    if ((MeterSMId = shmget(ShmOcppModuleKey, sizeof(struct OCPP16Data),  0777)) < 0) {
        DEBUG_ERROR("shmget ShmOCPP16Data NG \n");
        result = 0;
    } else if ((ShmOCPP16Data = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("shmat ShmOCPP16Data NG \n");
        result = 0;
    }

    if ((MeterSMId = shmget(ShmPrimaryMcuKey, sizeof(struct PrimaryMcuData), 0777)) < 0) {
        DEBUG_ERROR("shmget ShmPrimaryMcuData NG\n");
        result = 0;
    } else if ((ShmPrimaryMcuData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("shmat ShmPrimaryMcuData NG\n");
        result = 0;
    }

    //creat Audi customization info
    if ((MeterSMId = shmget(ShmAudiCustInfoKey, sizeof(AudiCustInfo), IPC_CREAT | 0777)) < 0) {
        DEBUG_ERROR("[main]CreatShareMemory:shmget select gun info NG \n");
        return 0;
    } else if ((gAudiCustInfo = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        DEBUG_ERROR("[main]CreatShareMemory:shmat shmget select gun info \n");
        return 0;
    }

    return result;
}

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

    if (ShmSysConfigAndInfo->SysWarningInfo.Level != 2) {
        for (plugNum = 0; plugNum < totalConnCount; plugNum++) {
            if (ChargingData[plugNum]->SystemStatus != S_IDLE &&
                    ChargingData[plugNum]->SystemStatus != S_RESERVATION &&
                    ChargingData[plugNum]->SystemStatus != S_MAINTAIN) {
                canUpdateFirmware = false;
            }
        }
    }

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

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

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

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

        if (DiffTimeb(gRegTimeUp[plugNum][REG_USER_ID], AuthNowTime) > LOOP_RETRY_TIME) {
            ret = writeUserID(fd,
                              ShmSysConfigAndInfo->SysInfo.CurGunSelected + 1,
                              ShmSysConfigAndInfo->SysConfig.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("gAudiCustInfo->PricesInfo[plugNum].Balance = %.2f, %.2f\r\n",
                //       gAudiCustInfo->PricesInfo[plugNum].Balance,
                //       FAIL_BALANCE_PRICES);
                if (gAudiCustInfo->PricesInfo[plugNum].Balance != FAIL_BALANCE_PRICES) {
                    ShmOCPP16Data->SpMsg.bits.AuthorizeConf    = 1; //isAuthorizedComplete
                    ShmOCPP16Data->SpMsg.bits.AuthorizeReq     = 0;
                    ShmSysConfigAndInfo->SysInfo.AuthorizeFlag = 0;
                    ShmPsuData->SystemAvailablePower           = 0;
                    ShmPsuData->SystemPresentPsuQuantity       = 0;
                }
            }
            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;
            }
            break;

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

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

        case REG_DISPENSER_STATUS:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME) {
                writeDispenserStatus(fd);
                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) > LOOP_RETRY_TIME * 10) {
        //            readSoftwareUpdate(fd);
        //            ftime(&gRegTimeUp[plugNum][curReg]);
        //        }
        //        while (ShmSysConfigAndInfo->SysInfo.FirmwareUpdate == YES) {
        //            ftime(&NowTime);
        //            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME) {
        //                readSoftwareUpdate(fd);
        //            }
        //            usleep(128);
        //        }
        //    }
        //    isContinue = 0;
        //    break;

        case REG_PLUG_IN_STATE:
            if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME) {
                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) {
                writeConnectorState(fd, plugNum, gunID);
                ftime(&gRegTimeUp[plugNum][curReg]);
            }

            //check misc command from power cabinet
            if (gDoCommGblData.MiscCmd != 0) {
                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)) {
                    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:
            DEBUG_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 (ShmSysConfigAndInfo->SysInfo.WaitForPlugit == YES) {
        return YES;
    }

    return NO;
}

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

    switch (ChargingData[plugNum]->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 (gAudiCustInfo->PricesInfo[plugNum].Balance != FAIL_BALANCE_PRICES) {
            ftime(&AuthNowTime);
            if (DiffTimeb(gRegTimeUp[plugNum][REG_WAIT_PLUG_IT_STATE], AuthNowTime) > (LOOP_RETRY_TIME / 10)) {
                writeWaitPlugItState(fd, gunID);
                ftime(&gRegTimeUp[plugNum][REG_WAIT_PLUG_IT_STATE]);
            }

            ftime(&AuthNowTime);
            if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME) {
                readChargingCapability(fd, 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) {
            writeConnectorState(fd, plugNum, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CONNECTOR_STATE]);
        }

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME) {
            if (readChargePermission(fd, gunID) && readChargingCapability(fd, gunID)) {
                ShmPsuData->SystemAvailablePower = (ChargingData[0]->AvailableChargingPower +
                                                    ChargingData[1]->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) {
            writePresentChargingInfo(fd, plugNum, 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) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }

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

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME) {
            writePresentChargingInfo(fd, plugNum, 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);

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

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

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

    case S_CHARGING: //charging
    case S_TERMINATING:
        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_CAP], AuthNowTime) > LOOP_RETRY_TIME) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }

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

        ftime(&AuthNowTime);
        if (DiffTimeb(gRegTimeUp[plugNum][REG_PRESENT_CHARGING_INFO], AuthNowTime) > LOOP_RETRY_TIME) {
            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) {
            readChargingCapability(fd, gunID);
            ftime(&gRegTimeUp[plugNum][REG_CHARGING_CAP]);
        }
        break;

    default:
        break;
    }
}

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

    /**************** Check Network **********/
    DEBUG_INFO("Check Dispenser Network Information");
    while (!CheckNetworkStatus()) {
        calDisconnectCount(&gDoCommGblData.DisConnCount, CHECK_NETWORK_FAIL_COUNT);
        DEBUG_ERROR("disconnect count = % d\r\n", gDoCommGblData.DisConnCount);
    }
    DEBUG_INFO("Dispenser Network Information checked: IP = % s Netmask = % s, Gateway = % s",
               ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress,
               ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthSubmaskAddress,
               ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthGatewayAddress);

    gDoCommGblData.DisConnCount = 0;

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

    gDoCommGblData.DisConnCount = 0;

    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;
    int fd = 0;
    int isContinue = 1;

    InitSocketSigPipe();

    /**************** Initialization **********/
    if (InitShareMemory() == 0) {
        if (ShmStatusCodeData != NULL) {
            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = YES;
        }

        DEBUG_ERROR("DoComm Initial share memory fail\r\n");
        sleep(5);

        return 0;
    }

    totalConnCount =  ShmSysConfigAndInfo->SysConfig.TotalConnectorCount;

    for (index = 0; index < totalConnCount; index++) {
        if (!FindChargingInfoData(index, &ChargingData[0])) {
            DEBUG_ERROR("FindChargingInfoData false \n");
            break;
        }
        gAudiCustInfo->PricesInfo[index].Balance = FAIL_BALANCE_PRICES;
    }

    //initial trigger message timer
    for (index = 0; index < MAX_REGISTER_NUM; index++) {
        ftime(&gRegTimeUp[0][index]);
        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 {
            checkAuthorProcess(fd, 0);

            plugNum = 0;
            gunID = 0;
            for (plugNum = 0; plugNum < totalConnCount; plugNum++) {
                //plugNum = setup chargingData value for bottom layer
                gunID = (plugNum  + 1); //connector Id, 1 = left gun, 2 = right gun
                systemStatusProcess(fd, plugNum, gunID);

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

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

        if (initDone == DISPENSER_SOCKET_RECONN) {
            DEBUG_INFO("Disconnected from power cabinet...re-connecting");
            setTcpStatus(ABNORMAL);

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

            closeSocket(fd);
            fd = 0;

            initDone = DISPENER_INIT_FAIL;
        }

        //update dispenser firmware
        if (initDone != DISPENER_INIT_FAIL) {
            updateFirmwareProcess(fd, totalConnCount);
        }

        usleep(1000);
    }
}