
#include "p2p_test.h"

unsigned int wps_pin_checksum(unsigned int pin)
{
	unsigned int accum = 0;
	while( pin )
	{
		accum += pin % 10;
		pin /= 10;
		accum += 3 * (pin % 10);
		pin /= 10;
	}
	return( accum % 10 );
}

void p2p_enable(struct p2p *p)
{
	if(p->enable == -1 )
	{
		if(p->wpa_open == 1){
			p->wpa_open = 0;
			system("killall wpa_supplicant");
#ifdef DHCP
			system("killall dhclient");
#endif			
		}
		if(p->ap_open ==1){
			p->ap_open = 0;
			system("killall hostapd");
#ifdef DHCP
			system("killall dhcpd");
#endif
		}

		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "iwpriv %s p2p_set enable=0", p->ifname);
		system( p->cmd );
		
		return;
	}
	
	p->p2p_get=0;
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set enable=%d", p->ifname, p->enable);
	system( p->cmd );
	
	if( p->enable == P2P_ROLE_DISABLE )
	{
		p->have_p2p_dev = 0;
		p->wps_info = 0;
		p->pin = 12345670;

		if(p->res == 0)
		{
			pthread_cancel(p->pthread);
			p->res = 1;
		}

		if(p->res_go == 0)
		{
			pthread_cancel(p->pthread_go);
			p->res_go = 1;
		}
				
		p->wpa_open = 0;
		system("killall wpa_supplicant");
#ifdef DHCP
		system("killall dhclient");
#endif
		system("clear");
		p->ap_open = 0;
		system("killall hostapd");
#ifdef DHCP
		system("killall dhcpd");
#endif
		system("clear");
		
	}
	else if( p->enable == P2P_ROLE_DEVICE )
	{
		char msg[5] = "NULL";

#ifdef P2P_AUTO
		p->res = pthread_create(&p->pthread, NULL, &polling_status, (void *)p);
#endif
		
		if( p->res !=0 )
		{
			p->p2p_get=1;
			memset( p->print_line, 0x00, CMD_SZ );
			sprintf( p->print_line, "Thread creation failed" );
		}
		
		if(p->wpa_open == 1){
			p->wpa_open = 0;
			system("killall wpa_supplicant");
#ifdef DHCP
			system("killall dhclient");
#endif
		}
		if(p->ap_open ==1){
			p->ap_open = 0;
			system("killall hostapd");
#ifdef DHCP
			system("killall dhcpd");
#endif
		}
		
		p2p_intent(p);
		
		p2p_set_opch(p, NULL, 0);
		usleep(50000);
		p2p_softap_ssid(p, NULL, 0);

		p2p_setDN(p);

		p2p_scan( p );
		
	}
	else if( p->enable == P2P_ROLE_CLIENT )
	{
		if(p->ap_open ==1){
			p->ap_open = 0;
			system("killall hostapd");
#ifdef DHCP
			system("killall dhcpd");
#endif
		}
		
		p2p_status(p, 0);
		p2p_role(p, 0);
		p2p_intent(p);
	}
	else if( p->enable == P2P_ROLE_GO )
	{
		if(p->wpa_open == 1){
			p->wpa_open = 0;
			system("killall wpa_supplicant");
#ifdef DHCP
			system("killall dhclient");
#endif
		}

		p2p_status(p, 0);
		p2p_role(p, 0);
		p2p_intent(p);
		
		p2p_set_opch(p, NULL, 0);
		usleep(50000);
		p2p_softap_ssid(p, NULL, 0);

		if(p->ap_open != 1)
		{
			memset( p->cmd, 0x00, CMD_SZ );
			sprintf( p->cmd, "%s/hostapd -B %s > temp.txt",p->ap_path, p->ap_conf);
			system( p->cmd );
	
			p->ap_open = 1;
		}

#ifdef P2P_AUTO
		p->res_go = pthread_create(&p->pthread_go, NULL, &polling_client, (void *)p);
#endif

		if( p->res_go != 0 )
		{
			p->p2p_get=1;
			memset( p->print_line, 0x00, CMD_SZ );
			sprintf( p->print_line, "Thread creation failed" );
		}
		
	}

}

void p2p_scan(struct p2p *p)
{
	p->p2p_get=0;
	if( p->enable >= P2P_ROLE_DEVICE )
	{
		p->have_p2p_dev=1;
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "iwlist %s scan > scan.txt", p->ifname );
		system( p->cmd );

		p2p_status(p, 0);
		
	}
	else
	{
		p->p2p_get=1;
		memset( p->print_line, 0x00, CMD_SZ );
		sprintf( p->print_line, "%s", p->scan_msg );
	}
}

void scan_result(struct p2p *p)
{
	FILE *pf=NULL;
	
	pf = fopen( "./scan.txt", "r" );
	if ( pf )
	{
		int no_dev=0;
		char cms[30] = { 0x00 };
		char dns[SSID_SZ] = { 0x00 };
		char parse[100] = { 0x00 };
		
		p->count_line=0;
		while( (!feof( pf )) && (no_dev <= SCAN_POOL_NO)){
			memset( parse, 0x00, CMD_SZ );
			fgets( parse, CMD_SZ, pf );

			if(parse[0] == '\n' || parse[0] == '\0')
				break;
			if( strncmp(parse+10, "Scan completed :", 16) == 0 )
			{
				printf("* NO   DEVICE NAME                BSSID                 GO    CONFIG METHOD                        *\n");
				p->count_line++;
			}
			else if( strncmp(parse+20, "Address:", 8) == 0 )
			{
				struct scan *pscan_pool = &p->scan_pool[no_dev];
				memset( pscan_pool->addr, 0x00, CMD_SZ );
				strncpy( pscan_pool->addr, parse+29, 17);
			}
			else if( strncmp(parse+20, "ESSID:", 6) == 0 )
			{
				struct scan *pscan_pool = &p->scan_pool[no_dev];

				p2p_wps_cm(p, pscan_pool->addr, cms);
				p2p_device_name(p, pscan_pool->addr, dns);
				
				if( strncmp(parse+26, "\"DIRECT-\"", 9) == 0 )
				{
					printf("*[%02d]  %-25s  %s          %-38s*\n",no_dev+1, dns, pscan_pool->addr, cms);	
				}
				else
				{
					printf("*[%02d]  %-25s  %s      *   %-38s*\n",no_dev+1, dns, pscan_pool->addr, cms);	
					pscan_pool->go = 1;
				}
				p->count_line++;
				no_dev++;
				
			}
		}
		if( p->count_line < (SCAN_POOL_NO + 1) )
		{
			for(p->count_line; p->count_line < SCAN_POOL_NO+1; p->count_line++ )
				printf("*                                                                                                  *\n");
		}
		fclose( pf );
	}

}

void p2p_setDN(struct p2p *p)
{

	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set setDN=%s", p->ifname, p->dev_name);
	system( p->cmd );
}

void p2p_intent(struct p2p *p)
{
	p->p2p_get=0;
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set intent=%d", p->ifname, p->intent);
	system( p->cmd );
}

void p2p_wpsinfo(struct p2p *p)
{
	p->p2p_get=0;
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set got_wpsinfo=%d", p->ifname, p->wps_info);
	system( p->cmd );
}

void p2p_pincode(struct p2p *p, char *ins_no, char *ins_no_again)
{
	int pin_check=0;
	p->p2p_get=0;
	ui_screen(p);
	printf("%s", ins_no);
	scanf("%d",&pin_check);
	while( wps_pin_checksum(pin_check) != 0 )
	{
		ui_screen(p);
		printf("%s", ins_no_again);	
		scanf("%d",&pin_check);
	}
	p->pin = pin_check;
}

void p2p_devaddr(struct p2p *p)
{
	int c;
	struct scan *pscan_pool;

	p->p2p_get=0;
	
	scanf("%d", &c);
	pscan_pool = &p->scan_pool[c-1];
	strncpy(p->peer_devaddr, pscan_pool->addr, 17);

	if( pscan_pool->go == 1)
		p->connect_go = 1;

}

void p2p_role(struct p2p *p, int flag)
{
	FILE *pf=NULL;
			
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get role > status.txt", p->ifname);
	system( p->cmd );

	pf = fopen( "./status.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( p->parse, 0x00, CMD_SZ );
			fgets( p->parse, CMD_SZ, pf );
			if( strncmp( p->parse, "Role", 4) == 0 )
			{
				p->role = atoi( &p->parse[ 5 ] );
				if(flag==1){
					p->p2p_get=1;
					sprintf( p->print_line, "Role=%s", naming_role(p->role));
				}
				break;
			}	
		}
		fclose( pf );
	}
}

void p2p_status(struct p2p *p, int flag)
{
	FILE *pf=NULL;
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get status > status.txt", p->ifname);
	system( p->cmd );

	pf = fopen( "./status.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( p->parse, 0x00, CMD_SZ );
			fgets( p->parse, CMD_SZ, pf );
			if( strncmp( p->parse, "Status", 6) == 0 )
			{
				p->status = atoi( &p->parse[ 7 ] );		
				if(flag==1){
					p->p2p_get=1;
					sprintf( p->print_line, "Status=%s", naming_status(p->status));
				}
				break;
			}	
		}
		fclose( pf );
	}
}

void p2p_prov_disc_no_addr(struct p2p *p, char *msg)
{
	p->p2p_get=1;
	memset( p->print_line, 0x00, CMD_SZ );
	sprintf( p->print_line, "%s", msg);
}

#ifdef P2P_AUTO
void p2p_prov_disc(struct p2p *p, char *msg, char *dis_msg, char *label_msg)
{
	int wps_cm, retry_count=0;
	char prov[100] = { 0x00 };
	
	p->p2p_get=0;
	ui_screen(p);
	printf("%s", msg);
	scanf("%d",&wps_cm);

	if(p->res == 0)
	{
		pthread_cancel(p->pthread);
		p->res = 1;
	}

	memset( p->cmd, 0x00, CMD_SZ );	
	if( wps_cm == 0 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_display", p->ifname, p->peer_devaddr);
	else if( wps_cm == 1 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_keypad", p->ifname, p->peer_devaddr);
	else if( wps_cm == 2 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_pbc", p->ifname, p->peer_devaddr);
	else if( wps_cm == 3 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_label", p->ifname, p->peer_devaddr);	
	system( p->cmd );
	strcpy( prov, p->cmd );

	usleep(500000);
	p2p_status( p, 0 );
	
	while(p->status != P2P_STATE_RX_PROVISION_DIS_RSP && retry_count < MAX_PROV_RETRY)
	{
		usleep( PROV_WAIT_TIME );
		retry_count++;
		p2p_status( p, 0 );
		if( (retry_count % PROV_RETRY_INTERVAL) == 0)
			system( prov );
	}
	
	if( p->status == P2P_STATE_RX_PROVISION_DIS_RSP )
	{
		switch(wps_cm)
		{
			case 0:	p->wps_info=1;	break;
			case 1: p->wps_info=2;	break;
			case 2: p->wps_info=3;	break;
			case 3: p->wps_info=1;	break;
		}
		
		if( wps_cm==1 || wps_cm==2 )
		{
			p2p_wpsinfo(p);
			
			if(p->connect_go == 1)
				p2p_client_mode(p);
			else
				p2p_set_nego(p);
		}
		else if( wps_cm==0 || wps_cm==3 )
		{
			ui_screen(p);
			if( wps_cm ==0 )
				printf("%s", dis_msg);	
			else if( wps_cm == 3 )
				printf("%s", label_msg);
			scanf("%d",&p->pin);

			p2p_wpsinfo(p);	

			if(p->connect_go == 1)
				p2p_client_mode(p);
			else
				p2p_set_nego(p);
				
		}
	}
	else
	{
		p->p2p_get = 1;
		memset( p->print_line, 0x00, CMD_SZ );
		sprintf( p->print_line, "Issue provision discovery fail" );
		ui_screen(p);	

#ifdef P2P_AUTO		
		pthread_create(&p->pthread, NULL, &polling_status, (void *)p);
#endif
		
	}

}
#else

// This mode is without the following procedures:
// 1.set config method
// 2.start group negotiation
// 3.start wpa_supplicant or hostapd
void p2p_prov_disc(struct p2p *p, char *msg, char *dis_msg, char *label_msg)
{
	int wps_cm;
	p->p2p_get=0;
	ui_screen(p);
	printf("%s", msg);
	scanf("%d",&wps_cm);
	memset( p->cmd, 0x00, CMD_SZ );

	
	if( wps_cm == 0 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_display", p->ifname, p->peer_devaddr);
	else if( wps_cm == 1 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_keypad", p->ifname, p->peer_devaddr);
	else if( wps_cm == 2 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_pbc", p->ifname, p->peer_devaddr);
	else if( wps_cm == 3 )
		sprintf( p->cmd, "iwpriv %s p2p_set prov_disc=%s_label", p->ifname, p->peer_devaddr);	
	system( p->cmd );

}
#endif

int p2p_set_nego(struct p2p *p)
{
	FILE *pf=NULL;
	int retry_count = 0, success = 0;

	p->p2p_get=1;
	memset( p->print_line, 0x00, CMD_SZ );
	strcpy( p->print_line, p->nego_msg);
	ui_screen(p);
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set nego=%s ", p->ifname, p->peer_devaddr);
	system( p->cmd );

	usleep( PRE_NEGO_INTERVAL );

	do
	{

		p2p_status(p, 0);
		p2p_role(p, 0);
		
		if( p->status != P2P_STATE_GONEGO_OK ){
			
			if( p->role == P2P_ROLE_CLIENT || p->role == P2P_ROLE_GO )
				break;
			
			if( retry_count > 0)
			{
				int check_status = 0;
				
				while(check_status < (NEGO_RETRY_INTERVAL/500000) && p->status != P2P_STATE_GONEGO_OK )
				{
					p2p_status(p, 1);
					if( check_status == 0 )
						ui_screen(p);

					if( p->status == P2P_STATE_GONEGO_OK )
					{
						success = 1;
						break;
					}
					
					usleep(500000);
					check_status++;
				}
				
				//usleep( NEGO_RETRY_INTERVAL );
			}

			if( success ==0 ){	//need to re-fine
				memset( p->cmd, 0x00, CMD_SZ );
				sprintf( p->cmd, "iwpriv %s p2p_set nego=%s ", p->ifname, p->peer_devaddr);
				system( p->cmd );
	
				usleep(10000);
				retry_count++;
			}
		}
		else
		{
			p->p2p_get = 1;
			memset( p->print_line, 0x00, CMD_SZ );
			sprintf( p->print_line, "%s", p->ok_msg );
			p2p_ifaddr(p);
			ui_screen(p);
			break;
		}
			
	}
	while( p->status != P2P_STATE_GONEGO_OK && retry_count < MAX_NEGO_RETRY );

	if( p->status == P2P_STATE_GONEGO_OK || p->role == P2P_ROLE_CLIENT || p->role == P2P_ROLE_GO )
	{
		p2p_role(p ,0);
		
		if( p->role == P2P_ROLE_CLIENT )
		{
			p2p_client_mode(p);
		}
		else if( p->role == P2P_ROLE_GO )
		{
			p2p_go_mode(p);
		}
	}
	else
	{
		p->p2p_get = 1;
		p2p_status(p, 0);
		memset( p->print_line, 0x00, CMD_SZ );
		sprintf( p->print_line, "Status= %d, %s", p->status, p->fail_msg );
		ui_screen(p);

#ifdef P2P_ATUO		
		pthread_create(&p->pthread, NULL, &polling_status, (void *)p);
#endif
		
	}
}

//After negotiation success, get peer device's interface address.
void p2p_ifaddr(struct p2p *p)
{
	FILE *pf=NULL;
	char addr_12[12] = { 0x00 };
	int i;
	
	/* peer_ifaddr */
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get peer_ifa > status.txt", p->ifname);
	system( p->cmd );
/*
	pf = fopen( "./status.txt", "r" );
	if ( pf )
	{
		memset( p->parse, 0x00, CMD_SZ );
		fgets( p->parse, CMD_SZ, pf );
		fgets( p->parse, CMD_SZ, pf );
		strncpy(addr_12, p->parse, 12 );

		for(i=0; i<6; i++)
		{
			p->peer_ifaddr[3*i] = addr_12[2*i];
			p->peer_ifaddr[3*i+1] = addr_12[2*i+1];
			if(i==5)
				p->peer_ifaddr[3*i+2] = '\0';
			else
				p->peer_ifaddr[3*i+2] = ':';
		}

		fclose( pf );
	}
*/	
	pf = fopen( "./status.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( p->parse, 0x00, CMD_SZ );
			fgets( p->parse, CMD_SZ, pf );
			if( strncmp( p->parse, "MAC", 3) == 0 )
			{
				strncpy( p->peer_ifaddr, p->parse+4, 17 );
				break;
			}	
		}
		fclose( pf );
	}
	
}

void p2p_client_mode(struct p2p *p)
{
	FILE *pf = NULL;
	int count = 0, ret = 0;

	if(p->wpa_open==1)
		return;
	else
		p->wpa_open = 1;

	p2p_ifaddr(p);
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "%s/wpa_supplicant -i %s -c %s -B ",p->wpa_path, p->ifname, p->wpa_conf);
	system( p->cmd );

	p->p2p_get=1;
	memset( p->print_line, 0x00, CMD_SZ );
	strcpy( p->print_line, "Start wpa_supplicant");
	ui_screen(p);

	usleep( SUPPLICANT_INIT_TIME );
	
	if(p->wps_info==1 || p->wps_info==2)
	{
		do
		{
		memset( p->cmd, 0x00, CMD_SZ );

		if(p->connect_go == 1)
			sprintf( p->cmd, "%s/wpa_cli wps_pin %s %d > supp_status.txt ", p->wpa_path, p->peer_devaddr, p->pin);
		else
			sprintf( p->cmd, "%s/wpa_cli wps_pin %s %d > supp_status.txt ", p->wpa_path, p->peer_ifaddr, p->pin);


		system( p->cmd );
	
			pf = fopen( "./supp_status.txt", "r" );	
			if ( pf )
			{
				while( !feof( pf ) ){
					memset(p->parse, 0x00, CMD_SZ);
					fgets(p->parse, CMD_SZ, pf);
					if(strncmp(p->parse, "Selected", 8) == 0)
					{
						ret = 1;
						p->wpsing = 1;
					}
				}
		
				fclose( pf );
			}
			
			if( ret == 0 )
				usleep( SUPPLICANT_INIT_TIME );
		}
		while( ret == 0 );
		
	}
	else if(p->wps_info==3)
	{
		do
		{
		memset( p->cmd, 0x00, CMD_SZ );
		
		if(p->connect_go == 1)
			sprintf( p->cmd, "%s/wpa_cli wps_pbc %s > supp_status.txt ", p->wpa_path, p->peer_devaddr);
		else
			sprintf( p->cmd, "%s/wpa_cli wps_pbc %s > supp_status.txt ", p->wpa_path, p->peer_ifaddr);
		
		system( p->cmd );
	
			pf = fopen( "./supp_status.txt", "r" );	
			if ( pf )
			{
				while( !feof( pf ) ){
					memset(p->parse, 0x00, CMD_SZ);
					fgets(p->parse, CMD_SZ, pf);
					if(strncmp(p->parse, "OK", 2) == 0)
					{
						ret = 1;
						p->wpsing = 1;
					}
				}
		
				fclose( pf );
			}
			
			if( ret == 0 )
				usleep( SUPPLICANT_INIT_TIME );
		}
		while( ret == 0 );

	}
	
	usleep( SUPPLICANT_INTERVAL );
		
	while( count < WPS_RETRY )		//query status
	{
	
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "%s/wpa_cli status > supp_status.txt", p->wpa_path);
		system( p->cmd );
		
		pf = fopen( "./supp_status.txt", "r" );
		if ( pf )
		{
			while( !feof( pf ) ){
				memset( p->parse, 0x00, CMD_SZ );
				fgets( p->parse, CMD_SZ, pf );
				if( strncmp( p->parse, "wpa_state=", 10) == 0 )
				{
					int i;
					if( strncmp( p->parse, "wpa_state=COMPLETED", 19) == 0 ){
						count = WPS_RETRY;
						p->wpsing=0;
#ifdef DHCP
						memset( p->cmd, 0x00, CMD_SZ );
						sprintf( p->cmd, "dhclient %s", p->ifname);
						system( p->cmd );
#endif //DHCP
					}

					p->p2p_get=1;
					memset(p->print_line, 0x00, CMD_SZ);
					for(i=0; i<CMD_SZ; i++){
						if(p->parse[i] == '\n'){
							p->parse[i] = ' ';
						}
					
					}
					sprintf(p->print_line, "%s", p->parse);
					ui_screen(p);
					
					break;
				}	
			}

			fclose( pf );
		}
		
		count++;
		usleep( SUPPLICANT_INTERVAL );
	
	}
}

void p2p_go_mode(struct p2p *p)
{
	FILE *pf = NULL;
	int count = 0, ret = 0, i = -1;
	char addr_lower[18];

	p2p_ifaddr(p);

	p->p2p_get=1;
	memset( p->print_line, 0x00, CMD_SZ );
	strcpy( p->print_line, "Start hostapd");
	ui_screen(p);

	if(p->ap_open != 1)
	{
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "%s/hostapd -B %s > temp.txt",p->ap_path, p->ap_conf);
		system( p->cmd );

		usleep( HOSTAPD_INIT_TIME );
	}

	if(p->wps_info==1 || p->wps_info==2)
	{
		do
		{
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "%s/hostapd_cli wps_pin any %d > supp_status.txt", p->ap_path, p->pin);
		system( p->cmd );
	
			pf = fopen( "./supp_status.txt", "r" );	
			if ( pf )
			{
				while( !feof( pf ) ){
					memset(p->parse, 0x00, CMD_SZ);
					fgets(p->parse, CMD_SZ, pf);
					if(strncmp(p->parse, "", 2) == 0)
					{
						ret = 1;
						p->wpsing = 1;
					}
				}
		
				fclose( pf );
			}
			
			if( ret == 0 )
				usleep( HOSTAPD_INIT_TIME );
		}
		while( ret == 0 );
	}
	else if(p->wps_info==3)
	{
		do
		{
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "%s/hostapd_cli wps_pbc any > supp_status.txt", p->ap_path);
		system( p->cmd );
						
			pf = fopen( "./supp_status.txt", "r" );	
			if ( pf )
			{
				while( !feof( pf ) ){
					memset(p->parse, 0x00, CMD_SZ);
					fgets(p->parse, CMD_SZ, pf);
					if(strncmp(p->parse, "OK", 2) == 0)
					{
						ret = 1;
						p->wpsing = 1;
					}
				}
		
				fclose( pf );
			}
			
			if( ret == 0 )
				usleep( HOSTAPD_INIT_TIME );
		}
		while( ret == 0 );
	}

	usleep( HOSTAPD_INTERVAL );
		
	while( count < WPS_RETRY )		//query status
	{
	
		memset( p->cmd, 0x00, CMD_SZ );
		sprintf( p->cmd, "%s/hostapd_cli all_sta > supp_status.txt", p->ap_path, p->peer_ifaddr);
		system( p->cmd );
		
		pf = fopen( "./supp_status.txt", "r" );
		if ( pf )
		{
			for(i=0;i<17;i++)
			{
				if( p->ap_open != 1 )
					addr_lower[i] = tolower(p->peer_ifaddr[i]);
				else
					addr_lower[i] = tolower(p->peer_devaddr[i]);
			}
			addr_lower[17]='\0';
     			
			while( !feof( pf ) ){
				memset( p->parse, 0x00, CMD_SZ );
				fgets( p->parse, CMD_SZ, pf );
				if( strncmp( p->parse, "dot11RSNAStatsSTAAddress=", 25) == 0 )
				{
					if( strncmp( p->parse+25, addr_lower, 17) == 0 )
					{
						count = WPS_RETRY;
						p->wpsing = 0;

						p->p2p_get=1;
						memset(p->print_line, 0x00, CMD_SZ);
						for(i=0; i<CMD_SZ; i++){
							if(p->parse[i] == '\n'){
								p->parse[i] = ' ';
							}
						}		
						sprintf(p->print_line, "%s", p->parse);
						ui_screen(p);
#ifdef DHCP
						memset( p->cmd, 0x00, CMD_SZ );
						sprintf( p->cmd, "ifconfig %s 192.168.1.254", p->ifname);
						system( p->cmd );
	
						usleep(50000);
						
						system( "/etc/rc.d/init.d/dhcpd start" );
						system( "clear" );
#endif //DHCP

						//After starting hostapd and doing WPS connection successful,
						//We create a thread to query driver if some other p2p devices connected.
						p2p_status(p, 0);
						usleep(50000);

#ifdef P2P_AUTO
						p->res_go = pthread_create(&p->pthread_go, NULL, &polling_client, (void *)p);

						if( p->res_go != 0 )
						{
							p->p2p_get=1;
							memset( p->print_line, 0x00, CMD_SZ );
							sprintf( p->print_line, "Thread creation failed" );
						}
#endif
						
						break;
					}
				}
			}

			fclose( pf );
		}
		
		if( count == WPS_RETRY)
			break;

		count++;
		usleep( HOSTAPD_INTERVAL );
		
		p->p2p_get=1;
		memset( p->print_line, 0x00, CMD_SZ );
		sprintf( p->print_line, "hostapd open, count:%d", count);
		ui_screen(p);

	}

	if(p->ap_open != 1)
		p->ap_open = 1;

}

void p2p_get_hostapd_conf(struct p2p *p)
{
	FILE *pf = NULL;
	
	pf = fopen( p->ap_conf, "r" );	
	if ( pf )
	{
		while( !feof( pf ) ){
			memset(p->parse, 0x00, CMD_SZ);
			fgets(p->parse, CMD_SZ, pf);
			if(strncmp(p->parse, "ssid=", 5) == 0)
			{
				strcpy( p->apd_ssid, p->parse+5 );
			}
			else if(strncmp(p->parse, "channel=", 8) == 0)
			{
				p->op_ch = atoi( p->parse+8 );
			}
			else if(strncmp(p->parse, "device_name=", 12) == 0)
			{
				int i;
				p->dev_name[0] = '"';
				strncpy( p->dev_name+1, p->parse+12, 32 );
				for(i=31; i>0; i--)
				{
					if(p->dev_name[i] == '\n')
					{
						p->dev_name[i]='"';
						p->dev_name[i+1]=' ';
						break;
					}
				}
			}
		}

		fclose( pf );
	}
	
}

void p2p_set_opch(struct p2p *p, char *msg, int print)
{
	if(print == 1)
	{
		ui_screen(p);
		printf("%s", msg);	
		scanf("%d",&p->op_ch);
	}
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set op_ch=%d", p->ifname, p->op_ch);
	system( p->cmd );
}

void p2p_softap_ssid(struct p2p *p, char *msg, int print)
{
	if(print == 1)
	{
		ui_screen(p);
		printf("%s", msg);
		scanf("%s",p->apd_ssid);
	}
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_set ssid=%s ", p->ifname, p->apd_ssid);
	system( p->cmd );
}

void p2p_listen_ch(struct p2p *p, char *msg)
{
	ui_screen(p);
	printf("%s", msg);
	scanf("%d",&p->listen_ch);
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "./iwpriv %s p2p_set listen_ch=%d ", p->ifname, p->listen_ch);
	system( p->cmd );
}

//When receive provision discovery request,
//it can show which device address that are connected.
void p2p_peer_devaddr(struct p2p *p, char *peer_devaddr)
{
	FILE *pf = NULL;
	char addr_12[12] = { 0x00 };
	int i;
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get peer_deva > peer.txt", p->ifname);
	system( p->cmd );
	
	pf = fopen( "./peer.txt", "r" );
	if ( pf )
	{
		memset( p->parse, 0x00, CMD_SZ );
		fgets( p->parse, CMD_SZ, pf );
		fgets( p->parse, CMD_SZ, pf );
		strncpy(addr_12, p->parse, 12 );

		for(i=0; i<6; i++)
		{
			p->peer_devaddr[3*i] = addr_12[2*i];
			p->peer_devaddr[3*i+1] = addr_12[2*i+1];

			if(i==5)
				p->peer_devaddr[3*i+2] = '\0';
			else
				p->peer_devaddr[3*i+2] = ':';
		}

		fclose( pf );

	}
	
}

//When receive provision discovery request,
//it can show which config method that want to be as WPS connection.
void p2p_peer_req_cm(struct p2p *p, char *peer_req_cm)
{
	FILE *pf = NULL;
	
	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get req_cm > peer.txt", p->ifname);
	system( p->cmd );
	
	pf = fopen( "./peer.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( p->parse, 0x00, CMD_SZ );
			fgets( p->parse, CMD_SZ, pf );
			if( strncmp( p->parse, "CM", 2) == 0 )
			{
				strncpy( peer_req_cm, p->parse+3, 3 );
				break;
			}	
		}
		fclose( pf );
	}
	
}

//When be regotiated passsively and successfully,
//it will show peer device's peer device address and request config method
void p2p_peer_info(struct p2p *p, char *peer_devaddr, char *peer_req_cm)
{
	p->p2p_get=1;

	memset( p->print_line, 0x00, CMD_SZ );
	sprintf( p->print_line, "Peer address:%s, req_cm: %s ", peer_devaddr, peer_req_cm);

}

//After scan, it will get other devices' supported config methods.
void p2p_wps_cm(struct p2p *p, char *scan_addr, char *cms)
{
	FILE *pf = NULL;
	int cm=0, i=0;
	char parse[100] = {0x00};
	memset( cms, 0x00, 30 );

	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get2 wpsCM=%s > cm.txt", p->ifname, scan_addr);	
	system( p->cmd );	
		
	pf = fopen( "./cm.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( parse, 0x00, CMD_SZ );
			fgets( parse, CMD_SZ, pf );
			if( strncmp( parse, "M=", 2 ) == 0 )
			{
				cm = atoi( &parse[ 2 ] );
				
				if((cm & WPS_CONFIG_METHOD_LABEL) == WPS_CONFIG_METHOD_LABEL){
					strncpy( cms+i, " LAB", 4 );
					i=i+4;
				}
				if((cm & WPS_CONFIG_METHOD_DISPLAY) == WPS_CONFIG_METHOD_DISPLAY){
					if((cm & WPS_CONFIG_METHOD_VDISPLAY) == WPS_CONFIG_METHOD_VDISPLAY){
						strncpy( cms+i, " VDIS", 5 );
						i=i+5;
					}else if((cm & WPS_CONFIG_METHOD_PDISPLAY) == WPS_CONFIG_METHOD_PDISPLAY){
						strncpy( cms+i, " PDIS", 5 );
						i=i+5;
					}else{
						strncpy( cms+i, " DIS", 4 );
						i=i+4;
					}
				}
				if((cm & WPS_CONFIG_METHOD_E_NFC) == WPS_CONFIG_METHOD_E_NFC){
					strncpy( cms+i, " ENFC", 5 );
					i=i+5;
				}
				if((cm & WPS_CONFIG_METHOD_I_NFC) == WPS_CONFIG_METHOD_I_NFC){
					strncpy( cms+i, " INFC", 5 );
					i=i+5;
				}
				if((cm & WPS_CONFIG_METHOD_NFC) == WPS_CONFIG_METHOD_NFC){
					strncpy( cms+i, " NFC", 4 );
					i=i+4;
				}
				if((cm & WPS_CONFIG_METHOD_PBC) == WPS_CONFIG_METHOD_PBC){
					if((cm & WPS_CONFIG_METHOD_VPBC) == WPS_CONFIG_METHOD_VPBC){
						strncpy( cms+i, " VPBC", 5 );
						i=i+5;
					}else if((cm & WPS_CONFIG_METHOD_PPBC) == WPS_CONFIG_METHOD_PPBC){
						strncpy( cms+i, " PPBC", 5 );
						i=i+5;
					}else{
						strncpy( cms+i, " PBC", 4 );
						i=i+4;
					}
				}
				if((cm & WPS_CONFIG_METHOD_KEYPAD) == WPS_CONFIG_METHOD_KEYPAD){
					strncpy( cms+i, " PAD", 4 );
					i=i+4;
				}

				break;
			}	
		}
		fclose( pf );
	}
	
}

//After scan, it will get other devices' device name.
void p2p_device_name(struct p2p *p, char *scan_addr, char *dns)
{
	FILE *pf = NULL;
	int i;
	char parse[100] = {0x00};
	memset( dns, 0x00, 32 );

	memset( p->cmd, 0x00, CMD_SZ );
	sprintf( p->cmd, "iwpriv %s p2p_get2 devN=%s > cm.txt", p->ifname, scan_addr);	
	system( p->cmd );	
		
	pf = fopen( "./cm.txt", "r" );
	if ( pf )
	{
		while( !feof( pf ) ){
			memset( parse, 0x00, CMD_SZ );
			fgets( parse, CMD_SZ, pf );
			if( strncmp( parse, "N=", 2) == 0 )
			{
				strncpy( dns, parse+2, 32);
				for( i=0; i<32; i++)
				{
					if(*(dns+i) == '\n'){
						*(dns+i) = ' ' ;
						break;
					}
				}

				break;
			}	
		}
		fclose( pf );
	}
}

//Successively query local device status with interval "POLLING_INTERVAL"
//when status == P2P_STATE_RX_PROVISION_DIS_REQ, 
//it require user to insert corresponding WPS config method
void *polling_status(void *arg)
{
	struct p2p *p=(struct p2p*)arg;
	
	while( 1 ){

		p2p_status(p, 0);
		if( p->status == P2P_STATE_RX_PROVISION_DIS_REQ )
		{
			p->thread_trigger = THREAD_DEVICE ;
			
			char peer_devaddr[18];
			char peer_req_cm[4];
			
			memset( peer_devaddr, 0x00, 18);
			memset( peer_req_cm, 0x00, 4);
			
			p2p_peer_devaddr(p, peer_devaddr);
			p2p_peer_req_cm(p, peer_req_cm);
			p2p_peer_info(p, p->peer_devaddr, peer_req_cm);

			ui_screen(p);

			//strncpy(p->peer_devaddr, peer_devaddr, 17);
			if( (strncmp( peer_req_cm, "dis", 3) == 0) || (strncmp( peer_req_cm, "lab", 3) == 0) )
			{
				printf("Here is your PIN, insert c to continue: %d\n", p->pin);
			}
			else if( (strncmp( peer_req_cm, "pbc", 3) == 0) )
			{
				printf("Please push b to accept:\n");
			}
			else if( (strncmp( peer_req_cm, "pad", 3) == 0) )
			{
				printf("Please insert peer PIN code:\n");
			}

			break;
  	}

		usleep( POLLING_INTERVAL );
	}

	return NULL;
}

//If p2p device becomes GO, we still polling driver status
//to check whether some other p2p devices connected
void *polling_client(void *arg)
{
	struct p2p *p=(struct p2p*)arg;
	
	while( 1 ){

		p2p_status(p, 0);
		if( p->status == P2P_STATE_RX_PROVISION_DIS_REQ )
		{
			p->thread_trigger = THREAD_GO ;
			
			char peer_devaddr[18];
			char peer_req_cm[4];
			
			memset( peer_devaddr, 0x00, 18);
			memset( peer_req_cm, 0x00, 4);
			
			p2p_peer_devaddr(p, peer_devaddr);
			p2p_peer_req_cm(p, peer_req_cm);
			p2p_peer_info(p, p->peer_devaddr, peer_req_cm);

			ui_screen(p);

			//strncpy(p->peer_devaddr, peer_devaddr, 17);
			if( (strncmp( peer_req_cm, "dis", 3) == 0) || (strncmp( peer_req_cm, "lab", 3) == 0) )
			{
				printf("Here is your PIN, insert c to continue: %d\n", p->pin);
			}
			else if( (strncmp( peer_req_cm, "pbc", 3) == 0) )
			{
				printf("Please push b to accept; stauts:%d:\n", p->status);
			}
			else if( (strncmp( peer_req_cm, "pad", 3) == 0) )
			{
				printf("Please insert peer PIN code:\n");
			}

			break;
  	}

		usleep( POLLING_INTERVAL );
	}

	return NULL;
}
