#include <string.h>

#include "socket.h"
#include "ConfigData.h"
#include "ConfigMessage.h"
#include "flash_programming.h"

uint8_t CONFIG_BUF[10] = {0,}; // 7byte
uint8_t GET_INFO_BUF[10] = {0,}; // 6byte
uint8_t SET_INFO_RECV_BUF[128] = {0,};
uint8_t SET_INFO_PASER_BUF[200] = {0,};

uint8_t SET_INFO_PASER_BUF11[200] = {0,};


extern wiz_NetInfo gWIZNETINFO;

#pragma CODE_SECTION(encrypt, ".TI.ramfunc");
#pragma CODE_SECTION(decrypt, ".TI.ramfunc");
#pragma CODE_SECTION(process_discovery, ".TI.ramfunc");
#pragma CODE_SECTION(process_get_info, ".TI.ramfunc");
#pragma CODE_SECTION(process_set_info, ".TI.ramfunc");
#pragma CODE_SECTION(do_udp_config, ".TI.ramfunc");

extern volatile uint16 config_flash_set_flag;

static void encrypt(uint8_t key, uint8_t *buffer, uint16_t len)
{
	uint16_t i;
	for(i=0; i<len; i++) {
		buffer[i] ^= key;
	}
}

static void decrypt(uint8_t key, uint8_t *buffer, uint16_t len)
{
	uint16_t i;
	for(i=0; i<len; i++) {
		buffer[i] ^= key;
	}
}

static void process_discovery(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
    uint16_t len;
	uint8_t buffer[7 + 52];	// WIZnet_Header(7) + most biggest structure(52)
	uint8_t send_buf[7 + 52];   // WIZnet_Header(7) + most biggest structure(52)
	uint32_t ptr;
	WIZnet_Discovery_Reply reply;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;
	uint8_t j,k;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	switch(wiznet_header->op_code[0]) {
	case DISCOVERY_ALL: // Config Search
		break;

	case DISCOVERY_PRODUCT_CODE:
	{
		WIZnet_Discovery_Product_Code product_code;
		recvfrom(sock, (uint8_t *)&product_code, sizeof(WIZnet_Discovery_Product_Code), ip, &port);

		if((wiznet_header->valid & 0x80))
			decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&product_code, len);

		if(memcmp(product_code.product_code, s2e_packet->module_type, 3))
			return;
		break;
	}

	case DISCOVERY_MAC_ADDRESS:
	{
		WIZnet_Discovery_MAC_Address mac_address;
		recvfrom(sock, (uint8_t *)&mac_address, sizeof(WIZnet_Discovery_MAC_Address), ip, &port);

		if((wiznet_header->valid & 0x80))
			decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&mac_address, len);

		if(   mac_address.start_mac_address[0] > s2e_packet->network_info_common.mac[0]
		   || mac_address.start_mac_address[1] > s2e_packet->network_info_common.mac[1]
		   || mac_address.start_mac_address[2] > s2e_packet->network_info_common.mac[2]
		   || mac_address.start_mac_address[3] > s2e_packet->network_info_common.mac[3]
		   || mac_address.start_mac_address[4] > s2e_packet->network_info_common.mac[4]
		   || mac_address.start_mac_address[5] > s2e_packet->network_info_common.mac[5])
			return;
		if(   mac_address.end_mac_address[0] < s2e_packet->network_info_common.mac[0]
		   || mac_address.end_mac_address[1] < s2e_packet->network_info_common.mac[1]
		   || mac_address.end_mac_address[2] < s2e_packet->network_info_common.mac[2]
		   || mac_address.end_mac_address[3] < s2e_packet->network_info_common.mac[3]
		   || mac_address.end_mac_address[4] < s2e_packet->network_info_common.mac[4]
		   || mac_address.end_mac_address[5] < s2e_packet->network_info_common.mac[5])
			return;
		break;
	}

	case DISCOVERY_ALIAS:
	{
		WIZnet_Discovery_Alias alias;
		recvfrom(sock, (uint8_t *)&alias, sizeof(WIZnet_Discovery_Alias), ip, &port);

		if((wiznet_header->valid & 0x80))
			decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&alias, len);

		if(memcmp(alias.alias, s2e_packet->module_name, strlen((char *)alias.alias)))
			return;
		break;
	}

	case DISCOVERY_MIXED_COND:
	{
		WIZnet_Discovery_Mixed_Condition mixed_condition;
		recvfrom(sock, (uint8_t *)&mixed_condition, sizeof(WIZnet_Discovery_Mixed_Condition), ip, &port);

		if((wiznet_header->valid & 0x80))
			decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&mixed_condition, len);

		break;
	}

	default:
		return;
	}

	// reply
	wiznet_header->length = 12; // WIZnet_Discovery_Reply = 12word
	wiznet_header->op_code[1] = WIZNET_REPLY;

	// Insert data values
    s2e_packet->module_type[0] = 0x02;
    s2e_packet->module_type[1] = 0x00;
    s2e_packet->module_type[2] = 0x00;

    s2e_packet->fw_ver[0] = 1;
    s2e_packet->fw_ver[1] = 2;
    s2e_packet->fw_ver[2] = 3;

    set_mac(gWIZNETINFO.mac); // set mac address

	memcpy(reply.product_code, s2e_packet->module_type, 3);
	memcpy(reply.fw_version, s2e_packet->fw_ver, 3);
	memcpy(reply.mac_address, s2e_packet->network_info_common.mac, 6);

	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, 12); // sizeof(WIZnet_Discovery_Reply) = 12word

	ptr = 0;
	memcpy(buffer, wiznet_header, 7); // sizeof(WIZnet_Header) = 7word
	ptr += 7;
	memcpy(buffer + ptr, (void *)&reply, 12); // sizeof(WIZnet_Discovery_Reply) = 12word
	ptr += 12;

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4); // broad cast
	}

    for (j=0, k=0; j<ptr; j++) // W5300 send 2ea value of 1byte.
    {
        if ((j & 0x01) == 0)
        {
            send_buf[k] = (buffer[j] << 8) & 0xFF00;
        }
        else
        {
            send_buf[k] |= buffer[j] & 0x00FF;
            k++;
        }
    }
	sendto(sock, send_buf, ptr, remote_info->ip, remote_info->port);
}

static void process_get_info(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
	uint16_t len;
	uint8_t buffer[7 + 168]; // 7=wiznet_header, 160=sizeof(WIZnet_Get_Info_Reply)
	uint8_t send_buf[7 + 168]; // 7=wiznet_header, 160=sizeof(WIZnet_Get_Info_Reply)
	uint32_t ptr;
	WIZnet_Get_Info get_info;
	WIZnet_Get_Info_Reply reply;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;
	uint8_t j,k;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	recvfrom(sock, GET_INFO_BUF, sizeof(WIZnet_Get_Info), ip, &port); // 6=sizeof(WIZnet_Get_Info)

	// W5300 read 2ea value of 1byte. so, when the read the RX_FIFO, value referred to below.
	// ex) 0  1  2  3  4  5  6  7 [byte]
	// ex) 00 5F 5F 5F 5F 5F 6F 00 [value]
	get_info.dst_mac_address[0] = GET_INFO_BUF[0] & 0x00FF;
	get_info.dst_mac_address[1] = (GET_INFO_BUF[1] >> 8) & 0x00FF;
	get_info.dst_mac_address[2] = GET_INFO_BUF[1] & 0x00FF;
	get_info.dst_mac_address[3] = (GET_INFO_BUF[2] >> 8) & 0x00FF;
	get_info.dst_mac_address[4] = GET_INFO_BUF[2] & 0x00FF;
	get_info.dst_mac_address[5] = (GET_INFO_BUF[3] >> 8) & 0x00FF;

	if((wiznet_header->valid & 0x80))
		decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&get_info, len);


	if(memcmp(get_info.dst_mac_address, s2e_packet->network_info_common.mac, 6))
		return;

	// Make reply packet
	wiznet_header->length = sizeof(WIZnet_Get_Info_Reply); // 168=sizeof(WIZnet_Get_Info_Reply)
	wiznet_header->op_code[1] = WIZNET_REPLY;

	set_S2E_Packet_to_getinfo_value();

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6);

#if 1
	// Config tool send ethernet packet should be the little-endian process.
	reply.system_info2.packet_size[1] = (s2e_packet->packet_size >> 8) & 0x00FF;
	reply.system_info2.packet_size[0] = s2e_packet->packet_size & 0x00FF;
	memcpy(&reply.system_info2.module_type, s2e_packet->module_type, sizeof(s2e_packet->module_type));
	memcpy(&reply.system_info2.module_name, s2e_packet->module_name, sizeof(s2e_packet->module_name));
	memcpy(&reply.system_info2.fw_ver, s2e_packet->fw_ver, sizeof(s2e_packet->fw_ver));
	memcpy(&reply.system_info2.network_info_common.mac, s2e_packet->network_info_common.mac, sizeof(s2e_packet->network_info_common.mac));
	memcpy(&reply.system_info2.network_info_common.local_ip, s2e_packet->network_info_common.local_ip, sizeof(s2e_packet->network_info_common.local_ip));
	memcpy(&reply.system_info2.network_info_common.gateway, s2e_packet->network_info_common.gateway, sizeof(s2e_packet->network_info_common.gateway));
	memcpy(&reply.system_info2.network_info_common.subnet, s2e_packet->network_info_common.subnet, sizeof(s2e_packet->network_info_common.subnet));

	reply.system_info2.network_info[0].working_mode = s2e_packet->network_info[0].working_mode;
	reply.system_info2.network_info[0].state = s2e_packet->network_info[0].state;

	memcpy(&reply.system_info2.network_info[0].remote_ip, s2e_packet->network_info[0].remote_ip, sizeof(s2e_packet->network_info->remote_ip));

	reply.system_info2.network_info[0].local_port[1] = (s2e_packet->network_info[0].local_port >> 8) & 0x00FF; // little-endian process
	reply.system_info2.network_info[0].local_port[0] = s2e_packet->network_info[0].local_port & 0x00FF;

	reply.system_info2.network_info[0].remote_port[1] = (s2e_packet->network_info[0].remote_port >> 8) & 0x00FF; // little-endian process
	reply.system_info2.network_info[0].remote_port[0] = s2e_packet->network_info[0].remote_port & 0x00FF;

	reply.system_info2.network_info[0].inactivity[1] = (s2e_packet->network_info[0].inactivity >> 8) & 0x00FF; // little-endian process
    reply.system_info2.network_info[0].inactivity[0] = s2e_packet->network_info[0].inactivity & 0x00FF;

    reply.system_info2.network_info[0].reconnection[1] = (s2e_packet->network_info[0].reconnection >> 8) & 0x00FF; // little-endian process
    reply.system_info2.network_info[0].reconnection[0] = s2e_packet->network_info[0].reconnection & 0x00FF;

    reply.system_info2.network_info[0].packing_time[1] = (s2e_packet->network_info[0].packing_time >> 8) & 0x00FF; // little-endian process
    reply.system_info2.network_info[0].packing_time[0] = s2e_packet->network_info[0].packing_time & 0x00FF;

	reply.system_info2.network_info[0].packing_size = s2e_packet->network_info[0].packing_size;
	memcpy(&reply.system_info2.network_info[0].packing_delimiter, s2e_packet->network_info[0].packing_delimiter, sizeof(s2e_packet->network_info->packing_delimiter));

	reply.system_info2.network_info[0].packing_delimiter_length = s2e_packet->network_info[0].packing_delimiter_length;
	reply.system_info2.network_info[0].packing_data_appendix = s2e_packet->network_info[0].packing_data_appendix;

    reply.system_info2.serial_info[0].baud_rate[3] = (s2e_packet->serial_info[0].baud_rate >> 24) & 0x00FF; // little-endian process
    reply.system_info2.serial_info[0].baud_rate[2] = (s2e_packet->serial_info[0].baud_rate >> 16) & 0x00FF;
    reply.system_info2.serial_info[0].baud_rate[1] = (s2e_packet->serial_info[0].baud_rate >> 8) & 0x00FF;
    reply.system_info2.serial_info[0].baud_rate[0] = s2e_packet->serial_info[0].baud_rate & 0x00FF;

    reply.system_info2.serial_info[0].data_bits = s2e_packet->serial_info[0].data_bits;
    reply.system_info2.serial_info[0].parity = s2e_packet->serial_info[0].parity;
    reply.system_info2.serial_info[0].stop_bits = s2e_packet->serial_info[0].stop_bits;
    reply.system_info2.serial_info[0].flow_control = s2e_packet->serial_info[0].flow_control;

    memcpy(&reply.system_info2.options.pw_setting, s2e_packet->options.pw_setting, sizeof(s2e_packet->options.pw_setting));
    memcpy(&reply.system_info2.options.pw_connect, s2e_packet->options.pw_connect, sizeof(s2e_packet->options.pw_connect));
    reply.system_info2.options.dhcp_use = s2e_packet->options.dhcp_use;
    reply.system_info2.options.dns_use = s2e_packet->options.dns_use;
    memcpy(&reply.system_info2.options.dns_server_ip, s2e_packet->options.dns_server_ip, sizeof(s2e_packet->options.dns_server_ip));
    memcpy(&reply.system_info2.options.dns_domain_name, s2e_packet->options.dns_domain_name, sizeof(s2e_packet->options.dns_domain_name));
    reply.system_info2.options.serial_command = s2e_packet->options.serial_command;
    memcpy(&reply.system_info2.options.serial_trigger, s2e_packet->options.serial_trigger, sizeof(s2e_packet->options.serial_trigger));
#else
	memcpy(&reply.system_info, s2e_packet, sizeof(S2E_Packet)); // 162=sizeof(s2e_packet)
#endif
	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Get_Info_Reply));

	ptr = 0;
    memcpy(buffer, wiznet_header, sizeof(WIZnet_Header));
    ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Get_Info_Reply));
	ptr += sizeof(WIZnet_Get_Info_Reply);

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4);
	}
    for (j=0, k=0; j<ptr; j++) // W5300 send 2ea value of 1byte.
    {
        if ((j & 0x01) == 0)
        {
            send_buf[k] = (buffer[j] << 8) & 0xFF00;
        }
        else
        {
            send_buf[k] |= buffer[j] & 0x00FF;
            k++;
        }
    }
	sendto(sock, send_buf, ptr, remote_info->ip, remote_info->port);
}

static void process_set_info(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
	uint16_t len;
	uint8_t buffer[sizeof(WIZnet_Header) + sizeof(WIZnet_Set_Info_Reply)];
	uint8_t send_buf[sizeof(WIZnet_Header) + sizeof(WIZnet_Set_Info_Reply)];
	uint32_t ptr;
	WIZnet_Set_Info set_info;
	WIZnet_Set_Info_Reply reply;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;
	uint8_t i,j,k;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	recvfrom(sock, SET_INFO_RECV_BUF, sizeof(WIZnet_Set_Info), ip, &port);
    for (j=0, k=0; j<len+2; j++)
    {
        if ((j & 0x01) == 0)
        {
            SET_INFO_PASER_BUF[j] = (SET_INFO_RECV_BUF[k] >> 8) & 0x00FF;
        }
        else
        {
            SET_INFO_PASER_BUF[j] = SET_INFO_RECV_BUF[k] & 0x00FF;
            k++;
        }
    }

    // Flash write
    //Flash_write((uint16*)&SET_INFO_PASER_BUF,len+2);
    //Flash_write();
    config_flash_set_flag = 1;

    if ((wiznet_header->valid & 0x80))
        decrypt((wiznet_header->valid & 0x7F), SET_INFO_PASER_BUF, len+2);

#if 1
	// Config tool send ethernet packet should be the little-endian process.
	// Data parsing from config tool setting
    // SET_INFO_PASER_BUF buffer First byte is dummy
	for(i=0;i<6;i++){
	    set_info.dst_mac_address[i] = SET_INFO_PASER_BUF[i+1];
	}
	set_info.set_pw_len = SET_INFO_PASER_BUF[7];
	for(i=0;i<16;i++){
	    set_info.set_pw[i] = SET_INFO_PASER_BUF[i+8];
	}
	set_info.system_info2.packet_size[0] = (SET_INFO_PASER_BUF[25] & 0x00FF); // 00 // little-endian
	set_info.system_info2.packet_size[1] = (SET_INFO_PASER_BUF[24] & 0x00FF); // A2
	set_info.system_info2.module_type[0] = SET_INFO_PASER_BUF[26];
    set_info.system_info2.module_type[1] = SET_INFO_PASER_BUF[27];
    set_info.system_info2.module_type[2] = SET_INFO_PASER_BUF[28];
    for(i=0;i<25;i++){
        set_info.system_info2.module_name[i] = SET_INFO_PASER_BUF[i+29];
    }
    set_info.system_info2.fw_ver[0] = SET_INFO_PASER_BUF[54];
    set_info.system_info2.fw_ver[1] = SET_INFO_PASER_BUF[55];
    set_info.system_info2.fw_ver[2] = SET_INFO_PASER_BUF[56];
    for(i=0;i<6;i++){
        set_info.system_info2.network_info_common.mac[i] = SET_INFO_PASER_BUF[i+57];
    }
    for(i=0;i<4;i++){
        set_info.system_info2.network_info_common.local_ip[i] = SET_INFO_PASER_BUF[i+63];
    }
    for(i=0;i<4;i++){
        set_info.system_info2.network_info_common.gateway[i] = SET_INFO_PASER_BUF[i+67];
    }
    for(i=0;i<4;i++){
        set_info.system_info2.network_info_common.subnet[i] = SET_INFO_PASER_BUF[i+71];
    }

    set_info.system_info2.network_info[0].working_mode = SET_INFO_PASER_BUF[75];
    set_info.system_info2.network_info[0].state = SET_INFO_PASER_BUF[76];

    for(i=0;i<4;i++){
        set_info.system_info2.network_info[0].remote_ip[i] = SET_INFO_PASER_BUF[i+77];
    }
    set_info.system_info2.network_info[0].local_port[0] = (SET_INFO_PASER_BUF[82] & 0x00FF); // little-endian
    set_info.system_info2.network_info[0].local_port[1] = (SET_INFO_PASER_BUF[81] & 0x00FF);
    set_info.system_info2.network_info[0].remote_port[0] = (SET_INFO_PASER_BUF[84] & 0x00FF); // little-endian
    set_info.system_info2.network_info[0].remote_port[1] = (SET_INFO_PASER_BUF[83] & 0x00FF);
    set_info.system_info2.network_info[0].inactivity[0] = (SET_INFO_PASER_BUF[86] & 0x00FF); // little-endian
    set_info.system_info2.network_info[0].inactivity[1] = (SET_INFO_PASER_BUF[85] & 0x00FF);
    set_info.system_info2.network_info[0].reconnection[0] = (SET_INFO_PASER_BUF[88] & 0x00FF); // little-endian
    set_info.system_info2.network_info[0].reconnection[1] = (SET_INFO_PASER_BUF[87] & 0x00FF);
    set_info.system_info2.network_info[0].packing_time[0] = (SET_INFO_PASER_BUF[90] & 0x00FF); // little-endian
    set_info.system_info2.network_info[0].packing_time[1] = (SET_INFO_PASER_BUF[89] & 0x00FF);

    set_info.system_info2.network_info[0].packing_size = SET_INFO_PASER_BUF[91];
    for(i=0;i<4;i++){
        set_info.system_info2.network_info[0].packing_delimiter[i] = SET_INFO_PASER_BUF[i+92]; // end RECV_BUF[48], PASER_BUF[96]
    }
    set_info.system_info2.network_info[0].packing_delimiter_length = SET_INFO_PASER_BUF[96];
    set_info.system_info2.network_info[0].packing_data_appendix = SET_INFO_PASER_BUF[97];

    set_info.system_info2.serial_info[0].baud_rate[0] = (SET_INFO_PASER_BUF[101] & 0x00FF);
    set_info.system_info2.serial_info[0].baud_rate[1] = (SET_INFO_PASER_BUF[100] & 0x00FF);
    set_info.system_info2.serial_info[0].baud_rate[2] = (SET_INFO_PASER_BUF[99] & 0x00FF);
    set_info.system_info2.serial_info[0].baud_rate[3] = (SET_INFO_PASER_BUF[98] & 0x00FF);

    set_info.system_info2.serial_info[0].data_bits = SET_INFO_PASER_BUF[102]; // 103
    set_info.system_info2.serial_info[0].parity = SET_INFO_PASER_BUF[103];
    set_info.system_info2.serial_info[0].stop_bits = SET_INFO_PASER_BUF[104];
    set_info.system_info2.serial_info[0].flow_control = SET_INFO_PASER_BUF[105];

    for(i=0;i<10;i++){
        set_info.system_info2.options.pw_setting[i] = SET_INFO_PASER_BUF[i+106];
    }
    for(i=0;i<10;i++){
        set_info.system_info2.options.pw_connect[i] = SET_INFO_PASER_BUF[i+116];
    }
    set_info.system_info2.options.dhcp_use = SET_INFO_PASER_BUF[126];
    set_info.system_info2.options.dns_use = SET_INFO_PASER_BUF[127];

    set_info.system_info2.options.dns_server_ip[0] = SET_INFO_PASER_BUF[128];
    set_info.system_info2.options.dns_server_ip[1] = SET_INFO_PASER_BUF[129];
    set_info.system_info2.options.dns_server_ip[2] = SET_INFO_PASER_BUF[130];
    set_info.system_info2.options.dns_server_ip[3] = SET_INFO_PASER_BUF[131];

    for(i=0;i<50;i++){
        set_info.system_info2.options.dns_domain_name[i] = SET_INFO_PASER_BUF[i+132];
    }
    set_info.system_info2.options.serial_command = SET_INFO_PASER_BUF[182];

    set_info.system_info2.options.serial_trigger[0] = SET_INFO_PASER_BUF[183];
    set_info.system_info2.options.serial_trigger[1] = SET_INFO_PASER_BUF[184];
    set_info.system_info2.options.serial_trigger[2] = SET_INFO_PASER_BUF[185];
#endif
//	if((wiznet_header->valid & 0x80))
//		decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&set_info, len+2);

	if(memcmp(set_info.dst_mac_address, s2e_packet->network_info_common.mac, 6))
		return;

	if(strcmp((char *)set_info.set_pw, (char *)s2e_packet->options.pw_setting))
		return;

	//update config msg
	set_info.system_info2.fw_ver[0] = MAJOR_VER;
	set_info.system_info2.fw_ver[1] = MINOR_VER;
	set_info.system_info2.fw_ver[2] = MAINTENANCE_VER;

	//save to storage
	save_S2E_Packet_to_storage(); // Ethernet state

	// reply
	wiznet_header->length = sizeof(WIZnet_Set_Info_Reply);
	wiznet_header->op_code[1] = WIZNET_REPLY;

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6); // copy mac address
	memcpy(&reply.system_info2, &set_info.system_info2, sizeof(S2E_Packet2)); // copy reply message

	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Set_Info_Reply));

	ptr = 0;
	memcpy(buffer, wiznet_header, sizeof(WIZnet_Header));
	ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Set_Info_Reply));
	ptr += sizeof(WIZnet_Set_Info_Reply);

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4);
	}
    for (j=0, k=0; j<ptr; j++) // W5300 send 2ea value of 1byte.
    {
        if ((j & 0x01) == 0)
        {
            send_buf[k] = (buffer[j] << 8) & 0xFF00;
        }
        else
        {
            send_buf[k] |= buffer[j] & 0x00FF;
            k++;
        }
    }
    sendto(sock, send_buf, ptr, remote_info->ip, remote_info->port);
	//NVIC_SystemReset();
}

static void process_firmware_upload_init(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
	S2E_Packet *value = get_S2E_Packet_pointer();
	struct __network_info_common tmp;
	uint16_t len;
	WIZnet_Firmware_Upload_Init firmware_upload;
	uint8_t buffer[sizeof(WIZnet_Header) + sizeof(WIZnet_Firmware_Upload_Done_Reply)];
	uint32_t ptr;
	WIZnet_Firmware_Upload_Init_Reply reply;
	Firmware_Upload_Info firmware_upload_info;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	recvfrom(sock, (uint8_t *)&firmware_upload, sizeof(WIZnet_Firmware_Upload_Init), ip, &port);

	if(wiznet_header->op_code[0] != FIRMWARE_UPLOAD_INIT)
		return;

	if((wiznet_header->valid & 0x80))
		decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&firmware_upload, len);

	if(memcmp(firmware_upload.dst_mac_address, s2e_packet->network_info_common.mac, 6))
		return;
	
	if(strcmp((char *)firmware_upload.set_pw, (char *)s2e_packet->options.pw_setting))
		return;

	/* Storee Current Network Information (For DHCP) */
	memcpy(&tmp, &(value->network_info_common), sizeof(struct __network_info_common));
	load_S2E_Packet_from_storage();
	memcpy(&(value->network_info_common), &tmp, sizeof(struct __network_info_common));
	save_S2E_Packet_to_storage();

	//write the TFTP server ip, port, filename and etc. to storage
	memcpy(&firmware_upload_info.wiznet_header                , wiznet_header                            , sizeof(WIZnet_Header));
	memcpy(&firmware_upload_info.configtool_info              , remote_info                              , sizeof(Remote_Info));
	memcpy(firmware_upload_info.tftp_info.ip                  , (uint8_t *) (firmware_upload.server_ip)  , sizeof(uint8_t)*4);
	memcpy((uint8_t *)&(firmware_upload_info.tftp_info.port)  , (uint8_t *)&(firmware_upload.server_port), sizeof(uint8_t)*2);
	memcpy(firmware_upload_info.filename                      , (uint8_t *) (firmware_upload.file_name)  , sizeof(uint8_t)*50);
	//write_storage(0, &firmware_upload_info, sizeof(Firmware_Upload_Info));

	// reply
	wiznet_header->length = sizeof(WIZnet_Firmware_Upload_Init_Reply);
	wiznet_header->op_code[1] = WIZNET_REPLY;

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6);

	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Firmware_Upload_Init_Reply));

	ptr = 0;
	memcpy(buffer, wiznet_header, sizeof(WIZnet_Header));
	ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Firmware_Upload_Init_Reply));
	ptr += sizeof(WIZnet_Firmware_Upload_Init_Reply);

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4);
	}
	sendto(sock, buffer, ptr, remote_info->ip, remote_info->port);

	//delay_ms(50);

	// Reboot to bootloader
	//NVIC_SystemReset();
}

void reply_firmware_upload_done(uint8_t sock)
{
	uint8_t buffer[sizeof(WIZnet_Header) + sizeof(WIZnet_Firmware_Upload_Done_Reply)];
	uint32_t ptr;
	WIZnet_Firmware_Upload_Done_Reply reply;
	Firmware_Upload_Info firmware_upload_info;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;

	close(sock);
	socket(sock, Sn_MR_UDP, REMOTE_CLIENT_PORT, 0);

	//read_storage(0, &firmware_upload_info, sizeof(Firmware_Upload_Info));

	// reply
	firmware_upload_info.wiznet_header.length = sizeof(WIZnet_Firmware_Upload_Done_Reply);
	firmware_upload_info.wiznet_header.op_code[0] = FIRMWARE_UPLOAD_DONE;
	firmware_upload_info.wiznet_header.op_code[1] = WIZNET_REPLY;

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6);

	if((firmware_upload_info.wiznet_header.valid & 0x80))
		encrypt((firmware_upload_info.wiznet_header.valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Firmware_Upload_Done_Reply));

	ptr = 0;
	memcpy(buffer, &firmware_upload_info.wiznet_header, sizeof(WIZnet_Header));
	ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Firmware_Upload_Done_Reply));
	ptr += sizeof(WIZnet_Firmware_Upload_Done_Reply);

	if(firmware_upload_info.wiznet_header.unicast == 0) {
		memset(firmware_upload_info.configtool_info.ip, 255, 4);
	}
	memcpy(ip, firmware_upload_info.configtool_info.ip, sizeof(uint8_t) * 4);
	memcpy(&port, &firmware_upload_info.configtool_info.port, sizeof(uint16_t));
	sendto(sock, buffer, ptr, ip, port);
}

static void process_remote_reset(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
	uint16_t len;
	uint8_t buffer[sizeof(WIZnet_Header) + sizeof(WIZnet_Reset_Reply)];
	uint32_t ptr;
	WIZnet_Reset reset;
	WIZnet_Reset_Reply reply;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	recvfrom(sock, (uint8_t *)&reset, sizeof(WIZnet_Reset), ip, &port);

	if((wiznet_header->valid & 0x80))
		decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reset, len);

	if(memcmp(reset.dst_mac_address, s2e_packet->network_info_common.mac, 6))
		return;

	if(strcmp((char *)reset.set_pw, (char *)s2e_packet->options.pw_setting))
		return;

	// reply
	wiznet_header->length = sizeof(WIZnet_Reset_Reply);
	wiznet_header->op_code[1] = WIZNET_REPLY;

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6);

	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Reset_Reply));

	ptr = 0;
	memcpy(buffer, wiznet_header, sizeof(WIZnet_Header));
	ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Reset_Reply));
	ptr += sizeof(WIZnet_Reset_Reply);

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4);
	}
	sendto(sock, buffer, ptr, remote_info->ip, remote_info->port);

	// Reboot to bootloader
	//NVIC_SystemReset();
}

static void process_factory_reset(uint8_t sock, Remote_Info *remote_info, WIZnet_Header *wiznet_header)
{
	uint16_t len;
	uint8_t buffer[sizeof(WIZnet_Header) + sizeof(WIZnet_Factory_Reset_Reply)];
	uint32_t ptr;
	WIZnet_Factory_Reset factory_reset;
	WIZnet_Factory_Reset_Reply reply;
	S2E_Packet *s2e_packet = get_S2E_Packet_pointer();
	uint8_t ip[4];
	uint16_t port;

	getsockopt(sock, SO_REMAINSIZE, &len);
	if(len != wiznet_header->length)
		return;

	recvfrom(sock, (uint8_t *)&factory_reset, sizeof(WIZnet_Factory_Reset), ip, &port);

	if((wiznet_header->valid & 0x80))
		decrypt((wiznet_header->valid & 0x7F), (uint8_t *)&factory_reset, len);

	if(memcmp(factory_reset.dst_mac_address, s2e_packet->network_info_common.mac, 6))
		return;

	if(strcmp((char *)factory_reset.set_pw, (char *)s2e_packet->options.pw_setting))
		return;

	// set factory value
	set_S2E_Packet_to_factory_value();
	save_S2E_Packet_to_storage();

	// reply
	wiznet_header->length = sizeof(WIZnet_Reset_Reply);
	wiznet_header->op_code[1] = WIZNET_REPLY;

	memcpy(reply.src_mac_address, s2e_packet->network_info_common.mac, 6);

	if((wiznet_header->valid & 0x80))
		encrypt((wiznet_header->valid & 0x7F), (uint8_t *)&reply, sizeof(WIZnet_Factory_Reset_Reply));

	ptr = 0;
	memcpy(buffer, wiznet_header, sizeof(WIZnet_Header));
	ptr += sizeof(WIZnet_Header);
	memcpy(buffer + ptr, (void *)&reply, sizeof(WIZnet_Factory_Reset_Reply));
	ptr += sizeof(WIZnet_Factory_Reset_Reply);

	if(wiznet_header->unicast == 0) {
		memset(remote_info->ip, 255, 4);
	}
	sendto(sock, buffer, ptr, remote_info->ip, remote_info->port);

	// Reboot to bootloader
	//NVIC_SystemReset();
}

void do_udp_config(uint8_t sock)
{
	static uint8_t sckState;
	uint32_t len;
	Remote_Info remote_info;
	WIZnet_Header wiznet_header;

	getsockopt(sock, SO_STATUS, &sckState);
	switch (sckState) {
	case SOCK_UDP:
		getsockopt(sock, SO_RECVBUF, &len); // Config data First receive
        if (len >= 7) {
		    // When the config tool pushed search button, it send 7 data length.(last data dummy)
		    // W5300 read 2ea value of 1byte. so when the read data, it should be 1byte classify.
		    // 0    1    2    3    [byte]
		    // a5b0 00a1 aa00 0000 [value]
		    //recvfrom(sock, (uint8_t *)&wiznet_header, sizeof(WIZnet_Header), remote_info.ip, &remote_info.port);
		    recvfrom(sock, CONFIG_BUF, 7, remote_info.ip, &remote_info.port);

	        wiznet_header.stx = (CONFIG_BUF[0] >> 8) & 0x00FF;
	        wiznet_header.valid = CONFIG_BUF[0] & 0x00FF;
	        wiznet_header.unicast = (CONFIG_BUF[1] >> 8) & 0x00FF;
	        wiznet_header.op_code[0] = CONFIG_BUF[1] & 0x00FF;
	        wiznet_header.op_code[1] = (CONFIG_BUF[2] >> 8) & 0x00FF;
	        wiznet_header.length = (CONFIG_BUF[3] << 8) & 0xFF00;
	        wiznet_header.length |= CONFIG_BUF[2] & 0x00FF;
	        wiznet_header.dummy = 0; // length dummy data

			if(wiznet_header.stx != STX)
				break;

			if(wiznet_header.op_code[1] != WIZNET_REQUEST)
				break;

			switch(wiznet_header.op_code[0] & 0xF0) {
			case DISCOVERY: // Config search operation First process (A0)
				process_discovery(sock, &remote_info, &wiznet_header);
				break;
			case GET_INFO: // Config search operation Second process (B0)
				process_get_info(sock, &remote_info, &wiznet_header);
				break;
			case SET_INFO: // Config Setting operation process (C0)
				process_set_info(sock, &remote_info, &wiznet_header);
				break;
			case FIRMWARE_UPLOAD:
				process_firmware_upload_init(sock, &remote_info, &wiznet_header);
				break;
			case REMOTE_RESET:
				process_remote_reset(sock, &remote_info, &wiznet_header);
				break;
			case FACTORY_RESET:
				process_factory_reset(sock, &remote_info, &wiznet_header);
				break;
			default:
				break;
			}
		}

#if 0
		// flushing remained data..
		getsockopt(sock, SO_REMAINSIZE, &len);
		if(len > 0) {
			do {
				if(len > BUFFER_SIZE) {
					recvfrom(sock, buffer, BUFFER_SIZE, rIP, &rPort);
					len -= BUFFER_SIZE;

				} else {
					recvfrom(sock, buffer, len, rIP, &rPort);
					len = 0;

				}
			} while(len != 0);
		}
#endif
		break;
	case SOCK_CLOSED:
		socket(sock, Sn_MR_UDP, REMOTE_CLIENT_PORT, SF_IO_NONBLOCK);
		break;
	}
}
