csu3_am335x/EVSE/Modularization/Module_Wifi.c

/*
 * Module_Wifi.c
 *
 *  Created on: 2020-01-14
 *      Author: Jerry Wang, Eason Yang, Folus Wen
 *     Version: D0.01
 */

#include 	<sys/time.h>
#include 	<sys/timeb.h>
#include    <sys/types.h>
#include    <sys/stat.h>
#include 	<sys/types.h>
#include 	<sys/ioctl.h>
#include 	<sys/socket.h>
#include 	<sys/ipc.h>
#include 	<sys/shm.h>
#include 	<sys/shm.h>
#include 	<sys/mman.h>
#include 	<linux/wireless.h>
#include 	<arpa/inet.h>
#include 	<netinet/in.h>

#include 	<unistd.h>
#include 	<stdarg.h>
#include    <stdio.h>      /*標準輸入輸出定義*/
#include    <stdlib.h>     /*標準函數庫定義*/
#include    <unistd.h>     /*Unix 標準函數定義*/
#include    <fcntl.h>      /*檔控制定義*/
#include    <termios.h>    /*PPSIX 終端控制定義*/
#include    <errno.h>      /*錯誤號定義*/
#include 	<errno.h>
#include 	<string.h>
#include	<time.h>
#include	<ctype.h>
#include 	<ifaddrs.h>
#include	"define.h"

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

#define ARRAY_SIZE(A)				(sizeof(A) / sizeof(A[0]))
#define PASS						1
#define FAIL			   			-1
#define ON							1
#define OFF							0
#define true			    		1
#define false						0
#define IP_TYPE_DHCP				0
#define IP_TYPE_STATIC				1
#define AUTH_TYPE_NONE				0
#define AUTH_TYPE_WEP				1
#define AUTH_TYPE_WPAPSK			2
#define AUTH_TYPE_WPA2PSK			3

#define WIFI_MODE_DISABLE			0
#define WIFI_MODE_STA				1
#define WIFI_MODE_AP				2
#define WIFI_MODE_ADHOC				3

#define STA_STATE_INIT        		0
#define STA_STATE_DEVICE_DETECT		1
#define STA_STATE_AP_CONNECTED	    2
#define STA_STATE_INTERNET_CHECKING 3

#define AP_STATE_INIT				0
#define AP_STATE_DEVICE_DETECT		1
#define AP_STATE_AP_START			2
#define AP_STATE_SERVER_LISTEN		3

#define LISTEN_PORT_UDP				54088
#define LISTEN_PORT_TCP				54089

#define	CONNECTION_LIMIT			1
#define MtdBlockSize				0x600000

#define UBLOX
//#define MT7601U

enum WIFI_COMMAND
{
	WIFI_CMD_GET_MODEL_NAME			= 0x0d,
	WIFI_CMD_GET_SERIAL_NNUMBER		= 0x0e,
	WIFI_CMD_GET_SYSTEM_TIME		= 0x0f,
	WIFI_CMD_GET_WHITE_CARD			= 0x12,
	WIFI_CMD_GET_VER_RFS			= 0x1e,
	WIFI_CMD_GET_VER_PRIMARY_CSU	= 0x1f,
	WIFI_CMD_GET_VER_LCM			= 0x20,
	WIFI_CMD_GET_VER_PRIMARY_PSU	= 0x21,
	WIFI_CMD_GET_VER_SECOND_PSU		= 0x22,
	WIFI_CMD_GET_VER_AUX_POWER		= 0x23,
	WIFI_CMD_GET_VER_FAN			= 0x24,
	WIFI_CMD_GET_VER_RELAY			= 0x25,
	WIFI_CMD_GET_VER_TELECOMM		= 0x26,
	WIFI_CMD_GET_EVSE_CONFIG		= 0x30,
	WIFI_CMD_GET_GUN_STS			= 0x32,
	WIFI_CMD_GET_SCHEDULE			= 0x36,
	WIFI_CMD_GET_NETWORK_STS		= 0x41,
	WIFI_CMD_GET_ETH_DHCP			= 0x42,
	WIFI_CMD_GET_ETH_MAC			= 0x44,
	WIFI_CMD_GET_ETH_IP				= 0x45,
	WIFI_CMD_GET_ETH_SUBMASK		= 0x47,
	WIFI_CMD_GET_ETH_GATEWAY		= 0x49,
	WIFI_CMD_GET_WIFI_MODE			= 0x54,
	WIFI_CMD_GET_WIFI_SSID			= 0x56,
	WIFI_CMD_GET_WIFI_PASSWD		= 0x58,
	WIFI_CMD_GET_WIFI_RSSI			= 0x5a,
	WIFI_CMD_GET_WIFI_DHCP			= 0x5b,
	WIFI_CMD_GET_WIFI_MAC			= 0x5d,
	WIFI_CMD_GET_WIFI_IP			= 0x5e,
	WIFI_CMD_GET_WIFI_NETMASK		= 0x60,
	WIFI_CMD_GET_WIFI_GATEWAY		= 0x62,
	WIFI_CMD_GET_TELE_APN			= 0x6b,
	WIFI_CMD_GET_TELE_RSSI			= 0x6d,
	WIFI_CMD_GET_TELE_CHAAP_ID		= 0x6e,
	WIFI_CMD_GET_TELE_CHAAP_PASSWD	= 0x70,
	WIFI_CMD_GET_TELE_IMEI			= 0x72,
	WIFI_CMD_GET_TELE_IMSI			= 0x73,
	WIFI_CMD_GET_TELE_SIM_STS		= 0x74,
	WIFI_CMD_GET_TELE_MODE			= 0x75,
	WIFI_CMD_GET_TELE_IP			= 0x76,


	WIFI_CMD_SET_SYSTEM_TIME		= 0x10,
	WIFI_CMD_SET_WHITE_CARD			= 0x13,
	WIFI_CMD_SET_FACTORY_RESET		= 0x14,
	WIFI_CMD_SET_EVSE_CONFIG		= 0x31,
	WIFI_CMD_SET_CHARGING_START		= 0x34,
	WIFI_CMD_SET_CHARGING_STOP		= 0x35,
	WIFI_CMD_SET_SCHEDULE			= 0x37,
	WIFI_CMD_SET_ETH_CONFIG			= 0x43,
	WIFI_CMD_SET_ETH_IP				= 0x46,
	WIFI_CMD_SET_ETH_SUBMASK		= 0x48,
	WIFI_CMD_SET_ETH_GATEWAY		= 0x4a,
	WIFI_CMD_SET_WIFI_MODE			= 0x55,
	WIFI_CMD_SET_WIFI_SSID			= 0x57,
	WIFI_CMD_SET_WIFI_PASSWD		= 0x59,
	WIFI_CMD_SET_WIFI_DHCP			= 0x5c,
	WIFI_CMD_SET_WIFI_IP			= 0x5f,
	WIFI_CMD_SET_WIFI_NETMASK		= 0x61,
	WIFI_CMD_SET_WIFI_GATEWAY		= 0x63,
	WIFI_CMD_SET_TELE_APN			= 0x6c,
	WIFI_CMD_SET_TELE_CHAAP_ID		= 0x6f,
	WIFI_CMD_SET_TELE_CHAAP_PASSWD	= 0x71,
	WIFI_CMD_SET_CONFIRM			= 0x81,

	WIFI_CMD_UNKNOW					= 0xfe

};

struct interface_info
{
	int ipType;
	int authType;
	int rssi;
	int cnt_InternetFail;
	char *currentInterface;
	char currentSSID[256];
	char currentPasswd[256];
	char currentMAC[24];
	char currentIPAddr[24];
	char currentNetmask[24];
	char currentGateway[24];
	char staticIPAddr[24];
}Wifi;

struct Display_Request
{
	unsigned char isShowed_IF_info:1;
	unsigned char isShowed_IP_info:1;
}dispReq;

struct auth_info
{
	int auth_type;
	unsigned char ssid[256];
	unsigned char passwd[256];
}Wifi_A;

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;
struct OCPP16Data				*ShmOCPP16Data;

int isFindInterface();
int getInterfaceInfo();
int isReachableInternet();
int getLinkQuality();
char* getTimeString(void);
int setWPAconf();
int restartWPA();
void refreshUSB();
void getParameters();

int Wifi_module_sts = STA_STATE_INIT;
int isSetWPA_OK = false;

int cnt_pingDNS_Fail;
int cnt_getAP_Fail;
int cnt_getIP_Fail;

void trim(char *s);
int mystrcmp(char *p1,char *p2);
void substr(char *dest, const char* src, unsigned int start, unsigned int cnt);
void split(char **arr, char *str, const char *del);
int isValidCheckSum(uint8_t *message);

char *Support_InterfaceSTA[2] = {"mlan0", "wlan0"};
char *Support_InterfaceAP[1]  = {"uap0"};
char *valid_Internet[2] 	  = {"8.8.8.8", "180.76.76.76"};

int StoreLogMsg(const char *fmt, ...)
{
	char Buf[4096+256];
	char buffer[4096];
	time_t CurrentTime;
	struct tm *tm;
	va_list args;

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

	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]Wifi_SystemLog",
			tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,
			buffer,
			tm->tm_year+1900,tm->tm_mon+1);
#ifdef SystemLogMessage
	system(Buf);
#endif

#ifdef ConsloePrintLog	
	printf("[%04d.%02d.%02d %02d:%02d:%02d] - %s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec, buffer);
#endif

	return rc;
}

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

//==========================================
// Init all share memory
//==========================================
int InitShareMemory()
{
	int result = PASS;
	int MeterSMId;

	//creat ShmSysConfigAndInfo
	if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0)
    {
		#ifdef SystemLogMessage
		DEBUG_ERROR("shmget ShmSysConfigAndInfo NG\n");
		#endif
		result = FAIL;
	}
    else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
    	#ifdef SystemLogMessage
    	DEBUG_ERROR("shmat ShmSysConfigAndInfo NG\n");
		#endif
    	result = FAIL;
   	 }
    else
    {}

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

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

    return result;
}

//==========================================
// Get parameters from shared memory
//==========================================
void getParameters(void)
{
	DEBUG_INFO("============= Configuration ===============\r\n");
	strcpy((char*)Wifi_A.ssid, (const char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid);
	DEBUG_INFO("Wifi SSID: %s\n", Wifi_A.ssid);
	strcpy((char*)Wifi_A.passwd, (const char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword);
	DEBUG_INFO("Wifi Password: %s\n", Wifi_A.passwd);
	if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress != NULL)
		strcpy((char*)Wifi.staticIPAddr, (const char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress);
	DEBUG_INFO("Wifi Static IP: %s\n", Wifi.staticIPAddr);
	Wifi.ipType = ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient;
	DEBUG_INFO("DHCP client flag: %d\n", Wifi.ipType);
	DEBUG_INFO("===========================================\r\n");
}

//==========================================
// Refresh USB interface
//==========================================
void refreshUSB(void)
{
	FILE *fp;
	char cmd[256];

	// Get IP address & net mask
	strcpy(cmd, "echo '1-1'> /sys/bus/usb/drivers/usb/unbind");
	fp = popen(cmd, "r");
	sleep(2);
	pclose(fp);

	strcpy(cmd, "echo '1-1'> /sys/bus/usb/drivers/usb/bind");
	fp = popen(cmd, "r");
	sleep(5);
	pclose(fp);
}

//==========================================
// Check wifi interface status
//==========================================
int isFindInterface()
{
	int result = FAIL;

	struct ifaddrs *ifaddr, *ifa;

	//refreshUSB();

	if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode == WIFI_MODE_AP)
	{
		if (getifaddrs(&ifaddr) != FAIL)
		{
			for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next)
			{
				if (ifa->ifa_addr == NULL ||
					ifa->ifa_addr->sa_family != AF_PACKET) continue;

				for(int idx=0;idx<ARRAY_SIZE(Support_InterfaceAP);idx++)
				{
					if(mystrcmp(ifa->ifa_name, Support_InterfaceAP[idx]) == PASS)
					{
						Wifi.currentInterface = ifa->ifa_name;
						result = PASS;
					}
				}
			}

			freeifaddrs(ifaddr);
			freeifaddrs(ifa);
		}
	}
	else
	{
		if (getifaddrs(&ifaddr) != FAIL)
		{
			for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next)
			{
				if (ifa->ifa_addr == NULL ||
					ifa->ifa_addr->sa_family != AF_PACKET) continue;

				for(int idx=0;idx<ARRAY_SIZE(Support_InterfaceSTA);idx++)
				{
					if(mystrcmp(ifa->ifa_name, Support_InterfaceSTA[idx]) == PASS)
					{
						Wifi.currentInterface = ifa->ifa_name;
						result = PASS;
					}
				}
			}

			freeifaddrs(ifaddr);
			freeifaddrs(ifa);
		}
	}



	return result;
}

//==========================================
// Get wifi interface info
//==========================================
int getInterfaceInfo()
{
	int result = PASS;
	FILE *fp;
	char cmd[256];
	char buf[512];
	char tmp[512];

	// Get IP address & net mask
	strcpy(cmd, "ifconfig ");
	strcat(cmd, Wifi.currentInterface);
	fp = popen(cmd, "r");
	if(fp == NULL)
		result = FAIL;
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, "HWaddr") > 0)
			{
				sscanf(buf, "%*s%*s%*s%*s%s", tmp);
				strcpy(Wifi.currentMAC, tmp);
			}

			if(strstr(buf, "inet addr:") > 0)
			{
				sscanf(buf, "%*s%s", tmp);
				substr(Wifi.currentIPAddr, tmp, strspn(tmp, "addr:"), strlen(buf)-strspn(tmp, "addr:"));

				sscanf(buf, "%*s%*s%*s%s", tmp);
				substr(Wifi.currentNetmask, tmp, strspn(tmp, "Mask:"), strlen(buf)-strspn(tmp, "Mask:"));
			}
		}
	}
	pclose(fp);

	// Get gateway
	fp = popen("ip route", "r");
	if(fp == NULL)
		result = FAIL;
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strncmp(buf, "default", strlen("default")) == 0)
				break;
		}
		sscanf(buf, "%*s%*s%s", tmp);
		substr(Wifi.currentGateway, tmp, 0, strlen(tmp));
	}
	pclose(fp);

	if(!dispReq.isShowed_IF_info)
	{
		DEBUG_INFO("MAC: %s\n", Wifi.currentMAC);
		dispReq.isShowed_IF_info = true;
	}

	if(!dispReq.isShowed_IP_info)
	{
		DEBUG_INFO("IP address: %s\n", Wifi.currentIPAddr);
		DEBUG_INFO("Net mask: %s\n", Wifi.currentNetmask);
		DEBUG_INFO("Default gateway: %s\n", Wifi.currentGateway);
		dispReq.isShowed_IP_info = true;
	}


	strcpy((char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress, (const char*)Wifi.currentIPAddr);
	strcpy((char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress, (const char*)Wifi.currentNetmask);
	strcpy((char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress, (const char*)Wifi.currentGateway);
	strcpy((char*)ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress, (const char*)Wifi.currentMAC);

	return result;
}

//==========================================
// Check if module connect to wifi AP
//==========================================
int isConnectAP()
{
	int result = PASS;
	FILE *fp;
	char cmd[256];
	char buf[512];

	// Get IP address & net mask
	strcpy(cmd, "/root/iwconfig ");
	strcat(cmd, Wifi.currentInterface);
	fp = popen(cmd, "r");
	if(fp == NULL)
		result = FAIL;
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, "Not-Associated") > 0)
			{
				result = FAIL;
				DEBUG_INFO("AP check: Not-Associated.\n");
			}
		}
	}
	pclose(fp);

	return result;
}

//==========================================
// Check internet access status
//==========================================
int isReachableInternet()
{
	int result = FAIL;
	FILE *fp;
	char cmd[256];
	char buf[512];
	//char tmp[512];

	for(int idx=0;idx<ARRAY_SIZE(valid_Internet);idx++)
	{
		strcpy(cmd, "ping -c 1 -w 3 ");
		strcat(cmd, valid_Internet[idx]);
		fp = popen(cmd, "r");
		if(fp != NULL)
		{
			while(fgets(buf, sizeof(buf), fp) != NULL)
			{
				if(strstr(buf, "transmitted") > 0)
				{
					//sscanf(buf, "%*s%*s%*s%*s%*s%*s%s", tmp);

					if(strstr(buf,"100%") != NULL)
					{

					}
					else
					{
						result = PASS;
					}
					//DEBUG_INFO("%s",buf);
					//DEBUG_INFO("%s\n",tmp);
				}
			}
		}
		pclose(fp);
	}

	return result;
}

//==========================================
// Check module soft AP is start up
//==========================================
int isStartUpAP()
{
	int result = FAIL;
	FILE *fp;
	char cmd[256];
	char buf[512];

	// Get IP address & net mask
	strcpy(cmd, "/root/uaputl sys_cfg_bss_status");

	fp = popen(cmd, "r");
	if(fp != NULL)
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, "BSS status = started") > 0)
			{
				result = PASS;
				DEBUG_INFO("AP already start up.\n");
			}
		}
	}
	pclose(fp);

	return result;
}

//==========================================
// Get wifi connection quality
//==========================================
int getLinkQuality()
{
	int result = FAIL;
	FILE *fp;
	char cmd[256];
	char buf[512];
	char tmp[512];

	strcpy(cmd, "/root/iwconfig ");
	strcat(cmd, Wifi.currentInterface);
	fp = popen(cmd, "r");
	if(fp == NULL)
		result = FAIL;
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, "Signal level") > 0)
			{
				sscanf(buf, "%*s%*s%*s%s", tmp);

				substr(tmp, tmp, strspn(tmp, "Signal level="), strlen(buf)-strspn(tmp, "Signal level="));

				Wifi.rssi = atoi(tmp);

				result = PASS;
			}
		}
	}
	pclose(fp);

	return result;
}

//==========================================
// WPA parameter set
//==========================================
int setWPAconf()
{
	int result = FAIL;

	char cmdBuf[512];
	FILE *pFile;
	char buffer[500]="ctrl_interface=/var/run/wpa_supplicant\nupdate_config=1\n";

	// Stop dhcp client or server
	sprintf(cmdBuf, "pgrep -f \"udhcpc -i %s\" | xargs kill", Wifi.currentInterface);
	system(cmdBuf);

	sprintf(cmdBuf, "pgrep -f \"udhcpd\" | xargs kill");
	system(cmdBuf);

	// Clear wifi configuration file
	system("cat /dev/null > /etc/wpa.conf");
	system("cat /dev/null > /etc/hostapd.conf");

	switch(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode)
	{
		case WIFI_MODE_STA:
			// Fill out configuration value depend on share memory setting
			if(strlen((const char*)Wifi_A.passwd)>0)
			{
				if(strlen((const char*)Wifi_A.passwd)>=8)
				{
					strcat(buffer, "network={\n    ssid=\"");
					strcat(buffer, (char*)Wifi_A.ssid);
					strcat(buffer, "\"\n    key_mgmt=WPA-EAP WPA-PSK \n");
					strcat(buffer, "    psk=\"");
					strcat(buffer, (char*)Wifi_A.passwd);
					strcat(buffer, "\"\n}\n\n");
				}

				if(strlen((const char*)Wifi_A.passwd)==5 || strlen((const char*)Wifi_A.passwd)==13)
				{
					strcat(buffer, "network={\n    ssid=\"");
					strcat(buffer, (char*)Wifi_A.ssid);
					strcat(buffer, "\"\n    key_mgmt=NONE\n");
					strcat(buffer, "    wep_key0=\"");
					strcat(buffer, (char*)Wifi_A.passwd);
					strcat(buffer, "\"\n    auth_alg=OPEN SHARED\n");
					strcat(buffer, "\n}\n\n");
				}

				strcat(buffer, "network={\n    ssid=\"");
				strcat(buffer, (char*)Wifi_A.ssid);
				strcat(buffer, "\"\n    key_mgmt=NONE");
				strcat(buffer, "\n}");
			}
			else
			{
				strcat(buffer, "network={\n    ssid=\"");
				strcat(buffer, (char*)Wifi_A.ssid);
				strcat(buffer, "\"\n    key_mgmt=NONE");
				strcat(buffer, "\n}");
			}

			// Configuration value write to /etc/wpa.conf
			pFile = fopen("/etc/wpa.conf","w");
			fwrite(buffer,strlen(buffer), 1, pFile);
			fclose(pFile);

			// Restart wpa_supplicant
			sprintf(cmdBuf, "pgrep -f \"wpa_supplicant\" | xargs kill");
			system(cmdBuf);
			sprintf(cmdBuf, "/root/wpa_supplicant -i %s -c /etc/wpa.conf -B", Wifi.currentInterface);
			system(cmdBuf);

			// Stop wifi AP
			sprintf(cmdBuf, "pgrep -f \"hostapd\" | xargs kill");
		        system(cmdBuf);

			sleep(5);

			// Wifi IP set by DHCP client or static
			if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient==0)
			{
				sprintf(cmdBuf, "/sbin/udhcpc -i %s -s /root/dhcp_script/wifi.script > /dev/null &", Wifi.currentInterface);
				system(cmdBuf);
			}
			else
			{
				memset(cmdBuf, 0, ARRAY_SIZE(cmdBuf));
				sprintf(cmdBuf,"/sbin/ifconfig %s %s netmask %s",
				Wifi.currentInterface,
				ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress,
				ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress);
				system(cmdBuf);

				memset(cmdBuf, 0, ARRAY_SIZE(cmdBuf));
				sprintf(cmdBuf,"route add default gw %s %s ",
				ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress,
				Wifi.currentInterface);
				system(cmdBuf);

				DEBUG_INFO("Setting %s static IP to %s submask to %s gateway to %s...\n",
						   Wifi.currentInterface,
						   ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress,
						   ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress,
						   ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress);
			}
			break;
		case WIFI_MODE_AP:
			sprintf(cmdBuf, "pgrep -f \"hostapd\" | xargs kill");
			system(cmdBuf);
			sprintf(cmdBuf, "echo interface=uap0 >> /etc/hostapd.conf");
			system(cmdBuf);
			sprintf(cmdBuf, "echo driver=nl80211 >> /etc/hostapd.conf");
			system(cmdBuf);
			sprintf(cmdBuf, "echo ssid=%s%s >> /etc/hostapd.conf", ShmSysConfigAndInfo->SysConfig.ModelName, ShmSysConfigAndInfo->SysConfig.SerialNumber);
			system(cmdBuf);
			sprintf(cmdBuf, "echo channel=6 >> /etc/hostapd.conf");
			system(cmdBuf);
			sprintf(cmdBuf, "echo wpa=2 >> /etc/hostapd.conf");
			system(cmdBuf);
			sprintf(cmdBuf, "echo wpa_passphrase=%s%s >> /etc/hostapd.conf", ShmSysConfigAndInfo->SysConfig.SerialNumber, ShmSysConfigAndInfo->SysConfig.ModelName);
			system(cmdBuf);
			sprintf(cmdBuf, "echo wpa_key_mgmt=WPA-PSK >> /etc/hostapd.conf");
			system(cmdBuf);
			sprintf(cmdBuf, "hostapd /etc/hostapd.conf -B");
			system(cmdBuf);

			sleep(5);

			// Set wifi AP ip address
			sprintf(cmdBuf, "ifconfig %s 192.168.10.10", Wifi.currentInterface);
			system(cmdBuf);
			DEBUG_INFO("Setting %s static IP to 192.168.10.10...\n", Wifi.currentInterface);

			// Start DHCP server
			if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpServer==0)
			{
				sprintf(cmdBuf, "/usr/sbin/udhcpd /etc/udhcpd.conf > /dev/null &");
				system(cmdBuf);
			}
			break;
		case WIFI_MODE_ADHOC:
			break;
		case WIFI_MODE_DISABLE:
		default:
			break;

	}

	result = PASS;

	return result;
}

//==========================================
// UDP socket server routine
//==========================================
int split2int(char *dest, char *src, const char *flag, int base)
{
	uint8_t result = 0;
	char *s = strtok(src, flag);

	while(s != NULL)
	{
		*dest++ = (base==10?atoi(s):strtol(s, NULL, 16));
		s = strtok(NULL, flag);
		result++;
	}

	return result;
}

uint8_t parseIP(uint8_t *ip)
{
	uint8_t result = FAIL;
	const int8_t		flag_dot[1] = {'.'};
	int8_t buf[16];
	memcpy(buf, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress, ARRAY_SIZE(buf));

	if(split2int((char*)ip , (char*)buf, (char*)flag_dot, 10)==4)
		result = PASS;

	return result;
}

uint8_t parseMAC(uint8_t *mac)
{
	uint8_t result = FAIL;
	const int8_t		flag_colon[1] = {':'};
	int8_t buf[18];
	memcpy(buf, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress, ARRAY_SIZE(buf));

	if(split2int((char*)mac , (char*)buf, (char*)flag_colon, 16)==6)
		result = PASS;

	return result;
}

int udpSocketServerStart(void)
{
	 int sockFd;
	 struct sockaddr_in servaddr;
	 struct sockaddr_in peeraddr;
	 socklen_t 			peerlen = sizeof(peeraddr);
	 uint8_t 			inputBuffer[2048] = {};
	 uint8_t 			outBuffer[2048] = {};
	 int16_t			read_size;
	 int16_t			tx_size;
	 uint8_t			broadcastCmd[6] = {0xff, 0xff, 0x00, 0x02, 0x00, 0x02};
	 uint8_t			validResult;
	 uint8_t			chksum;
	 uint8_t			ip[4];
	 uint8_t			mac[6];



	 memset(&servaddr, 0, sizeof(servaddr));
	 servaddr.sin_family = AF_INET;
	 servaddr.sin_port = htons(LISTEN_PORT_UDP);
	 servaddr.sin_addr.s_addr = htonl(INADDR_ANY);

	 if ((sockFd = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
		 return FAIL;

	 if (bind(sockFd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0)
	     return FAIL;

	 for(;;)
	 {
		 if((read_size = recvfrom(sockFd, inputBuffer, sizeof(inputBuffer), 0, (struct sockaddr *)&peeraddr, &peerlen)) > 0)
		 {
			 DEBUG_INFO("Revieve from: %s:%d\r\n", inet_ntoa(peeraddr.sin_addr), htons(peeraddr.sin_port));

			 if(read_size>=6)
			 {
				 validResult = PASS;
				 for(uint8_t idx=0;idx<6;idx++)
				 {
					 if(inputBuffer[idx] != broadcastCmd[idx])
					 {
						 validResult = FAIL;
						 break;
					 }
				 }

				 if(validResult)
				 {
					 memset(outBuffer, 0x00, ARRAY_SIZE(outBuffer));
					 tx_size = 41;

					 outBuffer[0] = 0xff;
					 outBuffer[1] = 0xff;
					 outBuffer[2] = (0x1a << 0x08) & 0xff;
					 outBuffer[3] = 0x1a;
					 outBuffer[4] = 0x00;
					 if(parseIP(ip)==PASS)
					 {
						 outBuffer[5] = ip[0];
						 outBuffer[6] = ip[1];
						 outBuffer[7] = ip[2];
						 outBuffer[8] = ip[3];
					 }
					 else
					 {
						 outBuffer[5] = 0x00;
						 outBuffer[6] = 0x00;
						 outBuffer[7] = 0x00;
						 outBuffer[8] = 0x00;
					 }

					 if(parseMAC(mac)==PASS)
					 {
						 outBuffer[9] 	= mac[0];
						 outBuffer[10] 	= mac[1];
						 outBuffer[11] 	= mac[2];
						 outBuffer[12] 	= mac[3];
						 outBuffer[13] 	= mac[4];
						 outBuffer[14] 	= mac[5];
					 }
					 else
					 {
						 outBuffer[9] 	= 0x00;
						 outBuffer[10] 	= 0x00;
						 outBuffer[11] 	= 0x00;
						 outBuffer[12] 	= 0x00;
						 outBuffer[13] 	= 0x00;
						 outBuffer[14] 	= 0x00;
					 }
					 memcpy(&outBuffer[15], ShmSysConfigAndInfo->SysConfig.ModelName, 0x0e);
					 memcpy(&outBuffer[29], ShmSysConfigAndInfo->SysConfig.SerialNumber, 0x0b);

					 chksum = 0x00;
					 for(uint8_t idx=0;idx<36;idx++)
					 {
						 chksum ^= outBuffer[idx];
					 }
					 outBuffer[40] = chksum;

					 sendto(sockFd, outBuffer, tx_size, 0, (struct sockaddr *)&peeraddr, peerlen);
				 }
			 }
		 }
	 }

	return FAIL;
}

//==========================================
// TCP socket server routine
//==========================================
int isValidCheckSum(uint8_t *message)
{
	uint8_t	chksum=0;

	for(int idx=0;idx<(((message[2]<<8) | message[3])>1024?1024:(((message[2]<<8) | message[3])-1));idx++)
	{
		chksum ^= message[4+idx];
	}

	return ((chksum == message[3+(((message[2]<<8) | message[3])>1024?1024:((message[2]<<8) | message[3]))]) ? PASS : FAIL);
}

int chksumCal(uint8_t *message)
{
	uint8_t chksum=0;

	for(int idx = 0;idx<(((message[2]<<8) | message[3])>1024?1024:(((message[2]<<8) | message[3])-1));idx++)
	{
		chksum ^= message[4+idx];
	}

	return chksum & 0xff;
}

int StoreUsrConfigData(struct SysConfigData *UsrData)
{
	int result = PASS;
	int fd,wrd;
	unsigned int i,Chk;
	unsigned char *ptr, *BufTmp;

	Chk=0;
	ptr=(unsigned char *)UsrData;
	if((BufTmp=malloc(MtdBlockSize))!=NULL)
	{
		memset(BufTmp,0,MtdBlockSize);
		memcpy(BufTmp,ptr,sizeof(struct SysConfigData));
		for(i=0;i<MtdBlockSize-4;i++)
			Chk+=*(BufTmp+i);
		memcpy(	BufTmp+MtdBlockSize-4,&Chk,4);
		fd = open("/dev/mtdblock10", O_RDWR);
		if (fd>0)
		{
			wrd=write(fd, BufTmp, MtdBlockSize);
			close(fd);
			if(wrd>=MtdBlockSize)
			{
				fd = open("/dev/mtdblock11", O_RDWR);
				if (fd>0)
				{
					wrd=write(fd, BufTmp, MtdBlockSize);
    					close(fd);
				   	if(wrd<MtdBlockSize)
					{
						DEBUG_ERROR("write /dev/mtdblock11(backup) NG\r\n");
				   		result = FAIL;
					}
				}
				else
				{
					DEBUG_ERROR("open /dev/mtdblock11(backup) NG\r\n");
					result = FAIL;
				}
			}
			else
			{
		    		DEBUG_ERROR("write /dev/mtdblock10 NG\r\n");
		    		result = FAIL;
			}
		}
		else
		{
			DEBUG_ERROR("open /dev/mtdblock10 NG\r\n");
			result = FAIL;
		}
	}
	else
	{
		DEBUG_ERROR("alloc BlockSize NG\r\n");
    		result = FAIL;
	}
	if(BufTmp!=NULL)
		free(BufTmp);

	return result;
}

void getConnectorInfo(uint8_t gun_index, uint8_t *outBuffer)
{
	uint8_t idxAC = 0 , idxCCS = 0, idxCHAdeMO = 0, idxGBT = 0;

	for(uint8_t idx=0;idx<gun_index;idx++)
	{
		switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+idx])
		{
			case '1' ... '6':
				idxAC++;
				break;
			case 'J':
				idxCHAdeMO++;
				break;
			case 'U':
			case 'E':
				idxCCS++;
				break;
			case 'G':
				idxGBT++;
				break;
		}
	}

	/*
	 *	TODO:
	 *	1. Alarm code
	 */
	switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+gun_index])
	{
		case '1' ... '6':
			// AC
			outBuffer[6] = ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].SystemStatus;
			outBuffer[7] = 0x00;
			outBuffer[8] = 0x00;
			outBuffer[9] = 0x00;
			outBuffer[10] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingVoltage*10)>>8)&0xff;
			outBuffer[11] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingVoltage*10)>>0)&0xff;
			outBuffer[12] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingCurrent*10)>>8)&0xff;
			outBuffer[13] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingCurrent*10)>>0)&0xff;
			outBuffer[14] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingPower*10)>>8)&0xff;
			outBuffer[15] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargingPower*10)>>0)&0xff;
			outBuffer[16] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargedEnergy*10)>>8)&0xff;
			outBuffer[17] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargedEnergy*10)>>0)&0xff;
			outBuffer[18] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargedDuration)>>8)&0xff;
			outBuffer[19] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].PresentChargedDuration)>>0)&0xff;
			outBuffer[20] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].RemainChargingDuration)>>8)&0xff;
			outBuffer[21] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].RemainChargingDuration)>>0)&0xff;
			outBuffer[22] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].EvBatterySoc)>>0)&0xff;

			break;
		case 'J':
			// CHAdeMO
			outBuffer[6] = ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].SystemStatus;
			outBuffer[7] = 0x00;
			outBuffer[8] = 0x00;
			outBuffer[9] = 0x00;
			outBuffer[10] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingVoltage*10)>>8)&0xff;
			outBuffer[11] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingVoltage*10)>>0)&0xff;
			outBuffer[12] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingCurrent*10)>>8)&0xff;
			outBuffer[13] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingCurrent*10)>>0)&0xff;
			outBuffer[14] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingPower*10)>>8)&0xff;
			outBuffer[15] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargingPower*10)>>0)&0xff;
			outBuffer[16] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargedEnergy*10)>>8)&0xff;
			outBuffer[17] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargedEnergy*10)>>0)&0xff;
			outBuffer[18] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargedDuration)>>8)&0xff;
			outBuffer[19] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].PresentChargedDuration)>>0)&0xff;
			outBuffer[20] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].RemainChargingDuration)>>8)&0xff;
			outBuffer[21] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].RemainChargingDuration)>>0)&0xff;
			outBuffer[22] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].EvBatterySoc)>>0)&0xff;

			break;
		case 'U':
		case 'E':
			// CCS
			outBuffer[6] = ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].SystemStatus;
			outBuffer[7] = 0x00;
			outBuffer[8] = 0x00;
			outBuffer[9] = 0x00;
			outBuffer[10] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingVoltage*10)>>8)&0xff;
			outBuffer[11] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingVoltage*10)>>0)&0xff;
			outBuffer[12] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingCurrent*10)>>8)&0xff;
			outBuffer[13] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingCurrent*10)>>0)&0xff;
			outBuffer[14] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingPower*10)>>8)&0xff;
			outBuffer[15] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargingPower*10)>>0)&0xff;
			outBuffer[16] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargedEnergy*10)>>8)&0xff;
			outBuffer[17] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargedEnergy*10)>>0)&0xff;
			outBuffer[18] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargedDuration)>>8)&0xff;
			outBuffer[19] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].PresentChargedDuration)>>0)&0xff;
			outBuffer[20] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].RemainChargingDuration)>>8)&0xff;
			outBuffer[21] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].RemainChargingDuration)>>0)&0xff;
			outBuffer[22] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].EvBatterySoc)>>0)&0xff;

			break;
		case 'G':
			// GBT
			outBuffer[6] = ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].SystemStatus;
			outBuffer[7] = 0x00;
			outBuffer[8] = 0x00;
			outBuffer[9] = 0x00;
			outBuffer[10] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingVoltage*10)>>8)&0xff;
			outBuffer[11] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingVoltage*10)>>0)&0xff;
			outBuffer[12] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingCurrent*10)>>8)&0xff;
			outBuffer[13] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingCurrent*10)>>0)&0xff;
			outBuffer[14] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingPower*10)>>8)&0xff;
			outBuffer[15] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargingPower*10)>>0)&0xff;
			outBuffer[16] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargedEnergy*10)>>8)&0xff;
			outBuffer[17] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargedEnergy*10)>>0)&0xff;
			outBuffer[18] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargedDuration)>>8)&0xff;
			outBuffer[19] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].PresentChargedDuration)>>0)&0xff;
			outBuffer[20] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].RemainChargingDuration)>>8)&0xff;
			outBuffer[21] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].RemainChargingDuration)>>0)&0xff;
			outBuffer[22] = ((uint16_t)(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].EvBatterySoc)>>0)&0xff;

			break;
		default:
			break;
	}
}

void getConnectorSchedule(uint8_t gun_index, uint8_t *outBuffer)
{
	uint8_t idxAC = 0 , idxCCS = 0, idxCHAdeMO = 0, idxGBT = 0;

	for(uint8_t idx=0;idx<gun_index;idx++)
	{
		switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+idx])
		{
			case '1' ... '6':
				idxAC++;
				break;
			case 'J':
				idxCHAdeMO++;
				break;
			case 'U':
			case 'E':
				idxCCS++;
				break;
			case 'G':
				idxGBT++;
				break;
		}
	}
	/*
	 * 	TODO:
	 * 	1. Connector schedule get
	 */
}

uint8_t startConnectorCharging(uint8_t gun_index)
{
	uint8_t idxAC = 0 , idxCCS = 0, idxCHAdeMO = 0, idxGBT = 0;
	uint8_t result = FAIL;

	for(uint8_t idx=0;idx<gun_index;idx++)
	{
		switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+idx])
		{
			case '1' ... '6':
				idxAC++;
				break;
			case 'J':
				idxCHAdeMO++;
				break;
			case 'U':
			case 'E':
				idxCCS++;
				break;
			case 'G':
				idxGBT++;
				break;
		}
	}

	switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+gun_index])
	{
		case '1' ... '6':
			// AC
			if(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].SystemStatus == SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'J':
			// CHAdeMO
			if(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].SystemStatus == SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'U':
		case 'E':
			// CCS
			if(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].SystemStatus == SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'G':
			// GBT
			if(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].SystemStatus == SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStartTransactionReq = ON;
				result = PASS;
			}

			break;
		default:
			break;
	}

	return result;
}

uint8_t stopConnectorCharging(uint8_t gun_index)
{
	uint8_t idxAC = 0 , idxCCS = 0, idxCHAdeMO = 0, idxGBT = 0;
	uint8_t result = FAIL;

	for(uint8_t idx=0;idx<gun_index;idx++)
	{
		switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+idx])
		{
			case '1' ... '6':
				idxAC++;
				break;
			case 'J':
				idxCHAdeMO++;
				break;
			case 'U':
			case 'E':
				idxCCS++;
				break;
			case 'G':
				idxGBT++;
				break;
		}
	}

	switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+gun_index])
	{
		case '1' ... '6':
			// AC
			if(ShmSysConfigAndInfo->SysInfo.AcChargingData[idxAC].SystemStatus != SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'J':
			// CHAdeMO
			if(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[idxCHAdeMO].SystemStatus != SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'U':
		case 'E':
			// CCS
			if(ShmSysConfigAndInfo->SysInfo.CcsChargingData[idxCCS].SystemStatus != SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = ON;
				result = PASS;
			}

			break;
		case 'G':
			// GBT
			if(ShmSysConfigAndInfo->SysInfo.GbChargingData[idxGBT].SystemStatus != SYS_MODE_IDLE)
			{
				ShmOCPP16Data->CsMsg.bits[gun_index].RemoteStopTransactionReq = ON;
				result = PASS;
			}

			break;
		default:
			break;
	}

	return result;
}

void setConnectorSchedule(uint8_t gun_index, uint8_t *inputBuffer)
{
	uint8_t idxAC = 0 , idxCCS = 0, idxCHAdeMO = 0, idxGBT = 0;

	for(uint8_t idx=0;idx<gun_index;idx++)
	{
		switch(ShmSysConfigAndInfo->SysConfig.ModelName[7+idx])
		{
			case '1' ... '6':
				idxAC++;
				break;
			case 'J':
				idxCHAdeMO++;
				break;
			case 'U':
			case 'E':
				idxCCS++;
				break;
			case 'G':
				idxGBT++;
				break;
		}
	}

	/*
	 * 	TODO:
	 * 	1. Connector schedule configuration
	 */
}

int tcpSocketServerStart(void)
{
	int 				sockFd = 0;
	int 				clientSockFd = 0;
	uint8_t 			inputBuffer[2048] = {};
	uint8_t 			outBuffer[2048] = {};
	int16_t 			read_size;
	int16_t				tx_size;
	uint32_t			randomChk;
	struct sockaddr_in 	serverInfo, clientInfo;
	socklen_t 			addrlen = sizeof(clientInfo);
	uint8_t				cmdBuf[512];
	time_t 				rawtime;
	struct tm  			ts;

	sockFd = socket(AF_INET , SOCK_STREAM , 0);
	if(sockFd == -1)
	{
		DEBUG_ERROR("InitSocketServer NG\n");
		sleep(5);
		return FAIL;
	}

	bzero(&serverInfo,sizeof(serverInfo));
	serverInfo.sin_family = PF_INET;
	serverInfo.sin_addr.s_addr = htonl(INADDR_ANY);
	serverInfo.sin_port = htons(LISTEN_PORT_TCP);
	bind(sockFd, (struct sockaddr *)&serverInfo, sizeof(serverInfo));
	listen(sockFd, CONNECTION_LIMIT);

	// Main loop
	for(;;)
	{
		clientSockFd = accept(sockFd, (struct sockaddr*) &clientInfo, &addrlen);
		DEBUG_INFO("Client connect in.\r\n");

		while((read_size = recv(clientSockFd, inputBuffer, sizeof(inputBuffer), 0)) > 0)
		{
			if(isValidCheckSum(inputBuffer) && (randomChk != ((inputBuffer[4]<<24) | (inputBuffer[5]<<16) | (inputBuffer[6]<<8) | (inputBuffer[7]<<0))))
			{
				randomChk = ((inputBuffer[4]<<24) | (inputBuffer[5]<<16) | (inputBuffer[6]<<8) | (inputBuffer[7]<<0));
				memset(outBuffer, 0x00, sizeof(outBuffer));

				switch(inputBuffer[8])
				{
					case WIFI_CMD_GET_MODEL_NAME:
						tx_size = 70;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x42;
						outBuffer[4] = WIFI_CMD_GET_MODEL_NAME;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.ModelName, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.ModelName));
						outBuffer[69] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_SERIAL_NNUMBER:
						tx_size = 70;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x42;
						outBuffer[4] = WIFI_CMD_GET_SERIAL_NNUMBER;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.ModelName, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.SerialNumber));
						outBuffer[69] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_SYSTEM_TIME:
						tx_size = 10;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x42;
						outBuffer[4] = WIFI_CMD_GET_SYSTEM_TIME;
						outBuffer[5] = (time(NULL)>>24)&0xff;
						outBuffer[6] = (time(NULL)>>16)&0xff;
						outBuffer[7] = (time(NULL)>>8)&0xff;
						outBuffer[8] = (time(NULL)>>0)&0xff;
						outBuffer[9] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WHITE_CARD:
						tx_size = 40;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x24;
						outBuffer[4] = WIFI_CMD_GET_SYSTEM_TIME;
						outBuffer[5] = inputBuffer[9];
						outBuffer[6] = ((strlen((char*)ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]])>0)?0x00:0x01);
						memcpy(&outBuffer[7], ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]], ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]]));
						outBuffer[39] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_RFS:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_RFS;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_PRIMARY_CSU:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_PRIMARY_CSU;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.CsuPrimFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.CsuPrimFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_LCM:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_LCM;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.LcmFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.LcmFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_PRIMARY_PSU:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_PRIMARY_PSU;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.PsuPrimFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.PsuPrimFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_SECOND_PSU:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_SECOND_PSU;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.PsuSecFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.PsuSecFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_AUX_POWER:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_AUX_POWER;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.AuxPwrFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.AuxPwrFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_FAN:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_FAN;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.FanModuleFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.FanModuleFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_RELAY:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_RELAY;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.RelayModuleFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.RelayModuleFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_VER_TELECOMM:
						tx_size = 38;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x22;
						outBuffer[4] = WIFI_CMD_GET_VER_TELECOMM;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysInfo.TelcomModemFwRev, ARRAY_SIZE(ShmSysConfigAndInfo->SysInfo.TelcomModemFwRev));
						outBuffer[37] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_EVSE_CONFIG:
						tx_size = 14;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x09;
						outBuffer[4] = WIFI_CMD_GET_EVSE_CONFIG;
						outBuffer[5] = (ShmSysConfigAndInfo->SysConfig.MaxChargingEnergy>>0x08) & 0xff;
						outBuffer[6] = (ShmSysConfigAndInfo->SysConfig.MaxChargingEnergy>>0x00) & 0xff;
						outBuffer[7] = (ShmSysConfigAndInfo->SysConfig.MaxChargingPower>>0x08) & 0xff;
						outBuffer[8] = (ShmSysConfigAndInfo->SysConfig.MaxChargingPower>>0x00) & 0xff;
						outBuffer[9] = (ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent>>0x08) & 0xff;
						outBuffer[10] = (ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent>>0x00) & 0xff;
						outBuffer[11] = (ShmSysConfigAndInfo->SysConfig.MaxChargingDuration>>0x08) & 0xff;
						outBuffer[12] = (ShmSysConfigAndInfo->SysConfig.MaxChargingDuration>>0x00) & 0xff;
						outBuffer[13] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_GUN_STS:
						tx_size = 24;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x14;
						outBuffer[4] = WIFI_CMD_GET_GUN_STS;
						outBuffer[5] = inputBuffer[9];
						getConnectorInfo(inputBuffer[9], outBuffer);
						outBuffer[23] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_SCHEDULE:
						tx_size = 10;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x14;
						outBuffer[4] = WIFI_CMD_GET_SCHEDULE;
						outBuffer[5] = inputBuffer[9];
						getConnectorSchedule(inputBuffer[9], outBuffer);
						outBuffer[9] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_NETWORK_STS:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_NETWORK_STS;
						outBuffer[5] = ShmSysConfigAndInfo->SysInfo.InternetConn;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_ETH_DHCP:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_ETH_DHCP;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthDhcpClient^1;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_ETH_MAC:
						tx_size = 24;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x14;
						outBuffer[4] = WIFI_CMD_GET_ETH_MAC;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthMacAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthMacAddress));
						outBuffer[23] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_ETH_IP:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_ETH_IP;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_ETH_SUBMASK:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_ETH_SUBMASK;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthSubmaskAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthSubmaskAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_ETH_GATEWAY:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_ETH_GATEWAY;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthGatewayAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthGatewayAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_MODE:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x01;
						outBuffer[4] = WIFI_CMD_GET_WIFI_MODE;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_SSID:
						tx_size = 261;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x01;
						outBuffer[4] = WIFI_CMD_GET_WIFI_SSID;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid));
						outBuffer[260] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_PASSWD:
						tx_size = 261;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x01;
						outBuffer[4] = WIFI_CMD_GET_WIFI_PASSWD;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword));
						outBuffer[260] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_RSSI:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_WIFI_RSSI;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi+100;
						outBuffer[260] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_DHCP:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_ETH_DHCP;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient^1;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_MAC:
						tx_size = 24;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x14;
						outBuffer[4] = WIFI_CMD_GET_ETH_MAC;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress));
						outBuffer[24] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_IP:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_WIFI_IP;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_NETMASK:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_WIFI_NETMASK;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_WIFI_GATEWAY:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_WIFI_GATEWAY;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_APN:
						tx_size = 262;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x02;
						outBuffer[4] = WIFI_CMD_GET_TELE_APN;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomApn, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomApn));
						outBuffer[261] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_RSSI:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_TELE_RSSI;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomRssi+100;
						outBuffer[260] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_CHAAP_ID:
						tx_size = 262;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x02;
						outBuffer[4] = WIFI_CMD_GET_TELE_CHAAP_ID;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapId, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapId));
						outBuffer[261] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_CHAAP_PASSWD:
						tx_size = 262;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x01;
						outBuffer[3] = 0x02;
						outBuffer[4] = WIFI_CMD_GET_TELE_CHAAP_PASSWD;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapPwd, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapPwd));
						outBuffer[261] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_IMEI:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_TELE_IMEI;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomModemImei, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomModemImei));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_IMSI:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_TELE_IMSI;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSimImsi, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSimImsi));
						outBuffer[21] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_SIM_STS:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_TELE_SIM_STS;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSimStatus;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_MODE:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_GET_TELE_MODE;
						outBuffer[5] = ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomModemMode;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_GET_TELE_IP:
						tx_size = 22;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x12;
						outBuffer[4] = WIFI_CMD_GET_TELE_IP;
						memcpy(&outBuffer[5], ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomIpAddress, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomIpAddress));
						outBuffer[21] = chksumCal(outBuffer);
						break;


					case WIFI_CMD_SET_SYSTEM_TIME:
						rawtime = (inputBuffer[9]<<24) | (inputBuffer[10]<<16) | (inputBuffer[11]<<8) |(inputBuffer[12]<<0);
						ts = *localtime(&rawtime);

						sprintf((char*)cmdBuf, "date -u -s \"%04d-%02d-%02d %02d:%02d:%02d\"", (ts.tm_year+1900),
																							   (ts.tm_mon+1),
																							   	ts.tm_mday,
																						        ts.tm_hour,
																								ts.tm_min,
																						        ts.tm_sec);
						system((char*)cmdBuf);
						system("hwclock -w -u");
						system("hwclock -s");

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_SYSTEM_TIME;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WHITE_CARD:
						if((0<=inputBuffer[9]) && (inputBuffer[9]<10))
						{
							if(inputBuffer[10])
							{
								memcpy(ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]], &inputBuffer[11], ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]]));
							}
							else
							{
								memset(ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]], 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.LocalWhiteCard[inputBuffer[9]]));
							}
							outBuffer[6] = 0x01;
						}
						else
						{
							outBuffer[6] = 0x00;
						}

						tx_size = 8;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x04;
						outBuffer[4] = WIFI_CMD_SET_WHITE_CARD;
						outBuffer[5] = inputBuffer[9];
						outBuffer[7] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_FACTORY_RESET:
						ShmSysConfigAndInfo->SysInfo.FactoryConfiguration = ON;

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_FACTORY_RESET;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_EVSE_CONFIG:

						if(inputBuffer[8]&0x01)
						{
							ShmSysConfigAndInfo->SysConfig.MaxChargingEnergy = (inputBuffer[9]<<8) | (inputBuffer[10]<<0);
						}

						if((inputBuffer[8]>>1)&0x01)
						{
							ShmSysConfigAndInfo->SysConfig.MaxChargingPower = (inputBuffer[11]<<8) | (inputBuffer[12]<<0);
						}

						if((inputBuffer[8]>>2)&0x01)
						{
							ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent = (inputBuffer[13]<<8) | (inputBuffer[14]<<0);
						}

						if((inputBuffer[8]>>3)&0x01)
						{
							ShmSysConfigAndInfo->SysConfig.MaxChargingDuration = (inputBuffer[15]<<8) | (inputBuffer[16]<<0);
						}

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_EVSE_CONFIG;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_CHARGING_START:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_CHARGING_START;
						outBuffer[5] = (startConnectorCharging(inputBuffer[9])?0x01:0x00);
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_CHARGING_STOP:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_CHARGING_STOP;
						outBuffer[5] = (stopConnectorCharging(inputBuffer[9])?0x01:0x00);
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_SCHEDULE:
						setConnectorSchedule(inputBuffer[9], inputBuffer);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_SCHEDULE;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_ETH_CONFIG:
						ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthDhcpClient = inputBuffer[9]^1;

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_ETH_CONFIG;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_ETH_IP:
						memcpy(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthIpAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_ETH_IP;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_ETH_SUBMASK:
						memcpy(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthSubmaskAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_ETH_SUBMASK;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_ETH_GATEWAY:
						memcpy(ShmSysConfigAndInfo->SysConfig.Eth0Interface.EthGatewayAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_ETH_GATEWAY;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_MODE:
						ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode = inputBuffer[9];

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_MODE;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_SSID:
						memcpy(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid, &inputBuffer[9], 256);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_SSID;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_PASSWD:
						memcpy(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword, &inputBuffer[9], 256);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_PASSWD;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_DHCP:
						ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient = inputBuffer[9]^1;
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_DHCP;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_IP:
						memcpy(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_IP;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_NETMASK:
						memcpy(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_NETMASK;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_WIFI_GATEWAY:
						memcpy(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress, &inputBuffer[9], 16);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_WIFI_GATEWAY;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_TELE_APN:
						memcpy(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomApn, &inputBuffer[9], 256);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_TELE_APN;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_TELE_CHAAP_ID:
						memcpy(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapId, &inputBuffer[9], 256);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_TELE_CHAAP_ID;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_TELE_CHAAP_PASSWD:
						memcpy(ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomChapPapPwd, &inputBuffer[9], 256);

						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_TELE_CHAAP_PASSWD;
						outBuffer[5] = 0x01;
						outBuffer[6] = chksumCal(outBuffer);

						break;
					case WIFI_CMD_SET_CONFIRM:
						tx_size = 7;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x03;
						outBuffer[4] = WIFI_CMD_SET_CONFIRM;
						outBuffer[5] = (StoreUsrConfigData(&ShmSysConfigAndInfo->SysConfig)!=0x01?0x00:0x01);
						outBuffer[6] = chksumCal(outBuffer);

						break;
					default:
						tx_size = 6;
						outBuffer[0] = 0xff;
						outBuffer[1] = 0xff;
						outBuffer[2] = 0x00;
						outBuffer[3] = 0x02;
						outBuffer[4] = WIFI_CMD_UNKNOW;
						outBuffer[5] = chksumCal(outBuffer);

						break;
				}
			}
			else
			{
				tx_size = 6;
				outBuffer[0] = 0xff;
				outBuffer[1] = 0xff;
				outBuffer[2] = 0x00;
				outBuffer[3] = 0x02;
				outBuffer[4] = inputBuffer[8];
				outBuffer[5] = chksumCal(outBuffer);
			}

			send(clientSockFd, outBuffer, tx_size, 0);
		}

		if(read_size == 0)
		{
			DEBUG_INFO("Client disconnected.\r\n");
			fflush(stdout);
		}
		else if(read_size == -1)
		{
			DEBUG_ERROR("Socket recv failed.\r\n");
		}

		sleep(1);
	}

	return FAIL;
}

//==========================================
// Common routine
//==========================================
char* getTimeString(void)
{
	char *result=malloc(21);
	time_t timep;
	struct tm *p;
	time(&timep);
	p=gmtime(&timep);

	sprintf(result, "[%04d-%02d-%02d %02d:%02d:%02d]", (1900+p->tm_year), (1+p->tm_mon), p->tm_mday, p->tm_hour, p->tm_hour, p->tm_sec);

	return result;
}

void trim(char *s)
{
    int i=0, j, k, l=0;

    while((s[i]==' ')||(s[i]=='\t')||(s[i]=='\n'))
        i++;

    j = strlen(s)-1;
    while((s[j]==' ')||(s[j]=='\t')||(s[j]=='\n'))
        j--;

    if(i==0 && j==strlen(s)-1) { }
    else if(i==0) s[j+1] = '\0';
    else {
        for(k=i; k<=j; k++) s[l++] = s[k];
        s[l] = '\0';
    }
}

int mystrcmp(char *p1,char *p2)
{
    while(*p1==*p2)
    {
        if(*p1=='\0' || *p2=='\0')
            break;
        p1++;
        p2++;
    }
    if(*p1=='\0' && *p2=='\0')
        return(PASS);
    else
        return(FAIL);
}

void substr(char *dest, const char* src, unsigned int start, unsigned int cnt)
{
	strncpy(dest, src + start, cnt);
	dest[cnt] = 0;
}

void split(char **arr, char *str, const char *del)
{
	char *s = strtok(str, del);

	while(s != NULL)
	{
		*arr++ = s;
		s = strtok(NULL, del);
	}
}

int checkIP(void)
{
	// DHCP or static setting
	int isGetIP = FAIL;
	FILE *fp;
	char buf[512];
	char tmp[512];
	char cmd[512];
	if(Wifi.ipType == IP_TYPE_DHCP) {
		// Get IP address & net mask
		strcpy(cmd, "ifconfig ");
		strcat(cmd, Wifi.currentInterface);
		fp = popen(cmd, "r");
		if(fp == NULL)
			isGetIP = FAIL;
		else
		{
			while(fgets(buf, sizeof(buf), fp) != NULL)
			{
				if(strstr(buf, "addr:") > 0)
				{
					if(strstr(buf, "192.168.1.10") > 0)
					{
						DEBUG_INFO("IP cannot be same as default value of Ethernet...\n");
					}
					else
					{
						isGetIP = PASS;
						cnt_getIP_Fail = 0;
					}

					sscanf(buf, "%*s%s", tmp);
					substr(Wifi.currentIPAddr, tmp, strspn(tmp, "addr:"), strlen(buf)-strspn(tmp, "addr:"));

					sscanf(buf, "%*s%*s%*s%s", tmp);
					substr(Wifi.currentNetmask, tmp, strspn(tmp, "Mask:"), strlen(buf)-strspn(tmp, "Mask:"));
				}else {

				}
			}
		}

		if(isGetIP == FAIL && cnt_getIP_Fail<3) {
			sprintf(cmd, "pgrep -f \"udhcpc -i %s\" | xargs kill", Wifi.currentInterface);
			system(cmd);
			if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient==0)
			{
				sprintf(cmd, "/sbin/udhcpc -i %s -s /root/dhcp_script/wifi.script > /dev/null &", Wifi.currentInterface);
				system(cmd);
			}
			DEBUG_INFO("Sending DHCP request...\n");

			cnt_getIP_Fail++;
			sleep(5);
		}

		pclose(fp);

	}else {
		strcpy(cmd, "ifconfig ");
		strcat(cmd, Wifi.currentInterface);
		strcat(cmd, " ");
		strcat(cmd, Wifi.staticIPAddr);
		fp = popen(cmd, "r");

		DEBUG_INFO("Setting static IP to %s ...\n", Wifi.staticIPAddr);
		pclose(fp);
		isGetIP = PASS;
	}
	cnt_getIP_Fail = 0;
	return isGetIP;
}

int restartWPA(void)
{
	int result = FAIL;
	FILE *fp;
	char buf[512];
	char cmd[50];

	// Get IP address & net mask
	strcpy(cmd, "wpa_cli -i ");
	strcat(cmd, Wifi.currentInterface);
	strcat(cmd, " reconfigure");
	fp = popen(cmd, "r");
	DEBUG_INFO("WPA reconfigure %s ...\n", Wifi.currentInterface);

	if(fp == NULL)
		result = FAIL;
	else
	{
		while(fgets(buf, sizeof(buf), fp) != NULL)
		{
			if(strstr(buf, "OK") > 0)
			{
				DEBUG_INFO("reconfigure OK.\n");
				result = PASS;
			}else {
				DEBUG_INFO("reconfigure failed.\n");
				result = FAIL;
			}
		}
	}

	pclose(fp);

	if(result == PASS) {
		strcpy(cmd, "wpa_cli -i ");
		strcat(cmd, Wifi.currentInterface);
		strcat(cmd, " reconfigure");
		fp = popen(cmd, "r");
		DEBUG_INFO("WPA reconnect %s ...\n", Wifi.currentInterface);

		if(fp == NULL)
			result = FAIL;
		else{
			while(fgets(buf, sizeof(buf), fp) != NULL)
			{
				if(strstr(buf, "OK") > 0){
					DEBUG_INFO("reconnect OK.\n");
					result = PASS;
				}else {
					DEBUG_INFO("reconnect failed.\n");
					result = FAIL;
				}
			}
		}
		pclose(fp);
	}

	return result;
}

void proc_sta()
{
	switch(Wifi_module_sts)
	{
		case STA_STATE_INIT:
			dispReq.isShowed_IF_info = false;
			// get info from shared memory
			getParameters();

			ShmStatusCodeData->InfoCode.InfoEvents.bits.InternetDisconnectViaWiFi=1;
			ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=1;
			isSetWPA_OK = false;
			// check interface
			if((strlen((const char*)Wifi_A.ssid)>0) && (isFindInterface() == PASS))
			{
				DEBUG_INFO("Wifi interface: %s\n", Wifi.currentInterface);
				Wifi_module_sts = STA_STATE_DEVICE_DETECT;
				ShmStatusCodeData->FaultCode.FaultEvents.bits.WiFiModuleBroken=0;
				DEBUG_INFO("=================[State 1]===================\n");
			}
			else
			{
				ShmStatusCodeData->FaultCode.FaultEvents.bits.WiFiModuleBroken=1;
				DEBUG_ERROR("Wifi support interface valid result: Fail\n");
				sleep(30);
			}
			break;
		case STA_STATE_DEVICE_DETECT:
			dispReq.isShowed_IP_info = false;
			if(!isSetWPA_OK && (setWPAconf() == PASS))
			{
				isSetWPA_OK = true;
				ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=0;
				sleep(5);
			}
			else if((isSetWPA_OK == true) && (isConnectAP() == PASS))
			{
				Wifi_module_sts = STA_STATE_AP_CONNECTED;
				ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=0;
				DEBUG_INFO("=================[State 2]===================\n");
			}
			else
			{
				cnt_getAP_Fail++;
				DEBUG_INFO("Fail to connect to the AP %d times...\n",cnt_getAP_Fail);
				if(cnt_getAP_Fail>=3)
				{
					Wifi_module_sts = STA_STATE_INIT;
					cnt_getAP_Fail = 0;

					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=1;
					DEBUG_INFO("=================[State 0]===================\n");
				}
				else
				{
					sleep(10);
				}
			}

			break;
		case STA_STATE_AP_CONNECTED:
			getInterfaceInfo();
			getLinkQuality();
			DEBUG_INFO("Wifi quality: %d dBm\n", Wifi.rssi);
			ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = Wifi.rssi;

			if(isReachableInternet() == PASS)
			{
				DEBUG_INFO("Wifi internet valid result: Pass\n");
				ShmStatusCodeData->InfoCode.InfoEvents.bits.InternetDisconnectViaWiFi=0;
				ShmSysConfigAndInfo->SysConfig.AthInterface.WifiNetworkConn=1;
				cnt_pingDNS_Fail = 0;
				sleep(30);
			}
			else
			{
				cnt_pingDNS_Fail++;
				DEBUG_INFO("Wifi internet valid result: Fail %d time\n", cnt_pingDNS_Fail);
				sleep(5);
			}

			if(cnt_pingDNS_Fail >= 3)
			{
				ShmStatusCodeData->InfoCode.InfoEvents.bits.InternetDisconnectViaWiFi=1;
				ShmSysConfigAndInfo->SysConfig.AthInterface.WifiNetworkConn=0;
				cnt_pingDNS_Fail = 0;

				DEBUG_INFO("Ping DNS failed...");
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress);
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress);
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress);

				if(isFindInterface() == PASS)
				{
					DEBUG_INFO("Wifi interface: %s\n", Wifi.currentInterface);
					getParameters();
					isSetWPA_OK = false;
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = STA_STATE_DEVICE_DETECT;
					DEBUG_INFO("=================[State 1]===================\n");
				}
				else
				{
					DEBUG_INFO("Wifi support interface valid result: Fail\n");
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = STA_STATE_INIT;
					DEBUG_INFO("=================[State 0]===================\n");
				}
			}
			break;
		default:
			Wifi_module_sts = STA_STATE_INIT;
			break;
	}
}

void proc_ap()
{
	switch(Wifi_module_sts)
	{
		case AP_STATE_INIT:
			// get info from shared memory
			getParameters();
			isSetWPA_OK = false;
			ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=1;

			// check interface
			if(isFindInterface() == PASS)
			{
				DEBUG_INFO("Wifi interface: %s\n", Wifi.currentInterface);
				Wifi_module_sts = AP_STATE_DEVICE_DETECT;
				ShmStatusCodeData->FaultCode.FaultEvents.bits.WiFiModuleBroken=0;
				DEBUG_INFO("=================[State 1]===================\n");
			}
			else
			{
				ShmStatusCodeData->FaultCode.FaultEvents.bits.WiFiModuleBroken=1;
				DEBUG_ERROR("Wifi support interface valid result: Fail\n");
				sleep(30);
			}
			break;
		case AP_STATE_DEVICE_DETECT:
			if(!isSetWPA_OK && (setWPAconf() == PASS))
			{
				isSetWPA_OK = true;
				ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=0;
				sleep(5);
			}
			else if(isSetWPA_OK && (isStartUpAP() == PASS))
			{
				Wifi_module_sts = AP_STATE_AP_START;
				ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=0;
				DEBUG_INFO("=================[State 2]===================\n");
			}
			else
			{
				cnt_getAP_Fail++;
				DEBUG_INFO("Fail to start up AP %d times...\n",cnt_getAP_Fail);
				if(cnt_getAP_Fail>=3)
				{
					Wifi_module_sts = AP_STATE_INIT;
					cnt_getAP_Fail = 0;

					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=1;
					DEBUG_INFO("=================[State 0]===================\n");
				}
				else
				{
					sleep(10);
				}
			}
			break;
		case AP_STATE_AP_START:
			getInterfaceInfo();

			if(isStartUpAP() == PASS)
			{
				DEBUG_INFO("Wifi AP start up valid result: Pass\n");
				ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=0;
				cnt_pingDNS_Fail = 0;
				sleep(30);
			}
			else
			{
				cnt_pingDNS_Fail++;
				DEBUG_INFO("Wifi AP start up valid result: Fail %d time\n", cnt_pingDNS_Fail);
				sleep(5);
			}

			if(cnt_pingDNS_Fail >= 3)
			{
				ShmStatusCodeData->InfoCode.InfoEvents.bits.ApDisconnectViaWiFi=1;
				cnt_pingDNS_Fail = 0;

				DEBUG_INFO("Wifi AP start up status failed...");
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress);
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress);
				memset(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress, 0, sizeof ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMacAddress);

				if(isFindInterface() == PASS)
				{
					DEBUG_INFO("Wifi interface: %s\n", Wifi.currentInterface);
					isSetWPA_OK = false;
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = STA_STATE_DEVICE_DETECT;
					DEBUG_INFO("=================[State 1]===================\n");
				}
				else
				{
					DEBUG_INFO("Wifi support interface valid result: Fail\n");
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = STA_STATE_INIT;
					DEBUG_INFO("=================[State 0]===================\n");
				}
			}
			break;
		default:
			Wifi_module_sts = AP_STATE_INIT;
			break;
	}
}

//==========================================
// Main loop
//==========================================
int main(void)
{
	pid_t pid;

	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("InitShareMemory NG\n");
		#endif
		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory=ON;
		}
		sleep(5);
		return 0;
	}

	// UDP socket server start
	pid = fork();
	if(pid == 0)
	{
		if(udpSocketServerStart() == FAIL)
		{
			DEBUG_ERROR("UDP socket server down.\r\n");
			return 0;
		}
	}

	// TCP socket server start
	pid = fork();
	if(pid == 0)
	{
		if(tcpSocketServerStart() == FAIL)
		{
			DEBUG_ERROR("TCP socket server down.\r\n");
			return 0;
		}
	}

	//=============================================
	// Install WIFI module driver
	//=============================================
	#ifdef UBLOX
		system("insmod /lib/modules/mlan.ko");
		system("insmod /lib/modules/usb8801.ko");
		sleep(5);
		system("ifconfig mlan0 up");
		system("ifconfig uap0 up");
	#endif

	#ifdef MT7601U
		system("insmod /lib/modules/mt7601u.ko");
		sleep(5);
		system("ifconfig wlan0 up");
	#endif


	DEBUG_INFO("=================[State 0]===================\n");
	for(;;)
	{
		switch(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode)
		{
			case WIFI_MODE_STA:
				proc_sta();
				break;
			case WIFI_MODE_AP:
				proc_ap();
				break;
			case WIFI_MODE_ADHOC:
				break;
			case WIFI_MODE_DISABLE:
			default:
				break;
		}
	}

	return 0;
}