csu3_am335x/EVSE/Projects/CCS/Apps/EvComm.c

#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"

#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]);
			MnbcSoundNum=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
			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))
			{
				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
				for(count=0;count<AagGroupsNum;count++)
					SendMmePacket.MMENTRY[SendMmePacketSize++]=(Aag[count]/AttenProfileNum)&0xFF;
				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)
			{
				#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
}