csu3_am335x/EVSE/Projects/AW-CCS/Apps/Module_PowerSharing.c

/*
 * Module_PowerSharing.c
 *
 *  Created on: 2020/12/07
 *      Author: foluswen
 */
#include "Module_PowerSharing.h"

struct SysConfigAndInfo		*ShmSysConfigAndInfo;
struct StatusCodeData 		*ShmStatusCodeData;
struct OCPP16Data			*ShmOCPP16Data;
struct Charger				*ShmCharger;
struct POWER_SHARING		*ShmPowerSharing;

//==========================================
// Common routine
//==========================================
int StoreLogMsg(const char *fmt, ...)
{
	char Buf[4096+256];
	char buffer[4096];
	time_t CurrentTime;
	struct tm *tm;
	struct timeval tv;
	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((time_t*)NULL);
	tm=localtime(&CurrentTime);
	gettimeofday(&tv, NULL); // get microseconds, 10^-6


	sprintf(Buf,"echo -n \"[%04d.%02d.%02d %02d:%02d:%02d.%06ld]%s\" >> /Storage/SystemLog/[%04d.%02d]Module_PowerSharingLog",
				tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec,
				buffer,
				tm->tm_year+1900,tm->tm_mon+1);

#ifdef SystemLogMessage
	system(Buf);
#endif

#ifdef ConsloePrintLog
	printf("[%04d.%02d.%02d %02d:%02d:%02d.%06ld]%s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec, 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;
}

void dM(uint8_t *data, uint16_t len, uint8_t isRX)
{
#ifdef DEBUG
	uint8_t output[8192];

	if(isRX)
	{
		DEBUG_INFO("- RX --------------------------------------------\n");
	}
	else
	{
		DEBUG_INFO("- TX --------------------------------------------\n");
	}

	memset(output, 0x00, ARRAY_SIZE(output));
	for(uint16_t idx=0;idx<16;idx++)
		sprintf((char*)output, "%s %02X", output, idx);
	DEBUG_INFO("%s\n", output);
	DEBUG_INFO("-------------------------------------------------\n");

	for(uint16_t idx = 0;idx<len;idx++)
	{
		if((idx%16)>0)
		{
			sprintf((char*)output, "%s %02X", output, data[idx]);
		}
		else
		{
			if(idx != 0)
				DEBUG_INFO("%s\n", output);
			memset(output, 0x00, ARRAY_SIZE(output));
			sprintf((char*)output, "%s %02X", output, data[idx]);
		}
	}
	DEBUG_INFO("%s\n", output);
	DEBUG_INFO("-------------------------------------------------\n");
#endif
}

int isValidCheckSum(struct Message *message)
{
	uint8_t chksum = 0x00;

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

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

uint8_t chksumCal(struct Message *message)
{
	uint8_t chksum=0;

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

	return chksum & 0xff;
}

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

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

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

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

	//Initial ShmCharger
	if ((MeterSMId = shmget(ShmChargerKey, sizeof(struct Charger), 0777)) < 0)
	{
		DEBUG_ERROR("shmget ShmCharger NG\n");
		result = FAIL;
	}
	else if ((ShmCharger = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		DEBUG_ERROR("shmat ShmCharger NG\n");
		result = FAIL;
	}

	//Create ShmPowerSharing
	if ((MeterSMId = shmget(ShmPowerShargingKey, sizeof(struct POWER_SHARING), IPC_CREAT | 0777)) < 0)
	{
		DEBUG_ERROR("shmget ShmPowerShargingKey NG\n");

		result = FAIL;
	}
	else if ((ShmPowerSharing = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		DEBUG_ERROR("shmat ShmPowerShargingKey NG\n");

		result = FAIL;
	}
	memset(ShmPowerSharing,0,sizeof(struct POWER_SHARING));

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		ShmPowerSharing->Connection_Info[idx].socketFd = (idx+1);

    return result;
}

//==========================================
// UDP socket server routine
//==========================================
int udpSocketServerStart(void)
{
	int sockFd;
	struct sockaddr_in servaddr;
	struct sockaddr_in peeraddr;
	socklen_t 			peerlen = sizeof(peeraddr);
	uint8_t 			inputBuffer[2048] = {};
	uint8_t 			outBuffer[2048] = {};
	int16_t				read_size;
	int16_t				tx_size;

	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)
	{
		DEBUG_ERROR("UDP server socket create fail.\n");
		return FAIL;
	}

	if (bind(sockFd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0)
	{
		DEBUG_ERROR("UDP server socket bind fail.\n");
	    return FAIL;
	}
	else
		DEBUG_INFO("UDP server initial.\n");

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

			if(read_size>=6)
			{
				/*
				 *	TODO:
				 *	1. Protocol validation
				 *	2. Protocol message parsing
				 */

				if(TRUE)
				{
					DEBUG_INFO("Receive UDP broadcast command.\n");
					memset(outBuffer, 0x00, ARRAY_SIZE(outBuffer));
					tx_size = 41;

					outBuffer[0] = 0xff;
					outBuffer[1] = 0xff;
					outBuffer[2] = (0x25 << 0x08) & 0xff;
					outBuffer[3] = 0x25 & 0xff;
					outBuffer[4] = 0x00;

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

	return FAIL;
}

//==========================================
// TCP socket server routine
//==========================================
int conn_getDupFd(void)
{
	int result = 0;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(!ShmPowerSharing->Connection_Info[idx].isConnected)
		{
			result = ShmPowerSharing->Connection_Info[idx].socketFd;
			break;
		}
	}

	return result;
}

int conn_register(int socketFd)
{
	int result = FAIL;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(!ShmPowerSharing->Connection_Info[idx].isConnected)
		{
			DEBUG_INFO("Dupfd-%d register to conn-%d.\n", socketFd, idx);
			ShmPowerSharing->Connection_Info[idx].isConnected = TRUE;
			ShmPowerSharing->Connection_Info[idx].socketFd = socketFd;
			ShmPowerSharing->Connection_Info[idx].lastHeartBeatTime = time((time_t*)NULL);
			result = PASS;
			break;
		}
	}

	return result;
}

int conn_reject(int socketFd)
{
	int result = FAIL;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			DEBUG_INFO("Dupfd-%d register from conn_info-%d.\n", socketFd, idx);
			ShmPowerSharing->Connection_Info[idx].isConnected = FALSE;
			ShmPowerSharing->Connection_Info[idx].isCharging = FALSE;
			result = PASS;
			break;
		}
	}

	return result;

}

int conn_getConectedQuantity(void)
{
	int result = 0;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].isConnected)
		{
			result += 1;
		}
	}

	DEBUG_INFO("Connection quantity: %d\n", result);
	ShmPowerSharing->connectedQty = result;
	return result;
}

int conn_getChargingQuantity(void)
{
	int result = 0;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].isCharging)
		{
			result += 1;
		}
	}

	//DEBUG_INFO("Charging quantity: %d\n", result);
	return result;
}

int conn_getTotalAvailableSharingCurrent(void)
{
	int result = 0;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].isConnected)
		{
			result += ShmPowerSharing->Connection_Info[idx].availableSharingCurrent;
		}
	}

	DEBUG_INFO("Total sharing current: %d\n", result);
	return result;
}

int conn_getTotalPresentOutputCurrent(void)
{
	int result = 0;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].isConnected && ShmPowerSharing->Connection_Info[idx].isCharging)
		{
			result += ShmPowerSharing->Connection_Info[idx].presentOutputCurrent;
		}
	}

	DEBUG_INFO("Total actual current: %d\n", result);
	return result;
}

uint16_t conn_querySharingCurrent(int socketFd)
{
	uint16_t result = 0x00;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			//DEBUG_INFO("Dupfd-%d on conn_info-%d query sharing current(0.1A): %d\n", socketFd, idx, ShmPowerSharing->Connection_Info[idx].sharingCurrent);
			result = ShmPowerSharing->Connection_Info[idx].availableSharingCurrent;
			break;
		}
	}

	return result;
}

int conn_updateHeartBeatTime(int socketFd)
{
	int result = FAIL;

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			//DEBUG_INFO("Dupfd-%d register from conn_info-%d update heart beat time.\n", socketFd, idx);
			ShmPowerSharing->Connection_Info[idx].lastHeartBeatTime = time((time_t*)NULL);
			result = PASS;
			break;
		}
	}

	return result;
}

int conn_updatePresentCurrentOutput(int socketFd, uint8_t isCharging, uint16_t outputCurrent)
{
	int result = FAIL;
	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			//DEBUG_INFO("Dupfd-%d on conn_info-%d update actual output current(0.1A): %d\n", socketFd, idx, outputCurrent);
			ShmPowerSharing->Connection_Info[idx].isCharging = isCharging;
			ShmPowerSharing->Connection_Info[idx].presentOutputCurrent = outputCurrent;
			result = PASS;
		}
	}

	return result;
}

int conn_check_loop(void)
{
	for(;;)
	{
		// Check conn heart beat
		for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		{
			if(ShmPowerSharing->Connection_Info[idx].isConnected &&
			   (difftime(time((time_t*)NULL), ShmPowerSharing->Connection_Info[idx].lastHeartBeatTime) > 300))
			{
				DEBUG_INFO("SocketFd-%d heart beat is over 300 seconds.\n", ShmPowerSharing->Connection_Info[idx].socketFd);
				ShmPowerSharing->Connection_Info[idx].isCharging = FALSE;
				ShmPowerSharing->Connection_Info[idx].isConnected = FALSE;
			}
		}

		// Check available power
		for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		{
			if(ShmPowerSharing->Connection_Info[idx].isConnected &&
			   ShmPowerSharing->Connection_Info[idx].isCharging)
			{
				if(ShmPowerSharing->Connection_Info[idx].availableSharingCurrent != ((ShmCharger->gun_info[0].primaryMcuState.rating_current*10) / conn_getChargingQuantity()))
				{
					DEBUG_INFO("Dupfd-%d on conn_info-%d update sharing current(0.1A): %d\n", ShmPowerSharing->Connection_Info[idx].socketFd, idx, ((ShmCharger->gun_info[0].primaryMcuState.rating_current*10) / conn_getChargingQuantity()));
				}
				ShmPowerSharing->Connection_Info[idx].availableSharingCurrent = (ShmCharger->gun_info[0].primaryMcuState.rating_current*10) / conn_getChargingQuantity();
			}
			else
			{
				if(ShmPowerSharing->Connection_Info[idx].availableSharingCurrent != 0)
				{
					DEBUG_INFO("Dupfd-%d on conn_info-%d update sharing current(0.1A): 0\n", ShmPowerSharing->Connection_Info[idx].socketFd, idx);
				}
				ShmPowerSharing->Connection_Info[idx].availableSharingCurrent = 0;
			}
		}

		sleep(1);
	}

	return FAIL;
}

int tcpSocketServerStart(void)
{
	int 				sockFd = 0;
	int 				clientSockFd = 0;
	int					dupFd = 0;
	struct Message		input;
	struct Message		output;
	struct sockaddr_in 	serverInfo, clientInfo;
	socklen_t 			addrlen = sizeof(clientInfo);
	uint16_t 			sharingCurrent=0;

	sockFd = socket(AF_INET , SOCK_STREAM , 0);
	if(sockFd == -1)
	{
		DEBUG_ERROR("TCP service socket create fail.\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);

	if(bind(sockFd, (struct sockaddr *)&serverInfo, sizeof(serverInfo)) < 0)
		DEBUG_ERROR("TCP server socket bind fail.\n");

	if(listen(sockFd, CONNECTION_LIMIT) < 0)
		DEBUG_ERROR("TCP server socket listen fail.\n");
	else
		DEBUG_INFO("Power sharing TCP server initial listen on port %d.\n", LISTEN_PORT_TCP);

	// Main loop
	for(;;)
	{
		clientSockFd = accept(sockFd, (struct sockaddr*) &clientInfo, &addrlen);
		fcntl(clientSockFd, F_SETFD, FD_CLOEXEC);
		DEBUG_INFO("Client connect in.\n");
		DEBUG_INFO("clientSockFd : %d\n", clientSockFd);

		if(clientSockFd > 0)
		{
			if(conn_getConectedQuantity() < CONNECTION_LIMIT)
			{
				// Fork a child process to handle the new conn
				if(fork()==0)
				{
					// Assign socket handle as available handle in conn info pool
					dupFd = dup2(clientSockFd, conn_getDupFd());
					conn_register(dupFd);
					while((input.size = recv(dupFd, input.buffer, sizeof(input.buffer), 0)) > 0)
					{
						dM(input.buffer, input.size, YES);

						if(isValidCheckSum(&input))
						{
							conn_updateHeartBeatTime(dupFd);

							memset(output.buffer, 0x00, ARRAY_SIZE(output.buffer));
							switch(input.buffer[1])
							{
								case SHARING_CMD_QUERY_SHARING:
									sharingCurrent = conn_querySharingCurrent(dupFd);
									output.size = 7;
									output.buffer[0] = 0xaa;
									output.buffer[1] = SHARING_CMD_QUERY_SHARING;
									output.buffer[2] = 0x02;
									output.buffer[3] = 0x00;
									output.buffer[4] = ((sharingCurrent>>0) & 0xff);
									output.buffer[5] = ((sharingCurrent>>8) & 0xff);
									output.buffer[6] = chksumCal(&output);
									break;

								case SHARING_CMD_SYNC_INFO:
									conn_updatePresentCurrentOutput(dupFd, input.buffer[4], (input.buffer[5] | (input.buffer[6]<<0x08)));
									output.size = 6;
									output.buffer[0] = 0xaa;
									output.buffer[1] = SHARING_CMD_SYNC_INFO;
									output.buffer[2] = 0x01;
									output.buffer[3] = 0x00;
									output.buffer[4] = 0x01;
									output.buffer[5] = chksumCal(&output);

									break;

								default:
									DEBUG_WARN("Receive unknown command.\n");
									output.size = 5;
									output.buffer[0] = 0xaa;
									output.buffer[1] = SHARING_CMD_UNKNOWN;
									output.buffer[2] = 0x00;
									output.buffer[3] = 0x00;
									output.buffer[4] = chksumCal(&output);

									break;
							}
						}
						else
						{
							DEBUG_WARN("Receive command check sum error.\n");
							output.size = 5;
							output.buffer[0] = 0xaa;
							output.buffer[1] = SHARING_CMD_CHKSUM_ERROR;
							output.buffer[2] = 0x00;
							output.buffer[3] = 0x00;
							output.buffer[4] = chksumCal(&output);
						}

						dM(output.buffer, output.size, NO);
						send(clientSockFd, output.buffer, output.size, 0);
					}

					if(input.size == 0)
					{
						DEBUG_INFO("Client disconnected.\n");

						conn_reject(dupFd);
						close(dupFd);
						close(clientSockFd);
						fflush(stdout);
					}
					else if(input.size == -1)
					{
						DEBUG_ERROR("Socket recv failed.\n");

						conn_reject(dupFd);
						close(dupFd);
						close(clientSockFd);
						fflush(stdout);
					}

					conn_getConectedQuantity();

					exit(0);
				}
				else
				{
					// if parent, close the socket and go back to listening new requests
					close(clientSockFd);
				}
			}
			else
			{
				DEBUG_WARN("Connection is over limit.\n");
				output.size = 5;
				output.buffer[0] = 0xaa;
				output.buffer[1] = SHARING_CMD_CONNECTION_FULL;
				output.buffer[2] = 0x00;
				output.buffer[3] = 0x00;
				output.buffer[4] = chksumCal(&output);
				send(clientSockFd, output.buffer, output.size, 0);
				close(clientSockFd);
			}
		}

		sleep(1);
	}

	return FAIL;
}

//==========================================
// Client routine
//==========================================
void create_cmd_sync(struct Message *out)
{
	memset(out->buffer, 0, ARRAY_SIZE(out->buffer));

	out->size = 8;
	out->buffer[0] = 0xaa;
	out->buffer[1] = SHARING_CMD_SYNC_INFO;
	out->buffer[2] = 0x03;
	out->buffer[3] = 0x00;
	out->buffer[4] = (ShmSysConfigAndInfo->SysInfo.AcChargingData[0].SystemStatus == SYS_MODE_CHARGING?YES:NO);
	out->buffer[5] = (((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[0].PresentChargingCurrent*10))>>0) & 0xff;
	out->buffer[6] = (((uint16_t)(ShmSysConfigAndInfo->SysInfo.AcChargingData[0].PresentChargingCurrent*10))>>8) & 0xff;
	out->buffer[7] = chksumCal(out);

	dM(out->buffer, out->size, FALSE);
}

void create_cmd_query(struct Message *out)
{
	memset(out->buffer, 0, ARRAY_SIZE(out->buffer));

	out->size = 5;
	out->buffer[0] = 0xaa;
	out->buffer[1] = SHARING_CMD_QUERY_SHARING;
	out->buffer[2] = 0x00;
	out->buffer[3] = 0x00;
	out->buffer[4] = chksumCal(out);
	dM(out->buffer, out->size, FALSE);
}

int tcpSocketClientStart(void)
{
	int 				sockfd;
	struct sockaddr_in 	info;
	struct hostent 		*ghbn;
	struct timeval 		tv;
	uint8_t 			socketEnable;

	struct Message		input;
	struct Message		output;

	uint8_t				cmdIdx;

	bzero(&info,sizeof(info));
	ghbn = gethostbyname((char*)"192.168.10.10");
	info.sin_family = PF_INET;
	info.sin_addr.s_addr = inet_addr(inet_ntoa(*(struct in_addr *)ghbn->h_addr_list[0]));
	info.sin_port = htons(LISTEN_PORT_TCP);
	ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = OFF;
	DEBUG_INFO("Connect to %s:%d\n", inet_ntoa(*(struct in_addr *)ghbn->h_addr_list[0]), LISTEN_PORT_TCP);

	sockfd = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd == -1)
	{
		DEBUG_ERROR("Fail to create a socket.");
		return 0;
	}

	if(connect(sockfd, (struct sockaddr *)&info,sizeof(info)) ==-1)
	{
		DEBUG_ERROR("Connection error.\n");
		ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = OFF;
		socketEnable = OFF;
	}
	else
	{
		DEBUG_INFO("Connect success.\n");
		tv.tv_sec = 0;
		tv.tv_usec = 500000;
		setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
		socketEnable = ON;
	}

	while(socketEnable)
	{
		memset(input.buffer, 0, ARRAY_SIZE(input.buffer));
		if((input.size = recv(sockfd, input.buffer, ARRAY_SIZE(input.buffer), 0)) > 0)
		{
			//DEBUG_INFO("Receive size: %d.\n", input.size);
			dM(input.buffer, input.size, TRUE);

			if(isValidCheckSum(&input))
			{
				switch(input.buffer[1])
				{
					case SHARING_CMD_QUERY_SHARING:
						if(ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent != ((input.buffer[4] | (input.buffer[5] << 8))/10))
						{
							ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent = ((input.buffer[4] | (input.buffer[5] << 8))/10);
							DEBUG_INFO("Update available sharing current(A): %d\n", ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent);
						}
						ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = ON;

						break;

					case SHARING_CMD_SYNC_INFO:
						if(!input.buffer[4])
							DEBUG_INFO("Charger status sync reject.\n");

						break;

					default:
						DEBUG_WARN("Receive unknown command.\n");
						break;
				}
			}
			else
			{
				DEBUG_WARN("Receive command check sum error.\n");
			}
		}
		else if(input.size == 0)
		{
			DEBUG_INFO("Disconnected.\n");
			fflush(stdout);

			socketEnable = OFF;
			ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = OFF;
		}
		else if(input.size == -1)
		{
			switch(cmdIdx)
			{
				case 0:
					create_cmd_sync(&output);
					cmdIdx += 1;
					break;
				default:
					create_cmd_query(&output);
					cmdIdx = 0;
					break;
			}
			send(sockfd, output.buffer, output.size, 0);
		}
		usleep(1000000);
	}
	close(sockfd);

	return FAIL;
}

//==========================================
// Main process
//==========================================
int main(void)
{
	signal(SIGCHLD,SIG_IGN);

	// Initial share memory
	if(InitShareMemory() == FAIL)
	{
		DEBUG_ERROR("InitShareMemory NG\n");

		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory=ON;
		}
		sleep(5);
		return 0;
	}

	// Enable server if rotary switch not slave mode
	if((ShmCharger->gun_info[0].primaryMcuState.rotatory_switch != SWITCH_F_SLAVE) &&
	   (AC_QUANTITY==1?TRUE:(ShmCharger->gun_info[1].primaryMcuState.rotatory_switch != SWITCH_F_SLAVE)))
	{
		// UDP socket server start
		/*
		if(fork() == 0)
		{
			if(udpSocketServerStart() == FAIL)
			{
				DEBUG_ERROR("UDP socket server down.\n");
				return 0;
			}
		}*/

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

		// Connection check loop
		if(fork() == 0)
		{
			if(conn_check_loop() == FAIL)
			{
				DEBUG_ERROR("Connection check loop fail.\n");
				return 0;
			}
		}
	}

	sleep(10);
	for(;;)
	{
		// Slave logic
		tcpSocketClientStart();

		usleep(100000);
	}

	return FAIL;
}