/*===========================================================================
                    Combined Charging System (CCS): SECC
                                 EVComm.c

                        initiated by Vern, Joseph
                           (since 2019/07/19)
   =============================================================================*/
#include <netinet/ip.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <linux/termios.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <stdlib.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/mman.h>
#include <linux/sockios.h>
#include <linux/socket.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/time.h>
#include <sys/timeb.h>
#include <math.h>//for pow
#include <unistd.h>
#include <linux/if_packet.h>
#include <netinet/if_ether.h>
#include <net/if.h>
#include <netinet/ether.h>

#include "define.h"
#include "EvComm.h"
#include "NidNmk.h"
#include "EvComm.h"

#define Debug

struct SysConfigAndInfo         *ShmSysConfigAndInfo;
struct StatusCodeData           *ShmStatusCodeData;
struct CcsData              	*ShmCcsData;
struct InternalComm         	*ShmInternalComm;

pid_t PilotDetectionPid;
enum MsgFlowStatus V2gFlowStatus;
int RawSock,UdpSock,TcpSock;
unsigned char                   *RecvBuffer,*SendBuffer;
int RecvBufferSize=64*1024;
int SendBufferSize=64*1024;
unsigned short Aag[64];
struct MmeHeader SendMmePacket;
int SendMmePacketSize;
unsigned char CsuMac[6],QcaMac[6],EvMac[6],SlacRunId[8];
struct sockaddr_ll DestSocketAddress;
struct ifreq Req;
unsigned int PwmStartTime;
struct timeb SeqStartTime,SeqEndTime;
unsigned char AagGroupsNum, MnbcSoundNum,AttenProfileNum;
unsigned char NewNmkKey[16],Nid[7];


#ifdef SystemLogMessage
int StoreLogMsg(unsigned char *DataString)
{
    unsigned char Buf[256];
    time_t CurrentTime;
    struct tm *tm;

    memset(Buf,0,sizeof(Buf));
    CurrentTime = time(NULL);
    tm=localtime(&CurrentTime);
    sprintf(Buf,"echo \"%04d.%02d.%02d %02d:%02d:%02d - %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,
            DataString,
            tm->tm_year+1900,
			tm->tm_mon+1
		   );
    system(Buf);
    #ifdef Debug
    printf("%s \n",DataString);
    #endif
}
#endif

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

int CreatShareMemory()
{
    int MeterSMId;

    //creat ShmSysConfigAndInfo
    if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo), IPC_CREAT | 0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmget ShmSysConfigAndInfo NG");
        #endif
        return 0;
    }
    else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmat ShmSysConfigAndInfo NG");
        #endif
        return 0;
    }
    memset(ShmSysConfigAndInfo,0,sizeof(struct SysConfigAndInfo));
    //creat ShmStatusCodeData
    if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData), IPC_CREAT | 0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmget ShmStatusCodeData NG");
        #endif
        return 0;
    }
    else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmat ShmStatusCodeData NG");
        #endif
        return 0;
    }
    memset(ShmStatusCodeData,0,sizeof(struct StatusCodeData));
    //creat ShmCcsData
    if ((MeterSMId = shmget(ShmCcsCommKey, sizeof(struct CcsData), IPC_CREAT | 0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmget ShmCcsData NG");
        #endif
        return 0;
    }
    else if ((ShmCcsData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[main]CreatShareMemory:shmat ShmCcsData NG");
        #endif
        return 0;
    }
    memset(ShmCcsData,0,sizeof(struct CcsData));
    return 1;
}

int InitShareMemory()
{
    int MeterSMId;

    //creat ShmSysConfigAndInfo
    if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmget ShmSysConfigAndInfo NG");
        #endif
        return 0;
    }
    else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmat ShmSysConfigAndInfo NG");
        #endif
        return 0;
    }
    //creat ShmStatusCodeData
    if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmget ShmStatusCodeData NG");
        #endif
        return 0;
    }
    else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
            #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmat ShmStatusCodeData NG");
        #endif
        return 0;
    }

    //creat ShmCcsData
    if ((MeterSMId = shmget(ShmCcsCommKey, sizeof(struct CcsData),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmget ShmCcsData NG");
        #endif
        return 0;
    }
    else if ((ShmCcsData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
            #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmat ShmCcsData NG");
        #endif
        return 0;
    }

    //creat ShmInternalComm
    if ((MeterSMId = shmget(ShmInternalCommKey, sizeof(struct InternalComm),   0777)) < 0)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmget ShmInternalComm NG");
        #endif
        return 0;
    }
    else if ((ShmInternalComm = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]InitShareMemory:shmat ShmInternalComm NG");
        #endif
        return 0;
    }

    return 1;
}

int GetEthMac(unsigned char *Eth, unsigned char *mac)
{
    //Parameters:MAC,IP,Mask,Gateway
    int fd,rd=0;
    unsigned char addr[18], Buffer[128];

    memset(Buffer,0,sizeof(Buffer));
    sprintf(Buffer,"cat /sys/class/net/%s/address > /mnt/GetEthInfo",Eth);
    system(Buffer);
    fd = open("/mnt/GetEthInfo", O_RDONLY);
    if(fd<0)
    {
        system("rm -f /mnt/GetEthInfo");
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]GetEthMac: MAC Address open error");
        #endif
        return 0;
    }
    memset(mac,0,6);
    memset(addr,0,sizeof(addr));
    rd=read(fd,addr,17);
    close(fd);
    system("rm -f /mnt/GetEthInfo");
    sscanf(addr, "%02x:%02x:%02x:%02x:%02x:%02x", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]);
    return 1;
}

float ReadAdcVolt(unsigned char AdcChannel)
{
    //AIN0=CCS GUN Temp 1
    //AIN1=CCS GUN Temp 2
    //AIN2=CCS_Proximity/2
    //AIN3=pilot voltage
    FILE *fp;
    unsigned char str[64];
    unsigned char AdcValue[8];

    if(AdcChannel>7)
        return -1;
    memset(str,0,sizeof(str));
    memset(AdcValue,0,sizeof(AdcValue));
    sprintf(str,"cat /sys/bus/iio/devices/iio\\:device0/in_voltage%d_raw",AdcChannel);
    fp=popen(str, "r");
    if(fgets(AdcValue,sizeof(AdcValue),fp)==NULL)
    {
        pclose(fp);
        return -1;
    }
    pclose(fp);
    //Vin =  Vref *D / (2^n - 1)
    return ((float)1.8*(float)atoi(AdcValue))/4095;
}

void Qca7kPowerReset()
{
    system("echo 1 > /sys/class/gpio/gpio88/value");
    usleep(500000);
    system("echo 0 > /sys/class/gpio/gpio88/value");
}

void SwitchCpStateE(unsigned char OnOff)
{
    //OnOff=1 => switch State to E
    //OnOff=0 => return noraml
    if(OnOff==0)
        system("echo 0 > /sys/class/gpio/gpio86/value");
    else
        system("echo 1 > /sys/class/gpio/gpio86/value");
}

int OutputCpPwmDuty(unsigned char Duty)
{
    unsigned char str[128];
    int DutyInNanoSec;

    if((Duty<0)||(Duty>100))
        return -1;

    memset(str,0,sizeof(str));
    DutyInNanoSec=10000*Duty;
    sprintf(str,"echo %d > /sys/class/pwm/pwmchip0/pwm0/duty_cycle",DutyInNanoSec);//nanoseconds
    system(str);
    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty=Duty;
    return 0;
}

int ConnectorPlugIn()
{
    //return 0 => unplug
    //return 1 => plug-in
    float TmpProximity;
    unsigned char Rtn=0;

    if(((FirmwareVersion&0x0000FF00)>>8)==0x01)
    {
        //UL version: judge with CP and proximity
        TmpProximity=ReadAdcVolt(2);
        if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState>=2)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState<=5)&&
           (TmpProximity>=0.4)&&(TmpProximity<=0.9))
        {
            Rtn=1;
        }
    }
    else
    {
        //CE version: judge with CP only
        if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState>=2)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState<=5))
            Rtn=1;
    }
    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].ConnectorPlugIn=Rtn;
    return Rtn;
}

void PilotDetection()
{
    pid_t tmp=0;
    struct timeb StartTime,EndTime;
    unsigned char CpState[3],count;
    float TmpVolt;

    if(PilotDetectionPid==0)
    {
        tmp=fork();
        if(tmp>0)
        {
            PilotDetectionPid=tmp;
            {
                unsigned char buf[64];
                memset(buf,0,sizeof(buf));
                sprintf(buf,"renice -20 -p %d",tmp);
                system(buf);
            }
            return;
        }
    }

    while(1)
    {
        //ADC_Voltage = (1+RE62/RE63)*0.9 - (RE62/RE63)*Pilot_Voltage,  RE62/RE63=0.06
        //=>Pilot_Voltage=(0.954-ADC_Voltage)/0.06
        //ftime(&StartTime);
        TmpVolt=0;
        for(count=0; count<3; count++)
        {
            TmpVolt+=(0.954-ReadAdcVolt(3))/0.06;
        }
        TmpVolt/=3;
        ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotVoltage=TmpVolt;
        if(TmpVolt>=13.5)
        {
            //Pilot Error
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==1)&&(TmpVolt<13.75))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=1;
            else
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=8;
        }
        else if((TmpVolt>=10.5)&&(TmpVolt<13.5))
        {
            //State A
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState>=2)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState<=3)&&(TmpVolt<10.75))
            {
                if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty>=5)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty<100))
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=3;
                else
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=2;
            }
            else if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==8)&&(TmpVolt>=13.25))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=8;
            else
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=1;
        }
        else if((TmpVolt>=7.5)&&(TmpVolt<10.5))
        {
            //State B
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==4)&&(TmpVolt<7.75))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=4;
            else if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==1)&&(TmpVolt>=10.25))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=1;
            else
            {
                if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty>=5)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty<100))
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=3;
                else
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=2;
            }
        }
        else if((TmpVolt>=4.5)&&(TmpVolt<7.5))
        {
            //State C
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==5)&&(TmpVolt<4.75))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=5;
            else if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState>=2)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState<=3)&&(TmpVolt>=7.25))
            {
                if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty>=5)&&(ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotDuty<100))
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=3;
                else
                    ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=2;
            }
            else
            {
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=4;
            }
        }
        else if((TmpVolt>=1.5)&&(TmpVolt<4.5))
        {
            //State D
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==6)&&(TmpVolt<1.75))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=6;
            else if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==4)&&(TmpVolt>=4.25))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=4;
            else
            {
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=5;
            }
        }
        else if((TmpVolt>= -1.5)&&(TmpVolt<1.5))
        {
            //State E
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==8)&&(TmpVolt< -1.25))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=8;
            else if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==5)&&(TmpVolt>=1.25))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=5;
            else
            {
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=6;
            }
        }
        else if((TmpVolt>= -13.5)&&(TmpVolt< -10.5))
        {
            //State F
            if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==8)&&(TmpVolt>= -10.75))
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=8;
            else
            {
                ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState=7;
            }
        }
        //ftime(&EndTime);
        //printf("duration:%d\n", DiffTimeb(StartTime, EndTime));
    }//while
}

int MmeProcess(unsigned char *Buffer, int DataLength)
{
    //struct ethhdr	*EthPacket;
    struct MmeHeader    *MmePacket;
    int Rtn;
    static unsigned char counter;

    MmePacket = (struct MmeHeader *)Buffer;

    #ifdef Debug
    printf("\n\n***********************************\n");
    printf("***** Received MME Packet *****\n");
    printf("***********************************\n");
    printf("DataLength=%d\n",DataLength);
    printf("ODA: %02x:%02x:%02x:%02x:%02x:%02x\n",
           MmePacket->ODA[0],MmePacket->ODA[1],MmePacket->ODA[2],MmePacket->ODA[3],MmePacket->ODA[4],MmePacket->ODA[5]);
    printf("OSA: %02x:%02x:%02x:%02x:%02x:%02x\n",
           MmePacket->OSA[0],MmePacket->OSA[1],MmePacket->OSA[2],MmePacket->OSA[3],MmePacket->OSA[4],MmePacket->OSA[5]);
    printf("MTYPE: 0x%x\n", htons(MmePacket->MTYPE));
    printf("MMV: 0x%x\n", MmePacket->MMV);
    printf("MMTYPE: 0x%x\n", MmePacket->MMTYPE);
    printf("FMI 0x%x, 0x%x\n", MmePacket->FMI[0],MmePacket->FMI[1]);
    #endif

    //check if the destination address is broadcast or EVSE itself, it can be ignore if not belong to itself.
    switch(MmePacket->MMTYPE)
    {
	    case MMTYPE_CM_SET_KEY_CNF:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_SET_KEY_CNF ---\n");
	        printf("Result: 0x%x\n", MmePacket->MMENTRY[0]);
	        #endif
	        V2gFlowStatus=CM_SET_KEY_CNF;
	        break;
		}

	    case MMTYPE_CM_SLAC_PARM_REQ:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_SLAC_PARM_REQ ---\n");
	        printf("APPLICATION_TYPE: 0x%x\n", MmePacket->MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", MmePacket->MMENTRY[1]);
	        printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[2],MmePacket->MMENTRY[3],MmePacket->MMENTRY[4],MmePacket->MMENTRY[5],MmePacket->MMENTRY[6],
	               MmePacket->MMENTRY[7],MmePacket->MMENTRY[8],MmePacket->MMENTRY[9]);
	        printf("CipherSuiteSetSize: 0x%x\n", MmePacket->MMENTRY[10]);
	        printf("CipherSuite [1]: 0x%x,0x%x\n", MmePacket->MMENTRY[11], MmePacket->MMENTRY[12]);
	        #endif
	        V2gFlowStatus=CM_SLAC_PARM_REQ;
	        memcpy(EvMac,MmePacket->OSA,sizeof(EvMac));
	        memcpy(DestSocketAddress.sll_addr,MmePacket->OSA,sizeof(EvMac));
	        memcpy(SlacRunId,MmePacket->MMENTRY+2,sizeof(SlacRunId));
	        memset(&SendMmePacket,0,sizeof(struct MmeHeader));
	        memcpy(SendMmePacket.ODA,MmePacket->OSA,6);
	        memcpy(SendMmePacket.OSA,CsuMac,6);
	        SendMmePacket.MTYPE=htons(EtherType_HomePlug);
	        SendMmePacket.MMV=MmePacket->MMV;
	        SendMmePacket.MMTYPE=MMTYPE_CM_SLAC_PARM_CNF;
	        SendMmePacket.FMI[0]=SendMmePacket.FMI[1]=0;
	        SendMmePacketSize=0;
	        memset(SendMmePacket.MMENTRY,0xFF,6);       //Fixed value indicating that M-Sounds to be sent as Ethernet broadcast
	        SendMmePacketSize+=6;
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=C_EV_match_MNBC;
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=TT_EVSE_match_MNBC;
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x01;        //Fixed value indicating ��Other GP station��, 0x00 �V HLE of the STA, 0x01 �V Another GP STA, 0x02 �V 0xFF �V Reserved
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,EvMac,6);
	        SendMmePacketSize+=6;
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0;           //Fixed value indicating ��PEVEVSEMatching��
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0;           //Fixed value indicating ��No Security��
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,SlacRunId,sizeof(SlacRunId));
	        SendMmePacketSize+=sizeof(SlacRunId);
	        SendMmePacketSize+=19;      //the size before MMENTRY
	        #ifdef Debug
	        printf("\n\n***** Response MME Packet *****\n");
	        printf("SendMmePacket.ODA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	               SendMmePacket.ODA[0],SendMmePacket.ODA[1],SendMmePacket.ODA[2],SendMmePacket.ODA[3],SendMmePacket.ODA[4],SendMmePacket.ODA[5]);
	        printf("SendMmePacket.OSA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	               SendMmePacket.OSA[0],SendMmePacket.OSA[1],SendMmePacket.OSA[2],SendMmePacket.OSA[3],SendMmePacket.OSA[4],SendMmePacket.OSA[5]);
	        printf("MTYPE: 0x%x\n", htons(SendMmePacket.MTYPE));
	        printf("MMV: 0x%x\n", SendMmePacket.MMV);
	        printf("MMTYPE: 0x%x\n", SendMmePacket.MMTYPE);
	        printf("FMI 0x%x, 0x%x\n", SendMmePacket.FMI[0],SendMmePacket.FMI[1]);
	        printf("--- CM_SLAC_PARM_CNF ---\n");
	        printf("M-SOUND_TARGET: %02x:%02x:%02x:%02x:%02x:%02x\n",
	               SendMmePacket.MMENTRY[0],SendMmePacket.MMENTRY[1],SendMmePacket.MMENTRY[2],SendMmePacket.MMENTRY[3],
	               SendMmePacket.MMENTRY[4],SendMmePacket.MMENTRY[5]);
	        printf("NUM_SOUNDS: 0x%x\n", SendMmePacket.MMENTRY[6]);
	        printf("Time_Out: 0x%x\n", SendMmePacket.MMENTRY[7]);
	        printf("RESP_TYPE: 0x%x\n", SendMmePacket.MMENTRY[8]);
	        printf("M-FORWARDING_STA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	               SendMmePacket.MMENTRY[9],SendMmePacket.MMENTRY[10],SendMmePacket.MMENTRY[11],SendMmePacket.MMENTRY[12],
	               SendMmePacket.MMENTRY[13],SendMmePacket.MMENTRY[14]);
	        printf("APPLICATION_TYPE: 0x%x\n", SendMmePacket.MMENTRY[15]);
	        printf("SECURITY_TYPE: 0x%x\n", SendMmePacket.MMENTRY[16]);
	        printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               SendMmePacket.MMENTRY[17],SendMmePacket.MMENTRY[18],SendMmePacket.MMENTRY[19],SendMmePacket.MMENTRY[20],
	               SendMmePacket.MMENTRY[21],SendMmePacket.MMENTRY[22],SendMmePacket.MMENTRY[23],SendMmePacket.MMENTRY[24]);
	        #endif
	        V2gFlowStatus=CM_SLAC_PARM_CONF;
	        Rtn=sendto(RawSock, &SendMmePacket, SendMmePacketSize, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
	        #ifdef Debug
	        printf("SendMmePacketSize=%d,Rtn=%d\n",SendMmePacketSize,Rtn);
	        #endif
	        ftime(&SeqStartTime);
	        counter=0;
	        break;
		}

	    case MMTYPE_CM_START_ATTEN_CHAR_IND:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_START_ATTEN_CHAR_IND (counter : %d/3 ) ---\n",counter+1);
	        printf("APPLICATION_TYPE: 0x%x\n", MmePacket->MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", MmePacket->MMENTRY[1]);
	        printf("NUM_SOUNDS: 0x%x\n", MmePacket->MMENTRY[2]);
	        printf("Time_Out 0x%x\n", MmePacket->MMENTRY[3]);
	        printf("RESP_TYPE 0x%x\n", MmePacket->MMENTRY[4]);    //Fixed value (0x01) indicating ��other Green PHY station��
	        printf("FORWARDING_STA: %02x:%02x:%02x:%02x:%02x:%02x\n",
	               MmePacket->MMENTRY[5],MmePacket->MMENTRY[6],MmePacket->MMENTRY[7],MmePacket->MMENTRY[8],
	               MmePacket->MMENTRY[9],MmePacket->MMENTRY[10]);
	        printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[11],MmePacket->MMENTRY[12],MmePacket->MMENTRY[13],MmePacket->MMENTRY[14],
	               MmePacket->MMENTRY[15],MmePacket->MMENTRY[16],MmePacket->MMENTRY[17],MmePacket->MMENTRY[18]);
	            #endif
	        MnbcSoundNum=MmePacket->MMENTRY[2];
	        V2gFlowStatus=CM_START_ATTEN_CHAR_IND;
	        counter++;
	        if(counter==1)
	        {
	            memset(Aag,0,sizeof(Aag));
	            AttenProfileNum=0;
	            ftime(&SeqStartTime);       //start TT_EVSE_match_MNBC
	        }
	        else if(counter>=3)
	        {
	            counter=0;
	        }
	        break;
		}

	    case MMTYPE_CM_MNBC_SOUND_IND:
		{
	        if(V2gFlowStatus>=CM_ATTEN_CHAR_IND)
	            break;
	        #ifdef Debug
	        printf("--- MMTYPE_CM_MNBC_SOUND_IND (counter : %d/%d) ---\n",counter+1,MnbcSoundNum);
	        printf("APPLICATION_TYPE: 0x%x\n", MmePacket->MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", MmePacket->MMENTRY[1]);
	        printf("SenderID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[2],MmePacket->MMENTRY[3],MmePacket->MMENTRY[4],MmePacket->MMENTRY[5],
	               MmePacket->MMENTRY[6],MmePacket->MMENTRY[7],MmePacket->MMENTRY[8],MmePacket->MMENTRY[9],
	               MmePacket->MMENTRY[10],MmePacket->MMENTRY[11],MmePacket->MMENTRY[12],MmePacket->MMENTRY[13],
	               MmePacket->MMENTRY[14],MmePacket->MMENTRY[15],MmePacket->MMENTRY[16],MmePacket->MMENTRY[17],
	               MmePacket->MMENTRY[18]);
	        printf("Cnt: 0x%x\n", MmePacket->MMENTRY[19]);
	        printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[20],MmePacket->MMENTRY[21],MmePacket->MMENTRY[22],MmePacket->MMENTRY[23],
	               MmePacket->MMENTRY[24],MmePacket->MMENTRY[25],MmePacket->MMENTRY[26],MmePacket->MMENTRY[27]);
	        printf("RSVD: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[28],MmePacket->MMENTRY[29],MmePacket->MMENTRY[30],MmePacket->MMENTRY[31],
	               MmePacket->MMENTRY[32],MmePacket->MMENTRY[33],MmePacket->MMENTRY[34],MmePacket->MMENTRY[35]);
	        printf("Rnd: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[36],MmePacket->MMENTRY[37],MmePacket->MMENTRY[38],MmePacket->MMENTRY[39],
	               MmePacket->MMENTRY[40],MmePacket->MMENTRY[41],MmePacket->MMENTRY[42],MmePacket->MMENTRY[43],
	               MmePacket->MMENTRY[44],MmePacket->MMENTRY[45],MmePacket->MMENTRY[46],MmePacket->MMENTRY[47],
	               MmePacket->MMENTRY[48],MmePacket->MMENTRY[49],MmePacket->MMENTRY[50],MmePacket->MMENTRY[51]);
	        #endif
	        V2gFlowStatus=CM_MNBC_SOUND_IND;
	        counter++;
	        break;
		}

	    case MMTYPE_CM_ATTEN_PROFILE_IND:
		{
	        if(V2gFlowStatus>=CM_ATTEN_CHAR_IND)
	        {
				break;
			}

	        #ifdef Debug
	        printf("--- MMTYPE_CM_ATTEN_PROFILE_IND (counter : %d/%d) ---\n",counter,MnbcSoundNum);
	        printf("EV MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
	               MmePacket->MMENTRY[0],MmePacket->MMENTRY[1],MmePacket->MMENTRY[2],MmePacket->MMENTRY[3],
	               MmePacket->MMENTRY[4],MmePacket->MMENTRY[5]);
	        printf("NumGroups: 0x%x\n", MmePacket->MMENTRY[6]);
	        printf("RSVD: 0x%x\n", MmePacket->MMENTRY[7]);
	        printf("AAG: \n");
	        for(Rtn=0; Rtn<MmePacket->MMENTRY[6]; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[8+Rtn]);
	        printf("\n");
	            #endif
	        AagGroupsNum=MmePacket->MMENTRY[6];
	        for(Rtn=0; Rtn<MmePacket->MMENTRY[6]; Rtn++)
	            Aag[Rtn]+=MmePacket->MMENTRY[8+Rtn];
	        AttenProfileNum++;
	        V2gFlowStatus=CM_MNBC_SOUND_IND;
	        break;
		}

	    case MMTYPE_CM_ATTN_CHAR_RSP:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_ATTN_CHAR_RSP ---\n");
	        printf("APPLICATION_TYPE: 0x%x\n", MmePacket->MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", MmePacket->MMENTRY[1]);
	        printf("SOURCE_ADDRESS: %02x:%02x:%02x:%02x:%02x:%02x\n",
	               MmePacket->MMENTRY[2],MmePacket->MMENTRY[3],MmePacket->MMENTRY[4],MmePacket->MMENTRY[5],
	               MmePacket->MMENTRY[6],MmePacket->MMENTRY[7]);
	        printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	               MmePacket->MMENTRY[8],MmePacket->MMENTRY[9],MmePacket->MMENTRY[10],MmePacket->MMENTRY[11],
	               MmePacket->MMENTRY[12],MmePacket->MMENTRY[13],MmePacket->MMENTRY[14],MmePacket->MMENTRY[15]);
	        printf("SOURCE_ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[16+Rtn]);
	        printf("\n");
	        printf("RESP_ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[33+Rtn]);
	        printf("\n");
	        printf("Result: 0x%x\n", MmePacket->MMENTRY[50]);    //Fixed value of 0x00 indicates a successful SLAC process
	            #endif
	        V2gFlowStatus=CM_ATTEN_CHAR_RSP;
	        ftime(&SeqStartTime);
	        break;
		}

	    case MMTYPE_CM_VALIDATE_REQ:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_VALIDATE_REQ ---\n");
	        printf("Signal Type: 0x%x\n", MmePacket->MMENTRY[0]);    //Fixed value (0x00) to indicate ��PEV S2 toggles on control pilot line��
	        printf("Timer: 0x%x\n", MmePacket->MMENTRY[1]);    //Fixed value In the first VALIDATE Request-Response exchange, the Timer field shall be set to zero.
	        printf("Result: 0x%x\n", MmePacket->MMENTRY[2]);    //Fixed value In the first VALIDATE Request-Response exchange, the Result field shall be set to 0x01 = ��ready��.
	        #endif
	        counter=0;

	        for(Rtn=0; Rtn<6; Rtn++)
	        {
	            if(MmePacket->ODA[Rtn]!=CsuMac[Rtn])
	            {
	                counter=1;
	                break;
	            }
	        }

	        memset(&SendMmePacket,0,sizeof(struct MmeHeader));
	        memcpy(SendMmePacket.ODA,EvMac,6);
	        memcpy(SendMmePacket.OSA,CsuMac,6);
	        SendMmePacket.MTYPE=htons(EtherType_HomePlug);
	        SendMmePacket.MMV=0x01;
	        SendMmePacket.MMTYPE=MMTYPE_CM_VALIDATE_CNF;
	        SendMmePacket.FMI[0]=SendMmePacket.FMI[1]=0;
	        SendMmePacketSize=0;
	        if(counter==0)
	        {
	            //First MMTYPE_CM_VALIDATE_REQ because Unicast
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=0;    //Fixed value to indicate ��PEV S2 toggles on control pilot line��
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=0;    //Fixed value In the first VALIDATE Request-Response exchange, the ToggleNum field shall be set to zero.
	                #ifdef SupportBcbToggle
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=1;    //0x01 = Ready
	                #else
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=4;    //0x04 = Not Required
	                #endif
	        }
	        else
	        {
	            //second MMTYPE_CM_VALIDATE_REQ because Broadcast
	            unsigned char PreStatus=3,ToggleNum=0;
	            ftime(&SeqStartTime);
	            while(1)
	            {
	                ftime(&SeqEndTime);
	                if((ShmSysConfigAndInfo->SysInfo.CcsChargingData[0].PilotState==4)&&(PreStatus==3))
	                {
	                    ToggleNum++;
	                    PreStatus=4;
	                }
	                else
	                {
	                    PreStatus=3;
	                }
	                if(DiffTimeb(SeqStartTime, SeqEndTime)>=(SendMmePacket.MMENTRY[1]*100+100))
	                {
	                    SendMmePacket.MMENTRY[SendMmePacketSize++]=0;    //Fixed value to indicate ��PEV S2 toggles on control pilot line��
	                    SendMmePacket.MMENTRY[SendMmePacketSize++]= ToggleNum;
	                        #ifdef SupportBcbToggle
	                    SendMmePacket.MMENTRY[SendMmePacketSize++]=2;    //0x02 = Success
	                        #else
	                    SendMmePacket.MMENTRY[SendMmePacketSize++]=4;    //0x04 = Not Required
	                        #endif
	                    break;
	                }
	            }
	        }
	        SendMmePacketSize+=19;      //the size before MMENTRY
	        sendto(RawSock, &SendMmePacket, SendMmePacketSize, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
	        V2gFlowStatus=CM_VALIDATE_CNF;
	        ftime(&SeqStartTime);
	        break;
		}

	    case MMTYPE_CM_SLAC_MATCH_REQ:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_CM_SLAC_MATCH_REQ ---\n");
	        printf("APPLICATION_TYPE: 0x%x\n", MmePacket->MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", MmePacket->MMENTRY[1]);
	        printf("MVFLength: 0x%x, 0x%x\n", MmePacket->MMENTRY[2],MmePacket->MMENTRY[3]);    //Fixed value (0x3E) for matching
	        printf("PEV ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[4+Rtn]);
	        printf("\n");
	        printf("PEV MAC: \n");
	        for(Rtn=0; Rtn<6; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[21+Rtn]);
	        printf("\n");
	        printf("EVSE ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[27+Rtn]);
	        printf("\n");
	        printf("EVSE MAC: \n");
	        for(Rtn=0; Rtn<6; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[44+Rtn]);
	        printf("\n");
	        printf("RunID: \n");
	        for(Rtn=0; Rtn<8; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[50+Rtn]);
	        printf("\n");
	        printf("RSVD: \n");
	        for(Rtn=0; Rtn<8; Rtn++)
	            printf("%02x, ",MmePacket->MMENTRY[58+Rtn]);
	        printf("\n");
	            #endif
	        V2gFlowStatus=CM_SLAC_MATCH_REQ;
	        memset(&SendMmePacket,0,sizeof(struct MmeHeader));
	        memcpy(SendMmePacket.ODA,MmePacket->OSA,6);
	        memcpy(SendMmePacket.OSA,CsuMac,6);
	        SendMmePacket.MTYPE=htons(EtherType_HomePlug);
	        SendMmePacket.MMV=MmePacket->MMV;
	        SendMmePacket.MMTYPE=MMTYPE_CM_SLAC_MATCH_CNF;
	        SendMmePacket.FMI[0]=SendMmePacket.FMI[1]=0;
	        SendMmePacketSize=0;
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x00;    //Fixed value (0x00) indicating ��PEV-EVSE matching��
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x00;    //Fixed value (0x00) indicating ��No Security��
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x00;    //Fixed value (0x0056) for matching
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x56;    //Fixed value (0x0056) for matching
	        memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,17);    //PEV ID
	        SendMmePacketSize+=17;
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,EvMac,6);
	        SendMmePacketSize+=6;
	        memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,17);    //EVSE ID
	        SendMmePacketSize+=17;
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,CsuMac,6);
	        SendMmePacketSize+=6;
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,SlacRunId,sizeof(SlacRunId));
	        SendMmePacketSize+=sizeof(SlacRunId);
	        memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,8);    //RSVD
	        SendMmePacketSize+=8;
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,Nid,sizeof(Nid));
	        SendMmePacketSize+=sizeof(Nid);
	        SendMmePacket.MMENTRY[SendMmePacketSize++]=0x00;    //RSVD
	        memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,NewNmkKey,sizeof(NewNmkKey));
	        SendMmePacketSize+=sizeof(NewNmkKey);
	        SendMmePacketSize+=19;      //the size before MMENTRY
	            #ifdef Debug
	        printf("\n\n***** Response MME Packet *****\n");
	        printf("SendMmePacket.ODA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	               SendMmePacket.ODA[0],SendMmePacket.ODA[1],SendMmePacket.ODA[2],SendMmePacket.ODA[3],SendMmePacket.ODA[4],SendMmePacket.ODA[5]);
	        printf("SendMmePacket.OSA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	               SendMmePacket.OSA[0],SendMmePacket.OSA[1],SendMmePacket.OSA[2],SendMmePacket.OSA[3],SendMmePacket.OSA[4],SendMmePacket.OSA[5]);
	        printf("MTYPE: 0x%x\n", htons(SendMmePacket.MTYPE));
	        printf("MMV: 0x%x\n", SendMmePacket.MMV);
	        printf("MMTYPE: 0x%x\n", SendMmePacket.MMTYPE);
	        printf("FMI 0x%x, 0x%x\n", SendMmePacket.FMI[0],SendMmePacket.FMI[1]);
	        printf("--- CM_SLAC_MATCH_CNF ---\n");
	        printf("APPLICATION_TYPE: 0x%x\n", SendMmePacket.MMENTRY[0]);
	        printf("SECURITY_TYPE: 0x%x\n", SendMmePacket.MMENTRY[1]);
	        printf("MVFLength: 0x%x, 0x%x\n", SendMmePacket.MMENTRY[2],SendMmePacket.MMENTRY[3]);
	        printf("PEV ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[4+Rtn]);
	        printf("\n");
	        printf("PEV MAC: \n");
	        for(Rtn=0; Rtn<6; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[21+Rtn]);
	        printf("\n");
	        printf("EVSE ID: \n");
	        for(Rtn=0; Rtn<17; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[27+Rtn]);
	        printf("\n");
	        printf("EVSE MAC: \n");
	        for(Rtn=0; Rtn<6; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[44+Rtn]);
	        printf("\n");
	        printf("RunID: \n");
	        for(Rtn=0; Rtn<8; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[50+Rtn]);
	        printf("\n");
	        printf("RSVD: \n");
	        for(Rtn=0; Rtn<8; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[58+Rtn]);
	        printf("\n");
	        printf("NID: \n");
	        for(Rtn=0; Rtn<7; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[66+Rtn]);
	        printf("\n");
	        printf("RSVD: 0x%x\n", SendMmePacket.MMENTRY[73]);
	        printf("NMK: \n");
	        for(Rtn=0; Rtn<16; Rtn++)
	            printf("%02x, ",SendMmePacket.MMENTRY[74+Rtn]);
	        printf("\n");
	            #endif
	        V2gFlowStatus=CM_SLAC_MATCH_CNF;
	        Rtn=sendto(RawSock, &SendMmePacket, SendMmePacketSize, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
	            #ifdef Debug
	        printf("SendMmePacketSize=%d,Rtn=%d\n",SendMmePacketSize,Rtn);
	            #endif
	        ftime(&SeqStartTime);
	        break;
		}

	    case MMTYPE_VENDOR_VS_HOST_ACTION:
	    {
	        struct QcaVendorMmeHeader *RecvPacket;
	        RecvPacket = (struct QcaVendorMmeHeader *)Buffer;
	        //#ifdef Debug
	        printf("--- MMTYPE_VENDOR_VS_HOST_ACTION ---\n");
	        //#endif
	        switch (RecvPacket->MBODY[0])
	        {
		        case 0x00:
		            //Loader (Device Softloader or Bootloader) ready
		            printf("QCA7K: Loader Ready\n");
		            break;

		        case 0x01:
		            //Firmware Upgrade Ready
		            printf("QCA7K: Firmware Upgrade Ready\n");
		            break;

		        case 0x02:
		            //PIB Update Ready
		            printf("QCA7K: PIB Update Ready\n");
		            break;

		        case 0x03:
		            //Firmware Upgrade and PIB Update ready
		            printf("QCA7K: Firmware Upgrade and PIB Update ready\n");
		            break;

		        case 0x04:
		            //Loader (Bootloader) ready to receive SDRAM configuration.
		            printf("QCA7K: Loader ready to receive SDRAM configuration\n");
		            break;

		        case 0x05:
		            //Reset to Factory Defaults.
		            printf("QCA7K: Reset to Factory Defaults\n");
		            break;

		        default:
		            //Reserved
		            printf("QCA7K: Reserved\n");
		            break;
	        }
			break;
	    }

	    case MMTYPE_VENDOR_ATTEN_CHAR:
		{
	        #ifdef Debug
	        printf("--- MMTYPE_VENDOR_ATTEN_CHAR ---\n");
	        #endif
	        break;
		}

	    case MMTYPE_VENDOR_VS_NW_INFO_CNF:
		{
	        memcpy(QcaMac,MmePacket->OSA,6);
	            #ifdef Debug
	        printf("--- MMTYPE_VENDOR_VS_NW_INFO_CNF ---\n");
	        printf("QcaMac: %02x:%02x:%02x:%02x:%02x:%02x\n",
	               QcaMac[0],QcaMac[1],QcaMac[2],QcaMac[3],QcaMac[4],QcaMac[5]);
	            #endif
	        V2gFlowStatus=CM_SET_KEY_REQ;
		}

	    default:
		{
	        break;
		}
    }
}

int SendSetKey()
{
    int i = 0;
    unsigned char nRandValue = 0x0;
    unsigned char ConstString[16]="PhihongKey000000";

    memset(&SendMmePacket,0,sizeof(struct MmeHeader));
    memcpy(SendMmePacket.ODA,QcaMac,6);
    memcpy(SendMmePacket.OSA,CsuMac,6);
    SendMmePacket.MTYPE=htons(EtherType_HomePlug);
    SendMmePacket.MMV=0x01;
    SendMmePacket.MMTYPE=MMTYPE_CM_SET_KEY_REQ;
    SendMmePacket.FMI[0]=SendMmePacket.FMI[1]=0;
    SendMmePacketSize=0;
    SendMmePacket.MMENTRY[SendMmePacketSize++]=0x01;//Fixed value (0x01) to indicate ��NMK��
    memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,4);//My Nonce, Fixed value(0x00000000), encrypted payload not used
    SendMmePacketSize+=4;
    memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,4);//Your Nonce, Fixed value(0x00000000), encrypted payload not used
    SendMmePacketSize+=4;
    SendMmePacket.MMENTRY[SendMmePacketSize++]=0x04;//PID, Fixed value (0x04) to indicate ��HLE protocol��
    memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,2);//PRN, Fixed value(0x00), encrypted payload not used
    SendMmePacketSize+=2;
    SendMmePacket.MMENTRY[SendMmePacketSize++]=0x00;//PMN, Fixed value(0x00) encrypted payload not used
    SendMmePacket.MMENTRY[SendMmePacketSize++]=0x01;//CCo Capablility
    srand(time(NULL));
    for(i=10; i<16; i++)
    {
        nRandValue = (rand()%62)+1;
        if((nRandValue>=0)&&(nRandValue<=9))  // 0 ~ 9
            ConstString[i]= nRandValue + 0x30;
        else if((nRandValue>=10)&&(nRandValue<=35)) // A ~ Z
            ConstString[i]= nRandValue -10 + 0x41;
        else if((nRandValue>=36)&&(nRandValue<=61)) // a ~ z
            ConstString[i]= nRandValue -37 + 0x61;
        else
            ConstString[i]= 0x30;
    }
    memset(NewNmkKey,0,sizeof(NewNmkKey));
    memset(Nid,0,sizeof(Nid));
    HPAVKeyNMK(NewNmkKey,ConstString);
    HPAVKeyNID(Nid,NewNmkKey,DEFAULT_LEVEL);
    memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,Nid,sizeof(Nid));    //NID, 54 LSBs contain the NID 2 MSBs = 0b00
    SendMmePacketSize+=sizeof(Nid);
    SendMmePacket.MMENTRY[SendMmePacketSize++]=0x01;//NewEKS,Fixed value (0x01)to indicate ��NMK��
    memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,NewNmkKey,sizeof(NewNmkKey));//NewKey
    SendMmePacketSize+=sizeof(NewNmkKey);
    SendMmePacketSize+=19;  //the size before MMENTRY
    V2gFlowStatus=CM_SET_KEY_REQ;
    i=sendto(RawSock, &SendMmePacket, SendMmePacketSize, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
    #ifdef Debug
    printf("SendSetKey: send size =%d\n",i);
    #endif

}

int GetQca7kMac()
{
    int i = 0;
    struct QcaVendorMmeHeader SendPacket;

    memset(&SendPacket,0,sizeof(struct QcaVendorMmeHeader));
    memset(SendPacket.ODA, 0xFF, 6);
    memcpy(SendPacket.OSA,CsuMac,6);
    SendPacket.MTYPE=htons(EtherType_HomePlug);
    SendPacket.MMV=0x00;
    SendPacket.MMTYPE=MMTYPE_VENDOR_VS_NW_INFO;
    SendPacket.OUI[0]=0x00;
    SendPacket.OUI[1]=0xB0;
    SendPacket.OUI[2]=0x52;

    i=sendto(RawSock, &SendPacket, 20, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
    #ifdef Debug
    printf("GetQca7kMac: send size =%d\n",i);
    #endif
}

int SlacComm()
{
    int packet_size,count;
    static unsigned int STime;

    if(RawSock>=0)
    {
        memset(RecvBuffer,0,RecvBufferSize);
        packet_size = recvfrom(RawSock, RecvBuffer, RecvBufferSize, 0, NULL, NULL);
        if(packet_size>0)
        {
            /*#ifdef Debug
               printf("Raw Data: ");
               for(count=0;count<packet_size;count++)
                printf("0x%x, ",RecvBuffer[count]);
               printf("\n");
             #endif*/
            MmeProcess(RecvBuffer, packet_size);
        }
    }
    switch(V2gFlowStatus)
    {
	    case Idle:
		{
	        if(RawSock<0)
	        {
	            struct timeval tv;
	            RawSock= socket(PF_PACKET, SOCK_RAW, htons(EtherType_HomePlug));
	                #ifdef Debug
	            printf("RawSock=%d\n",RawSock);
	                #endif
	            if(RawSock == -1)
	            {

	                    #ifdef SystemLogMessage
	                StoreLogMsg("[EvComm]SlacComm:Failed to create socke");
	                    #endif
	                V2gFlowStatus=Other_Fault;
	                return -1;
	            }
	            setsockopt(RawSock, SOL_SOCKET, SO_BINDTODEVICE,QcaInterface, 4);
	            /*tv.tv_sec = 0;
	               tv.tv_usec = 10000;
	               if (setsockopt(RawSock, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(struct	timeval)) < 0)
	               {
	             #ifdef SystemLogMessage
	               StoreLogMsg("[EvComm]SlacComm:Set SO_RCVTIMEO NG");
	             #endif
	               V2gFlowStatus=Other_Fault;
	               return -1;
	               }*/
	            memset(&Req, 0, sizeof(struct ifreq));
	            strcpy( (char*)Req.ifr_name, QcaInterface);
	            if (ioctl(RawSock, SIOCGIFINDEX, &Req) < 0)
	            {
	                        #ifdef SystemLogMessage
	                StoreLogMsg("[EvComm]SlacComm: ioctl NG");
	                    #endif
	                V2gFlowStatus=Other_Fault;
	                return -1;
	            }
	            memset( &DestSocketAddress, 0, sizeof(struct sockaddr_ll) );
	            DestSocketAddress.sll_ifindex = Req.ifr_ifindex;
	            DestSocketAddress.sll_halen = ETH_ALEN;
	            //Get QCA7K MAC address
	            GetQca7kMac();
	            PwmStartTime=0;
	            count=0;
	            STime=time(NULL);
	        }
	        else
	        {
	            if((time(NULL)-STime)>=3)
	            {
	                if((count++) >=3)
	                    V2gFlowStatus=Sequence_Timeout;
	                else
	                {
	                    GetQca7kMac();
	                    STime=time(NULL);
	                }
	            }
	        }
	        break;
		}

	    case CM_SET_KEY_REQ:
		{
	        //CM_SET_KEY_REQ
	        SendSetKey();
	        break;
		}

	    case CM_SET_KEY_CNF:
		{
	        OutputCpPwmDuty(5);
	        if(PwmStartTime<=0)
	            PwmStartTime=time(NULL);
	        else
	        {
	            if((time(NULL)-PwmStartTime)>TT_EVSE_SLAC_init)
	            {
	                    #ifdef SystemLogMessage
	                StoreLogMsg("[EvComm]SlacComm: Wait CM_SLAC_PARM_REQ Timeout - TT_EVSE_SLAC_init ");
	                    #endif
	                V2gFlowStatus=Sequence_Timeout;
	                return -1;
	            }
	        }
	        break;
		}

	    case CM_SLAC_PARM_CONF:
		{
	        ftime(&SeqEndTime);
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_sequence)
	        {
	                #ifdef SystemLogMessage
	            StoreLogMsg("[EvComm]SlacComm: Wait CM_START_ATTEN_CHAR_IND Timeout - TT_match_sequence ");
	                #endif
	            V2gFlowStatus=Sequence_Timeout;
	            return -1;
	        }
	        break;
		}

	    case CM_START_ATTEN_CHAR_IND:
		{
	        ftime(&SeqEndTime);
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>(3*TP_EV_batch_msg_interval))    //one more time interval for tolerance
	        {
	                #ifdef SystemLogMessage
	            StoreLogMsg("[EvComm]SlacComm: Wait CM_MNBC_SOUND_IND Timeout - 3*TP_EV_batch_msg_interval ");
	                #endif
	            V2gFlowStatus=Sequence_Timeout;
	            return -1;
	        }
	        break;
		}

	    case CM_MNBC_SOUND_IND:
		{
	        ftime(&SeqEndTime);
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>(TT_EVSE_match_MNBC*100)||(AttenProfileNum>=MnbcSoundNum))
	        {
	            memset(&SendMmePacket,0,sizeof(struct MmeHeader));
	            memcpy(SendMmePacket.ODA,EvMac,6);
	            memcpy(SendMmePacket.OSA,CsuMac,6);
	            SendMmePacket.MTYPE=htons(EtherType_HomePlug);
	            SendMmePacket.MMV=0x01;
	            SendMmePacket.MMTYPE=MMTYPE_CM_ATTN_CHAR_IND;
	            SendMmePacket.FMI[0]=SendMmePacket.FMI[1]=0;
	            SendMmePacketSize=0;
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=0;    //APPLICATION_TYPE,  Fixed value indicating ��PEVEVSE matching��
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=0;    //SECURITY_TYPE, Fixed value indicating ��No Security��
	            memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,EvMac,6);    //SOURCE_ADDRESS, MAC address of the EV Host
	            SendMmePacketSize+=6;
	            memcpy(SendMmePacket.MMENTRY+SendMmePacketSize,SlacRunId,sizeof(SlacRunId));
	            SendMmePacketSize+=sizeof(SlacRunId);
	            memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,17);    //SOURCE_ID
	            SendMmePacketSize+=17;
	            memset(SendMmePacket.MMENTRY+SendMmePacketSize,0,17);    //RESP_ID
	            SendMmePacketSize+=17;
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=AttenProfileNum;    //NumSounds
	            SendMmePacket.MMENTRY[SendMmePacketSize++]=AagGroupsNum;    //NumGroups
	            for(count=0; count<AagGroupsNum; count++)
	            {
	                unsigned char TmpAag;
	                TmpAag=((Aag[count]/AttenProfileNum)&0xFF);
	                if(TmpAag>=39)
	                {
						/*
						// [To-do] If this statement is enabled, SLAC will fail due to timeout.
	                    #ifdef SystemLogMessage
	                    {
	                        unsigned char TmpBuf[64];
	                        memset(TmpBuf,0,sizeof(TmpBuf));
	                        sprintf(TmpBuf,"[EvComm]SlacComm: bad Aag[%d]=%d",count,TmpAag);
	                        StoreLogMsg(TmpBuf);
	                    }
	                    #endif
						*/
	                    TmpAag=37;
	                }
	                SendMmePacket.MMENTRY[SendMmePacketSize++]=TmpAag;
	            }
	            SendMmePacketSize+=19;      //the size before MMENTRY
	                #ifdef Debug
	            printf("\n\n***** Send MME Packet *****\n");
	            printf("SendMmePacket.ODA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	                   SendMmePacket.ODA[0],SendMmePacket.ODA[1],SendMmePacket.ODA[2],SendMmePacket.ODA[3],SendMmePacket.ODA[4],SendMmePacket.ODA[5]);
	            printf("SendMmePacket.OSA: %02x:%02x:%02x:%02x:%02x:%02x:\n",
	                   SendMmePacket.OSA[0],SendMmePacket.OSA[1],SendMmePacket.OSA[2],SendMmePacket.OSA[3],SendMmePacket.OSA[4],SendMmePacket.OSA[5]);
	            printf("MTYPE: 0x%x\n", htons(SendMmePacket.MTYPE));
	            printf("MMV: 0x%x\n", SendMmePacket.MMV);
	            printf("MMTYPE: 0x%x\n", SendMmePacket.MMTYPE);
	            printf("FMI 0x%x, 0x%x\n", SendMmePacket.FMI[0],SendMmePacket.FMI[1]);
	            printf("--- CM_ATTEN_CHAR_IND ---\n");
	            printf("APPLICATION_TYPE: 0x%x\n", SendMmePacket.MMENTRY[0]);
	            printf("SECURITY_TYPE: 0x%x\n", SendMmePacket.MMENTRY[1]);
	            printf("SOURCE_ADDRESS: %02x:%02x:%02x:%02x:%02x:%02x\n",
	                   SendMmePacket.MMENTRY[2],SendMmePacket.MMENTRY[3],SendMmePacket.MMENTRY[4],SendMmePacket.MMENTRY[5],
	                   SendMmePacket.MMENTRY[6],SendMmePacket.MMENTRY[7]);
	            printf("RunID: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\n",
	                   SendMmePacket.MMENTRY[8],SendMmePacket.MMENTRY[9],SendMmePacket.MMENTRY[10],SendMmePacket.MMENTRY[11],
	                   SendMmePacket.MMENTRY[12],SendMmePacket.MMENTRY[13],SendMmePacket.MMENTRY[14],SendMmePacket.MMENTRY[15]);
	            printf("SOURCE_ID: \n");
	            for(count=0; count<17; count++)
	                printf("%02x, ",SendMmePacket.MMENTRY[16+count]);
	            printf("\n");
	            printf("RESP_ID: \n");
	            for(count=0; count<17; count++)
	                printf("%02x, ",SendMmePacket.MMENTRY[33+count]);
	            printf("\n");
	            printf("NumSounds: 0x%x\n", SendMmePacket.MMENTRY[50]);
	            printf("ATTEN_PROFILE: \n");
	            for(count=0; count<AagGroupsNum; count++)
	                printf("%02x, ",SendMmePacket.MMENTRY[51+count]);
	            printf("\n");
	                #endif
	            V2gFlowStatus=CM_ATTEN_CHAR_IND;
	            count=sendto(RawSock, &SendMmePacket, SendMmePacketSize, 0, (struct sockaddr*)&DestSocketAddress, sizeof(struct sockaddr_ll));
	                #ifdef Debug
	            printf("SendMmePacketSize=%d,Rtn=%d\n",SendMmePacketSize,count);
	                #endif
	            ftime(&SeqStartTime);
	        }
	        break;
		}

	    case CM_ATTEN_CHAR_IND:
		{
	        ftime(&SeqEndTime);
	        //if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_response)
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_response*2)     //extended to 400ms due to the response of CM_ATTEN_CHAR.RSP of some EVCC is slower than 200ms.
	        {
	                #ifdef SystemLogMessage
	            StoreLogMsg("[EvComm]SlacComm: Wait CM_ATTEN_CHAR_RSP Timeout - TT_match_response ");
	                #endif
	            V2gFlowStatus=Sequence_Timeout;
	            return -1;
	        }
	        break;
		}

	    case CM_ATTEN_CHAR_RSP:
		{
	        ftime(&SeqEndTime);
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_EVSE_match_session)
	        {
	                #ifdef SystemLogMessage
	            StoreLogMsg("[EvComm]SlacComm: Wait CM_VALIDATE_REQ or CM_SLAC_MATCH_REQ Timeout - TT_EVSE_match_session ");
	                #endif
	            V2gFlowStatus=Sequence_Timeout;
	            return -1;
	        }
	        break;
		}

	    case CM_VALIDATE_CNF:
		{
	        ftime(&SeqEndTime);
	        if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_sequence)
	        {
	                #ifdef SystemLogMessage
	            StoreLogMsg("[EvComm]SlacComm: Wait CM_VALIDATE_CNF or CM_SLAC_MATCH_REQ Timeout - TT_match_sequence ");
	                #endif
	            V2gFlowStatus=Sequence_Timeout;
	            return -1;
	        }
	        break;
		}

	    case CM_SLAC_MATCH_CNF:
		{
	        if(UdpSock>0)
	        {
	            close(UdpSock);
	            UdpSock=-1;
	        }
	        if(TcpSock>0)
	        {
	            close(TcpSock);
	            TcpSock=-1;
	        }
	        ftime(&SeqStartTime);
	        V2gFlowStatus=SLACC_SDP_UDP_Connection;
	        break;
		}

		defaudlt:
		{
		    break;
		}
    }
    return 0;
}

int V2gMsgProcess(unsigned char *Buffer, int DataLength)
{
    struct V2gtpHeader *RecvHeader;
    unsigned char *PayloadData;

    RecvHeader= (struct V2gtpHeader *) Buffer;

    #ifdef Debug
    printf("\n\n***********************************\n");
    printf("***** Received V2G Packet *****\n");
    printf("***********************************\n");
    printf("ProtocolVersion=%d\n",RecvHeader->ProtocolVersion);
    printf("InverseProtocolVersion=0x%x\n",RecvHeader->InverseProtocolVersion);
    printf("PayloadType=0x%x\n",htons(RecvHeader->PayloadType));
    printf("PayloadLength=0x%x\n",htonl(RecvHeader->PayloadLength));
    #endif

    if(htons(RecvHeader->PayloadType)!=V2GTP_PAYLOAD_TYPE_EXI_MSG)
    {
        #ifdef SystemLogMessage
        StoreLogMsg("[EvComm]V2gMsgProcess: Wrong Payload Type");
        #endif
        return 0;
    }
    //EXI decode
    //process received message and change status flag

}

int V2gComm(int AcceptFd)
{
    int packet_size,count;

    memset(RecvBuffer,0,RecvBufferSize);
    packet_size=recv(AcceptFd, RecvBuffer, RecvBufferSize, 0);
    if(packet_size>0)
    {
        /*#ifdef Debug
           printf("V2gComm Data: ");
           for(count=0;count<packet_size;count++)
            printf("0x%x, ",RecvBuffer[count]);
           printf("\n");
         #endif*/
        V2gMsgProcess(RecvBuffer, packet_size);
    }
    //following are the response message handling according to status flag
    switch(V2gFlowStatus)
    {
	    case SupportedAppProtocolRequest:
	        break;
	    default:
	        break;
    }
    return 0;
}

int SdpUdpConnected()
{
    int packet_size,Rtn;
    struct sockaddr_in6 ServerAddr,ClientAddr;
    struct V2gtpHeader *RecvHeader;
    unsigned char *PayloadData;

    if(UdpSock<=0)
    {
        if ((UdpSock = socket(AF_INET6, SOCK_DGRAM, 0)) <0)
        {
                    #ifdef SystemLogMessage
            StoreLogMsg("[EvComm]SdpUdpConnected: Fail to open UdpSock");
            #endif
            return 0;
        }
        memset(&ServerAddr,0, sizeof(struct sockaddr_in));
        ServerAddr.sin6_family = AF_INET6;
        ServerAddr.sin6_addr=in6addr_any;
        ServerAddr.sin6_port = htons(SdpUdpServerPort);
        if(bind(UdpSock, (struct sockaddr *)&ServerAddr, sizeof(struct sockaddr_in6)) <0)
        {
                    #ifdef SystemLogMessage
            StoreLogMsg("[EvComm]SdpUdpConnected: Fail to bind UdpSock");
            #endif
            close(UdpSock);
            UdpSock=-1;
            return 0;
        }
            #ifdef Debug
        printf("UdpSock=%d\n",UdpSock);
            #endif
    }
    memset(RecvBuffer,0,RecvBufferSize);
    memset(&ClientAddr,0, sizeof(struct sockaddr_in));
    Rtn=sizeof(struct sockaddr_in6);
    packet_size = recvfrom(UdpSock, RecvBuffer, RecvBufferSize, MSG_DONTWAIT, (struct sockaddr *)&ClientAddr, &Rtn);
    #ifdef Debug
    printf("packet_size=%d\n",packet_size);
        #endif
    if(packet_size>0)
    {
        RecvHeader= (struct V2gtpHeader *) RecvBuffer;
        PayloadData=RecvBuffer+sizeof(struct V2gtpHeader);
        #ifdef Debug
        printf("\n\n***********************************\n");
        printf("***** Received SDP Packet *****\n");
        printf("***********************************\n");
        printf("ClientAddress=");
        for(Rtn=0; Rtn<16; Rtn+=2)
            printf("%02x%02x:",ClientAddr.sin6_addr.s6_addr[Rtn],ClientAddr.sin6_addr.s6_addr[Rtn+1]);
        printf("\n");
        printf("ClientPort=%d\n",ClientAddr.sin6_port);
        printf("ProtocolVersion=%d\n",RecvHeader->ProtocolVersion);
        printf("InverseProtocolVersion=0x%x\n",RecvHeader->InverseProtocolVersion);
        printf("PayloadType=0x%x\n",htons(RecvHeader->PayloadType));
        printf("PayloadLength=0x%x\n",htonl(RecvHeader->PayloadLength));
        #endif
        if((RecvHeader->ProtocolVersion==0x01)&&(RecvHeader->InverseProtocolVersion==0xFE)&&(htons(RecvHeader->PayloadType)==V2GTP_PAYLOAD_TYPE_SDP_REQUEST))
        {
            #ifdef Debug
            printf("Security=0x%x\n",*(PayloadData+0));
            printf("TransportProtocol=0x%x\n",*(PayloadData+1));
            #endif
            RecvHeader->PayloadType=htons(V2GTP_PAYLOAD_TYPE_SDP_RESPONSE);
            RecvHeader->PayloadLength=htonl(20);    //Fixed Length=20
            memset(PayloadData,0,20);
            // MAC address[0:2] + FFFE + MAC address[3:5]
            PayloadData[0]=(IPV6_LINK_LOCAL_PREFIX>>8)&0xFF;
            PayloadData[1]=IPV6_LINK_LOCAL_PREFIX&0xFF;
            PayloadData[8]=CsuMac[0];
            PayloadData[8]^=0x02;// bit 1 should complemented.
            PayloadData[9]=CsuMac[1];
            PayloadData[10]=CsuMac[2];
            PayloadData[11]=0xFF;
            PayloadData[12]=0xFE;
            PayloadData[13] =CsuMac[3];
            PayloadData[14]=CsuMac[4];
            PayloadData[15]=CsuMac[5];
            //TCP port
            PayloadData[16]=(SdpTcpServerPort>>8)&0xFF;
            PayloadData[17]=SdpTcpServerPort&0xFF;
            PayloadData[18]=SDP_PAYLOAD_SECURITY_NONE;      //Security
            PayloadData[19]=SDP_PAYLOAD_TRANS_PROTOCOL_TCP; //Transport protocol
            Rtn=sendto(UdpSock, RecvBuffer, sizeof(struct V2gtpHeader)+htonl(RecvHeader->PayloadLength), 0, (struct sockaddr *)&ClientAddr, sizeof(struct sockaddr_in6));
            #ifdef Debug
            printf("\n\n***** Response SDP Packet *****\n");
            printf("Send size=%d\n",Rtn);
            printf("Destination Address=");
            for(Rtn=0; Rtn<16; Rtn++)
                printf("%02x, ",ClientAddr.sin6_addr.s6_addr[Rtn]);
            printf("\n");
            printf("Destination Port=%d\n",ClientAddr.sin6_port);
            printf("ProtocolVersion=%d\n",RecvHeader->ProtocolVersion);
            printf("InverseProtocolVersion=0x%x\n",RecvHeader->InverseProtocolVersion);
            printf("PayloadType=0x%x\n",htons(RecvHeader->PayloadType));
            printf("PayloadLength=0x%x\n",htonl(RecvHeader->PayloadLength));
            printf("SECC Ipv6 Address=");
            for(Rtn=0; Rtn<16; Rtn++)
                printf("%02x:",PayloadData[Rtn]);
            printf("\n");
            printf("SECC Port=%d\n",(PayloadData[16]<<8|PayloadData[17]));
            printf("Security=0x%x\n",PayloadData[19]);
            printf("TransportProtocol=0x%x\n",PayloadData[20]);
            #endif
            if(Rtn>0)
                return 1;
        }
    }
    return 0;
}

int V2gTcpConnected()
{
    int packet_size,Rtn,AcceptFd;
    struct sockaddr_in6 ServerAddr,ClientAddr;

    if(TcpSock<=0)
    {
        if ((TcpSock = socket(PF_INET6, SOCK_STREAM, 0)) <0)
        {
                    #ifdef SystemLogMessage
            StoreLogMsg("[EvComm]V2gTcpConnected: Fail to open TcpSock");
            #endif
            return 0;
        }
        fcntl(TcpSock, F_SETFL, O_NONBLOCK);    //set to O_NONBLOCK
        memset(&ServerAddr,0, sizeof(struct sockaddr_in));
        ServerAddr.sin6_family = PF_INET6;
        ServerAddr.sin6_addr=in6addr_any;
        ServerAddr.sin6_port = htons(SdpTcpServerPort);
        if(bind(TcpSock, (struct sockaddr *)&ServerAddr, sizeof(struct sockaddr_in6)) <0)
        {
                    #ifdef SystemLogMessage
            StoreLogMsg("[EvComm]V2gTcpConnected: Fail to bind TcpSock");
            #endif
            close(TcpSock);
            TcpSock=-1;
            return 0;
        }
        if(listen(TcpSock, 1) == -1)     //only accept one connection
        {
                #ifdef SystemLogMessage
            StoreLogMsg("[EvComm]V2gTcpConnected: Fail to listen TcpSock");
            #endif
            close(TcpSock);
            TcpSock=-1;
            return 0;
        }
            #ifdef Debug
        printf("TcpSock=%d\n",TcpSock);
            #endif
    }
    Rtn=sizeof(struct sockaddr_in6);
    if((AcceptFd=accept(TcpSock,(struct sockaddr *)&ClientAddr,&Rtn))==-1)
    {
        #ifdef Debug
        printf("Wait TCP connection\n");
        #endif
        return 0;
    }
    #ifdef Debug
    printf("Accept one TCP connection:\n");
    printf("AcceptFd=%d\n",AcceptFd);
    printf("ClientAddress=");
    for(Rtn=0; Rtn<16; Rtn+=2)
        printf("%02x%02x:",ClientAddr.sin6_addr.s6_addr[Rtn],ClientAddr.sin6_addr.s6_addr[Rtn+1]);
    printf("\n");
    printf("ClientPort=%d\n",ClientAddr.sin6_port);
    #endif
    return AcceptFd;
}

int main(int argc,char *argv[])
{
    unsigned char Rtn;
    int TcpAcceptFd;
    //Initialization
    InitShareMemory();
    //start to detect pilot state
    PilotDetectionPid=0;
    PilotDetection();
    //Init communication parameters
    GetEthMac(QcaInterface, CsuMac);
    RecvBuffer=(unsigned char *)malloc(RecvBufferSize);
    memset(RecvBuffer,0,RecvBufferSize);
    SendBuffer=(unsigned char *)malloc(SendBufferSize);
    memset(SendBuffer,0,SendBufferSize);
    if(RawSock>0)
        close(RawSock);
    if(UdpSock>0)
        close(UdpSock);
    if(TcpSock>0)
        close(TcpSock);
    RawSock=UdpSock=TcpSock=-1;
    V2gFlowStatus=0;
    AttenProfileNum=0;
    while(1)
    {
        #ifdef Debug
        printf("V2gFlowStatus=%d\n",V2gFlowStatus);
        #endif
        //if((ShmInternalComm->ChargingPermission==0x01)&&(ConnectorPlugIn()==1))
        if(1)
        {
            if(V2gFlowStatus<SLACC_SDP_UDP_Connection)
                SlacComm();
            else if(V2gFlowStatus==SLACC_SDP_UDP_Connection)
            {
                if(SdpUdpConnected()==1)
                {
                    V2gFlowStatus=SLACC_SDP_TCP_Connection;
                    continue;
                }
                ftime(&SeqEndTime);
                if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_join)
                {
                    #ifdef SystemLogMessage
                    StoreLogMsg("[EvComm]main: Wait SLACC_SDP_UDP_Connection Timeout - TT_match_join ");
                    #endif
                    V2gFlowStatus=Sequence_Timeout;
                }
            }
            else if(V2gFlowStatus==SLACC_SDP_TCP_Connection)
            {
                if((TcpAcceptFd=V2gTcpConnected())>0)
                {
                    V2gFlowStatus=SupportedAppProtocolRequest;
                    continue;
                }
                ftime(&SeqEndTime);
                if(DiffTimeb(SeqStartTime, SeqEndTime)>TT_match_join)
                {
                    #ifdef SystemLogMessage
                    StoreLogMsg("[EvComm]main: Wait SLACC_SDP_TCP_Connection Timeout - TT_match_join ");
                    #endif
                    V2gFlowStatus=Sequence_Timeout;
                }
            }
            else if(V2gFlowStatus<Performance_Timeout)
                V2gComm(TcpAcceptFd);
        }
        if(V2gFlowStatus>=Performance_Timeout)
        {
            //Normal Stop
            //alarm and duty to 100%
            OutputCpPwmDuty(100);
            goto ReSet;
        }
        /*else if((ConnectorPlugIn()==0)&&(V2gFlowStatus>Idle))
           {
            //Emergency stop
            OutputCpPwmDuty(100);
            goto ReSet;
           }*/
        continue;

ReSet:
        if(RawSock>0)
            close(RawSock);
        if(UdpSock>0)
            close(UdpSock);
        if(TcpSock>0)
        {
            close(TcpSock);
            close(TcpAcceptFd);
        }
        RawSock=UdpSock=TcpSock=TcpAcceptFd=-1;
        V2gFlowStatus=0;
        while(1)
        {
            //wait for CSU configrm
        }
    }//main while
}