csu3_am335x/EVSE/Modularization/Module_Wifi.c

/*
 * Module_Wifi.c
 *
 *  Created on: 2020-01-14
 *      Author: Jerry Wang
 *     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 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				8857
#define LISTEN_PORT_TCP				8856

#define	CONNECTION_LIMIT			1
#define PROTOCOL_ADDR				0

#define UBLOX
//#define MT7601U

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;


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

    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
			{
				strcpy(cmdBuf, "ifconfig ");
				strcat(cmdBuf, Wifi.currentInterface);
				strcat(cmdBuf, " ");
				strcat(cmdBuf, Wifi.staticIPAddr);
				system(cmdBuf);

				DEBUG_INFO("Setting %s static IP to %s ...\n", Wifi.currentInterface, Wifi.staticIPAddr);
			}
			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 >> /etc/hostapd.conf", Wifi_A.ssid);
                        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 >> /etc/hostapd.conf", Wifi_A.passwd);
                        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 udpSocketServerStart()
{
	 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;

	 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));
			 DEBUG_INFO("Data: %s\r\n", inputBuffer);

			 /*
			  * TODO:
			  *   - UDP broadcast protocol process here
			  */
			 memcpy(outBuffer, inputBuffer, read_size);
			 tx_size = read_size;

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

	return FAIL;
}

//==========================================
// TCP socket server routine
//==========================================
int tcpSocketServerStart()
{
	int 				sockFd = 0;
	int 				clientSockFd = 0;
	uint8_t 			inputBuffer[2048] = {};
	uint8_t 			outBuffer[2048] = {};
	uint8_t				chksum;
	int16_t 			read_size;
	int16_t				tx_size;
	struct sockaddr_in 	serverInfo, clientInfo;
	socklen_t 			addrlen = sizeof(clientInfo);

	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))
			{
				chksum = 0;
				memset(outBuffer, 0x00, sizeof(outBuffer));
				/*
				 * TODO:
				 *   - TCP broadcast protocol process here
				 */

				switch(inputBuffer[3])
				{
					case 0:
						tx_size = 9;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = 0x00;
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x01;
						outBuffer[7] = 0x01;
						break;
					default:
						tx_size = 9;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = inputBuffer[3];
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x00;
						outBuffer[7] = 0x00;
						break;
				}
			}
			else
			{
				tx_size = 9;
				outBuffer[0] = 0xaa;
				outBuffer[1] = PROTOCOL_ADDR;
				outBuffer[2] = inputBuffer[1];
				outBuffer[3] = inputBuffer[3];
				outBuffer[4] = 0x01;
				outBuffer[5] = 0x00;
				outBuffer[6] = 0x00;
				outBuffer[7] = chksum;
			}

			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 isValidCheckSum(uint8_t *message)
{
	uint8_t	chksum=0;

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

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

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);
					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((strlen((const char*)Wifi_A.ssid)>0) && (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=1;
		}
		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;
}