csu3_am335x/EVSE/Modularization/Module_Wifi.c

#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	"../Projects/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 Debug
#define ARRAY_SIZE(A)		(sizeof(A) / sizeof(A[0]))
#define PASS				1
#define FAIL				-1
#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 Initializing          0
#define Wifi_device_detected  1
#define Wifi_AP_connected     2
#define Internet_checking     3

#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 auth_info
{
	int auth_type;
	char *ssid;
	char *passwd;
};*/

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 setWPApara();
int restartWPA();
void refreshUSB();
void getParameters();

int Wifi_module_sts = 0;

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

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_%s_%s_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,
			ShmSysConfigAndInfo->SysConfig.ModelName,
			ShmSysConfigAndInfo->SysConfig.SerialNumber);
#ifdef SystemLogMessage
	system(Buf);
#endif

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

	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)
{
	strcpy(Wifi_A.ssid, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid);
	DEBUG_INFO("Wifi SSID: %s\n", Wifi_A.ssid);
	strcpy(Wifi_A.passwd, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword);
	DEBUG_INFO("Wifi Password: %s\n", Wifi_A.passwd);
	if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress != NULL)
		strcpy(Wifi.staticIPAddr, 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);
}

//==========================================
// 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);
				result = PASS;
			}

			if(strstr(buf, "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:"));

				result = PASS;
			}
		}
	}
	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);

	return result;
}

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

	// Get IP address & net mask
	strcpy(cmd, "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, "unassociated") > 0)
			{
				result = FAIL;
			}
		}
	}
	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;
}

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

	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, "Quality") > 0)
			{
				sscanf(buf, "%*s%s", tmp);
				substr(tmp, tmp, strspn(tmp, "Quality="), strlen(buf)-strspn(tmp, "Quality="));

				split(q,tmp,"/");
				Wifi.rssi = (atoi(q[0])*100)/atoi(q[1]);

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

	return result;
}

//==========================================
// WPA parameter set
//==========================================
int setWPAconf()
{
	int result = FAIL;
	char cmdBuf[512];

	/*system("cat /dev/null > /etc/wpa.conf");
	system("echo ctrl_interface_group=0 >> /etc/wpa.conf");
	system("echo ctrl_interface=/var/run/wpa_supplicant >> /etc/wpa.conf");
	system("echo ap_scan=1 >> /etc/wpa.conf");
	system("echo update_config=1 >> /etc/wpa.conf");

	system("echo network={ >> /etc/wpa.conf");
	sprintf(cmdBuf, "echo ssid=\\\"%s\\\" >> /etc/wpa.conf", ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSsid);
	system(cmdBuf);
	sprintf(cmdBuf, "echo psk=\\\"%s\\\" >> /etc/wpa.conf", ShmSysConfigAndInfo->SysConfig.AthInterface.WifiPassword);
	system(cmdBuf);
	system("echo scan_ssid=1 >> /etc/wpa.conf");
	system("echo pairwise=CCMP TKIP >> /etc/wpa.conf");
	system("echo key_mgmt=WPA-PSK >> /etc/wpa.conf");

	switch(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiMode)
	{
		case WIFI_MODE_STA:
			system("echo mode=0 >> /etc/wpa.conf");
			break;
		case WIFI_MODE_AP:
			system("echo mode=2 >> /etc/wpa.conf");
			break;
		case WIFI_MODE_ADHOC:
			system("echo mode=1 >> /etc/wpa.conf");

			break;
		case WIFI_MODE_DISABLE:
		default:
			break;

	}
	system("echo } >> /etc/wpa.conf");*/

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

	if(strlen(Wifi_A.passwd)>0) {
		if(strlen(Wifi_A.passwd)>=8) {
			strcat(buffer, "network={\n    ssid=\"");
			strcat(buffer, Wifi_A.ssid);
			strcat(buffer, "\"\n    key_mgmt=WPA-EAP WPA-PSK \n");
			strcat(buffer, "    psk=\"");
			strcat(buffer, Wifi_A.passwd);
			strcat(buffer, "\"\n}\n\n");
		}

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

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


	pFile = fopen("/etc/wpa.conf","w");

	fwrite(buffer,strlen(buffer),1,pFile);
	fclose(pFile);
	/*if(restartWPA() == PASS && checkAP() == PASS) {
		result = PASS;
	}*/

	sprintf(cmdBuf, "pgrep -f \"wpa_supplicant -i %s\" | xargs kill", Wifi.currentInterface);
	system(cmdBuf);

	sprintf(cmdBuf, "/root/wpa_supplicant -i %s -c /etc/wpa.conf -B", Wifi.currentInterface);
	system(cmdBuf);

	sleep(5);

	sprintf(cmdBuf, "pgrep -f \"udhcpc -i %s\" | xargs kill", Wifi.currentInterface);
	system(cmdBuf);
	if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient==0)
	{
		sprintf(cmdBuf, "/sbin/udhcpc -i %s -s /root/simple.script > /dev/null &", Wifi.currentInterface);
		system(cmdBuf);
	}

	result = PASS;
	return result;
}

//==========================================
// WPA parameter set
//==========================================
int setWPApara()
{
	int result = FAIL;
	char cmdBuf[512];

	DEBUG_INFO("Taregt SSID: %s\n", Wifi.currentSSID);
	DEBUG_INFO("Taregt password: %s\n", Wifi.currentPasswd);

	sprintf(cmdBuf, "wpa_cli -i %s remove_network 0 > /dev/null 2>&1", Wifi.currentInterface);
	DEBUG_INFO("%s\n", cmdBuf);
	system(cmdBuf);

	sprintf(cmdBuf, "wpa_cli -i %s add_network 0 > /dev/null 2>&1", Wifi.currentInterface);
	DEBUG_INFO("%s\n", cmdBuf);
	system(cmdBuf);

	sprintf(cmdBuf, "wpa_cli -i %s set_network 0 ssid '\"%s\"' > /dev/null 2>&1", Wifi.currentInterface, Wifi.currentSSID);
	DEBUG_INFO("%s\n", cmdBuf);
	system(cmdBuf);

	switch(Wifi.authType)
	{
		case AUTH_TYPE_NONE:
			DEBUG_INFO("Wifi auth type is NONE.\n");

			sprintf(cmdBuf, "wpa_cli -i %s set_network 0 key_mgmt NONE > /dev/null 2>&1", Wifi.currentInterface);
			DEBUG_INFO("%s\n", cmdBuf);
			system(cmdBuf);

			break;
		case AUTH_TYPE_WEP:
			DEBUG_INFO("Wifi auth type is WEP.\n");

			sprintf(cmdBuf, "wpa_cli -i %s set_network 0 key_mgmt NONE > /dev/null 2>&1", Wifi.currentInterface);
			DEBUG_INFO("%s\n", cmdBuf);
			system(cmdBuf);

			sprintf(cmdBuf, "wpa_cli -i %s set_network 0 wep_key0 '\"%s\"' > /dev/null 2>&1", Wifi.currentInterface, Wifi.currentPasswd);
			DEBUG_INFO("%s\n", cmdBuf);
			system(cmdBuf);

			break;
		case AUTH_TYPE_WPAPSK:
			DEBUG_INFO("Wifi auth type is WPAPSK.\n");

			sprintf(cmdBuf, "wpa_cli -i %s set_network 0 psk '\"%s\"' > /dev/null 2>&1", Wifi.currentInterface, Wifi.currentPasswd);
			DEBUG_INFO("%s\n", cmdBuf);
			system(cmdBuf);

			break;
		case AUTH_TYPE_WPA2PSK:
			DEBUG_INFO("Wifi auth type is WPA2PSK.\n");
			break;
		default:
			DEBUG_INFO("Wifi auth type can not be identify and switch to default NONE.\n");

			sprintf(cmdBuf, "wpa_cli -i %s set_network 0 key_mgmt NONE > /dev/null 2>&1", Wifi.currentInterface);
			DEBUG_INFO("%s\n", cmdBuf);
			system(cmdBuf);

			break;
	}

	sprintf(cmdBuf, "wpa_cli -i %s enable_network 0 > /dev/null 2>&1", Wifi.currentInterface);
	DEBUG_INFO("%s\n", cmdBuf);
	system(cmdBuf);


	if(ShmSysConfigAndInfo->SysConfig.AthInterface.WifiDhcpClient != 1)
	{
		// IP obtain method is configuration DHCP
		sprintf(cmdBuf, "dhclient %s > /dev/null 2>&1", Wifi.currentInterface);
		DEBUG_INFO("%s\n", cmdBuf);
		system(cmdBuf);
	}
	else
	{
		// IP obtain method is configuration static
		sprintf(cmdBuf, "ifconfig %s %s netmask %s > /dev/null 2>&1", Wifi.currentInterface, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiIpAddress, ShmSysConfigAndInfo->SysConfig.AthInterface.WifiSubmaskAddress);
		DEBUG_INFO("%s\n", cmdBuf);
		system(cmdBuf);

		sprintf(cmdBuf, "route add default gw %s > /dev/null 2>&1", ShmSysConfigAndInfo->SysConfig.AthInterface.WifiGatewayAddress);
		DEBUG_INFO("%s\n", cmdBuf);
		system(cmdBuf);
	}


	result = PASS;
	return result;
}


/*
int setWPApara()
{
	int result = FAIL;

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

	if(strlen(Wifi_A.passwd)>0) {
		strcat(buffer, "network={\n    ssid=\"");
		strcat(buffer, Wifi_A.ssid);
		strcat(buffer, "\"\n    key_mgmt=WPA-EAP WPA-PSK \n");
		strcat(buffer, "    psk=\"");
		strcat(buffer, Wifi_A.passwd);
		strcat(buffer, "\"\n}\n\n");

		strcat(buffer, "network={\n    ssid=\"");
		strcat(buffer, Wifi_A.ssid);
		strcat(buffer, "\"\n    key_mgmt=NONE\n");
		strcat(buffer, "    wep_key0=\"");
		strcat(buffer, Wifi_A.passwd);
		strcat(buffer, "\"\n}\n\n");

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


	pFile = fopen("/etc/wpa_supplicant/wpa_supplicant.conf","w");

	fwrite(buffer,strlen(buffer),1,pFile);
	fclose(pFile);
	if(restartWPA() == PASS && checkAP() == PASS) {
		result = PASS;
	}

	return result;
}*/

//=================================
// 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[50];
	if(Wifi.ipType == IP_TYPE_DHCP) {
		while(isGetIP == FAIL && cnt_getIP_Fail<=3) {
			// 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)
					{
						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:"));

						isGetIP = PASS;
						cnt_getIP_Fail = 0;
					}else {

					}
				}
			}

			if(isGetIP == FAIL && cnt_getIP_Fail<3) {
				/*strcpy(cmd, "dhclient ");
				strcat(cmd, Wifi.currentInterface);
				fp = popen(cmd, "r");
				DEBUG_INFO("Sending DHCP request...\n");

				pclose(fp);*/
				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/simple.script > /dev/null &", Wifi.currentInterface);
					system(cmd);
				}
				DEBUG_INFO("Sending DHCP request...\n");

				cnt_getIP_Fail++;
				sleep(5);
			}
		}

		if(cnt_getIP_Fail>=3) {
			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);
			cnt_getIP_Fail = 0;
			isGetIP = PASS;
		}else {
			isGetIP = PASS;
		}

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

int main(void)
{
	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("InitShareMemory NG\n");
		#endif
		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory=1;
		}
		sleep(5);
		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");
	#endif

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

	ShmStatusCodeData->InfoCode.InfoEvents.bits.InternetDisconnectViaWiFi=1;
	DEBUG_INFO("=================[State 0]===================\n");


	for(;;)
	{
		switch(Wifi_module_sts) {
		case Initializing:
			// get info from shared memory
			getParameters();

			// check interface
			if( strlen(Wifi_A.ssid)>0 && isFindInterface() == PASS){
				DEBUG_INFO("Wifi interface: %s\n", Wifi.currentInterface);
				Wifi_module_sts = Wifi_device_detected;
				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 Wifi_device_detected:
			if(setWPAconf() == PASS) {
				Wifi_module_sts = Wifi_AP_connected;
				ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=0;
				DEBUG_INFO("=================[State 2]===================\n");
				sleep(10);
			}else {
				cnt_getAP_Fail++;
				if(cnt_getAP_Fail>=3) {
					Wifi_module_sts = Initializing;
					cnt_getAP_Fail = 0;

					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.WiFiModuleCommFail=1;
					DEBUG_INFO("=================[State 0]===================\n");
				}else {
					sleep(30);
				}
			}
			break;
		case Wifi_AP_connected:
			checkIP();
			getInterfaceInfo();
			DEBUG_INFO("IP address: %s\n", Wifi.currentIPAddr);
			DEBUG_INFO("Net mask: %s\n", Wifi.currentNetmask);
			DEBUG_INFO("Default gateway: %s\n", Wifi.currentGateway);

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

			Wifi_module_sts = Internet_checking;
			DEBUG_INFO("=================[State 3]===================\n");

			break;
		case Internet_checking:
			getLinkQuality();
			DEBUG_INFO("Wifi quality: %d\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;
				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;
				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);
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = Wifi_device_detected;
					DEBUG_INFO("=================[State 1]===================\n");
				}else {
					DEBUG_INFO("Wifi support interface valid result: Fail\n");
					ShmSysConfigAndInfo->SysConfig.AthInterface.WifiRssi = 0;
					Wifi_module_sts = Initializing;
					DEBUG_INFO("=================[State 0]===================\n");
				}
			}
			break;
		}
	}

	return 0;
}