Test/EVSE/Modularization/Module_PowerSharing.c

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

//-----------------------------------------------------------------------------
//#define MODPS_FW_VER_TYPE_FORMAL	//V0.XX
#define MODPS_FW_VER_TYPE_TEST		//T0.XX

#define MODPS_FW_VER_NUM	(1)    //V/T0.01
#define MODPS_FW_DATE		("20220730")

//-----------------------------------------------------------------------------
#if (MODPS_FW_VER_NUM >= 1)
#define MODIFY_MODPS_BALANCE_CHECK_LOOP
#define FUNC_MODPS_APPEND_LOG
#endif

//-----------------------------------------------------------------------------

char Version_And_Date[2][10] = { 0 };

void Init_FW_Info(void)
{
#ifdef MODPS_FW_VER_TYPE_FORMAL
    sprintf(Version_And_Date[0], "V%4.2f", MODPS_FW_VER_NUM * 0.01);
#endif

#ifdef MODPS_FW_VER_TYPE_TEST
    sprintf(Version_And_Date[0], "T%4.2f", MODPS_FW_VER_NUM * 0.01);
#endif

    sprintf(Version_And_Date[1], "%s", MODPS_FW_DATE);
}

struct SysConfigAndInfo		*ShmSysConfigAndInfo;
struct StatusCodeData 		*ShmStatusCodeData;
struct OCPP16Data			*ShmOCPP16Data;
struct OCPP20Data			*ShmOCPP20Data;
struct POWER_SHARING		*ShmPowerSharing;
ParsingRatedCur modelnameInfo 	= {0};
uint8_t gunType[4] 				= {0};
uint8_t gunTotalNumber			= 0;

//==========================================
// 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.%03ld]%s\" >> /Storage/SystemLog/[%04d.%02d]Module_PowerSharingServerLog",
				tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec/1000,
				buffer,
				tm->tm_year+1900,tm->tm_mon+1);

#ifdef SystemLogMessage
	system(Buf);
#endif

#ifdef ConsloePrintLog
	printf("[%04d.%02d.%02d %02d:%02d:%02d.%03ld]%s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec/1000, buffer);
#endif

	return rc;
}

int StoreLogMsgClient(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.%03ld]%s\" >> /Storage/SystemLog/[%04d.%02d]Module_PowerSharingClientLog",
				tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec/1000,
				buffer,
				tm->tm_year+1900,tm->tm_mon+1);

#ifdef SystemLogMessage
	system(Buf);
#endif

#ifdef ConsloePrintLog
	printf("[%04d.%02d.%02d %02d:%02d:%02d.%03ld]%s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,tv.tv_usec/1000, buffer);
#endif

	return rc;
}

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

long long DiffTimebWithNow(struct timeb ST)
{
	//return milli-second
	struct timeb ET;
	long long StartTime,StopTime;

	ftime(&ET);
	StartTime=(long long)ST.time;
	StopTime=(long long)ET.time;
	return ((StopTime-StartTime)*1000) + (ET.millitm-ST.millitm);
}

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 refreshStartTimer(struct timespec *timer)
{
	clock_gettime(CLOCK_MONOTONIC, timer);
}

int getDiffSecNow(struct timespec timer)
{
	struct timespec timerNow;

	clock_gettime(CLOCK_MONOTONIC, &timerNow);

	return (int)((((unsigned long)(timerNow.tv_sec - timer.tv_sec) * 1000) + ((unsigned long)((timerNow.tv_nsec / 1000000) - (timer.tv_nsec / 1000000))))/1000);
}

int getDiffSecBetween(struct timespec start, struct timespec end)
{
	return (int)((((unsigned long)(end.tv_sec - start.tv_sec) * 1000) + ((unsigned long)((end.tv_nsec / 1000000) - (start.tv_nsec / 1000000))))/1000);
}

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

	if(isRX)
	{
		DEBUG_INFO_CLIENT("- RX --------------------------------------------\n");
	}
	else
	{
		DEBUG_INFO_CLIENT("- 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_CLIENT("%s\n", output);
	DEBUG_INFO_CLIENT("-------------------------------------------------\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_CLIENT("%s\n", output);
	DEBUG_INFO_CLIENT("-------------------------------------------------\n");
#endif
}

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

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

	return ((chksum == message->buffer[((message->buffer[1]+3)>ARRAY_SIZE(message->buffer)?ARRAY_SIZE(message->buffer):(message->buffer[1]+3))]) ? PASS : FAIL);
}

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

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

	return chksum & 0xff;
}

uint8_t ocpp_get_maxcharging_profileId()
{
	uint8_t result = 0;

	if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_16)
	{
		result = ShmOCPP16Data->MaxChargingProfile.ChargingProfileId;
	}
	else if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_20)
	{
		result = ShmOCPP20Data->MaxChargingProfile.id;
	}

	return result;
}

int getMaxScheduleStart()
{
	int result = -1;
	struct tm tmScheduleStart;;
	struct timeb tbScheduleStart;

	if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_16)
	{
		if((sscanf((char*)ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.StartSchedule, "%4d-%2d-%2dT%2d:%2d:%2d", &tmScheduleStart.tm_year, &tmScheduleStart.tm_mon, &tmScheduleStart.tm_mday, &tmScheduleStart.tm_hour, &tmScheduleStart.tm_min, &tmScheduleStart.tm_sec) == 6))
		{
			tmScheduleStart.tm_year -= 1900;
			tmScheduleStart.tm_mon -= 1;
			tbScheduleStart.time = mktime(&tmScheduleStart);
			tbScheduleStart.millitm = 0;

			result = DiffTimebWithNow(tbScheduleStart)/1000;
			//DEBUG_INFO("Max schedule start compare Now(seconds): %d\n", result);
		}
		else
		{
			DEBUG_WARN("Max schedule start date parsing error.\n");
		}
	}
	else if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_20)
	{
		if((sscanf((char*)ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].startSchedule, "%4d-%2d-%2dT%2d:%2d:%2d", &tmScheduleStart.tm_year, &tmScheduleStart.tm_mon, &tmScheduleStart.tm_mday, &tmScheduleStart.tm_hour, &tmScheduleStart.tm_min, &tmScheduleStart.tm_sec) == 6))
		{
			tmScheduleStart.tm_year -= 1900;
			tmScheduleStart.tm_mon -= 1;
			tbScheduleStart.time = mktime(&tmScheduleStart);
			tbScheduleStart.millitm = 0;

			result = DiffTimebWithNow(tbScheduleStart)/1000;
			//DEBUG_INFO("Max schedule start compare Now(seconds): %d\n", result);
		}
		else
		{
			DEBUG_WARN("Max schedule start date parsing error.\n");
		}
	}

	return result;
}

uint16_t checkChargingProfileLimit()
{
	/*
	 * 	TODO:
	 * 	1. Default capacity check
	 */
	uint16_t targetMaxCurrent = ((ShmSysConfigAndInfo->SysConfig.PowerSharingCapacityPower==0)?(modelnameInfo.ratedPower/220):(ShmSysConfigAndInfo->SysConfig.PowerSharingCapacityPower/220));

	if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_16)
	{
		// Get max charging profile limit
		if((ocpp_get_maxcharging_profileId() > 0))
		{
			// Checking limitation
			for(uint8_t idx_period=0;idx_period<ARRAY_SIZE(ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod);idx_period++)
			{
				if((getMaxScheduleStart() >= ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod[idx_period].StartPeriod) &&
				   ((idx_period == 0) || (ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod[idx_period].StartPeriod > 0))
				  )
				{
					targetMaxCurrent = (mystrcmp((char*)ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingRateUnit,"W")==PASS?ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod[idx_period].Limit/(220*ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod[idx_period].NumberPhases):ShmOCPP16Data->MaxChargingProfile.ChargingSchedule.ChargingSchedulePeriod[idx_period].Limit);
					//DEBUG_INFO("targetMaxCurrent on period[%d]: %d\n", idx_period, targetMaxCurrent);
				}
				else
					break;
			}
		}
	}
	else if(ShmSysConfigAndInfo->SysInfo.OcppRunningVer == OCPP_RUNNING_VERSION_20)
	{
		// Get max charging profile limit
		if((ocpp_get_maxcharging_profileId() > 0))
		{
			// Checking limitation
			for(uint8_t idx_period=0;idx_period<ARRAY_SIZE(ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod);idx_period++)
			{
				if((getMaxScheduleStart() >= ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod[idx_period].startPeriod) &&
				   ((idx_period == 0) || (ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod[idx_period].startPeriod > 0))
				  )
				{
					targetMaxCurrent = (mystrcmp((char*)ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingRateUnit,"W")==PASS?ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod[idx_period].limit/(220*ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod[idx_period].numberPhases):ShmOCPP20Data->MaxChargingProfile.chargingSchedule[0].chargingSchedulePeriod[idx_period].limit);
					//DEBUG_INFO("targetMaxCurrent on period[%d]: %d\n", idx_period, targetMaxCurrent);
				}
				else
					break;
			}
		}
	}

	return targetMaxCurrent;
}

void getClientp(int socketFd)
{
	char clientip[20];
	struct sockaddr_in addr;
	socklen_t addr_size = sizeof(struct sockaddr_in);

	getpeername(socketFd, (struct sockaddr *)&addr, &addr_size);
	strcpy(clientip, inet_ntoa(addr.sin_addr));

	DEBUG_INFO("Client IP address: %s\n", clientip);
}

//==========================================
// 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 ShmOCPP20Data
	if ((MeterSMId = shmget(ShmOcpp20ModuleKey, sizeof(struct OCPP20Data),  0777)) < 0)
	{
		DEBUG_ERROR("shmget ShmOCPP20Data NG");
		result = FAIL;
	}
	else if ((ShmOCPP20Data = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		DEBUG_ERROR("shmat ShmOCPP20Data NG");
		result = FAIL;
	}
	else
	{}

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

	// Model name parsing
	if(RatedCurrentParsing((char*)ShmSysConfigAndInfo->SysConfig.ModelName, &modelnameInfo) != -1)
	{
		DEBUG_INFO("Model name rated power: %d\n", modelnameInfo.ratedPower);
		if((ShmSysConfigAndInfo->SysConfig.ModelName[0]=='D') &&
		   ((ShmSysConfigAndInfo->SysConfig.ModelName[1]=='B') ||
			(ShmSysConfigAndInfo->SysConfig.ModelName[1]=='K') ||
			(ShmSysConfigAndInfo->SysConfig.ModelName[1]=='O'))
		   ) // 'D' means DC
		{
			// DO series
			for(int gun_index=0; gun_index<GENERAL_GUN_QUANTITY ; gun_index++)
			{
				gunTotalNumber += 1;
				gunType[gun_index] = GUN_TYPE_DO;

				switch(modelnameInfo.ParsingInfo[gun_index].GunType)
				{
					case Gun_Type_Chademo:
						DEBUG_INFO("Gun-%02d type: Cabinet CHAdeMO\n", gun_index);
						break;
					case Gun_Type_CCS_2:
						DEBUG_INFO("Gun-%02d type: Cabinet CCS\n", gun_index);
						break;
					case Gun_Type_GB:
						DEBUG_INFO("Gun-%02d type: Cabinet GBT\n", gun_index);
						break;
					case Gun_Type_AC:
						DEBUG_INFO("Gun-%02d type: Cabinet AC\n", gun_index);
						break;
					default:
						DEBUG_WARN("Gun-%02d type: Cabinet unknown\n", gun_index);
						break;
				}
			}
		}
		else
		{
			for(int gun_index=0;gun_index<modelnameInfo.GetGunCount;gun_index++)
			{
				gunTotalNumber += 1;

				switch(modelnameInfo.ParsingInfo[gun_index].GunType)
				{
					case Gun_Type_Chademo:
						gunType[gun_index] = GUN_TYPE_CHAdeMO;
						DEBUG_INFO("Gun-%02d type: CHAdeMO\n", gun_index);
						break;
					case Gun_Type_CCS_2:
						gunType[gun_index] = GUN_TYPE_CCS;
						DEBUG_INFO("Gun-%02d type: CCS\n", gun_index);
						break;
					case Gun_Type_GB:
						gunType[gun_index] = GUN_TYPE_GBT;
						DEBUG_INFO("Gun-%02d type: GBT\n", gun_index);
						break;
					case Gun_Type_AC:
						gunType[gun_index] = GUN_TYPE_AC;
						DEBUG_INFO("Gun-%02d type: AC\n", gun_index);
						break;
					default:
						DEBUG_WARN("Gun-%02d type: Unknown\n", gun_index);
						break;
				}
			}
		}
	}
	else
	{
		DEBUG_ERROR("Model name parsing fail.\n");
		result = FAIL;
	}

    return result;
}

//==========================================
// 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].isSocketConnected)
		{
			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].isSocketConnected)
		{
			DEBUG_INFO("Dupfd-%d register to conn-%d.\n", socketFd, idx);
			ShmPowerSharing->Connection_Info[idx].isSocketConnected = TRUE;
			ShmPowerSharing->Connection_Info[idx].socketFd = socketFd;
			refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_HEARTBEAT]);
			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].isSocketConnected = FALSE;

			for(uint8_t gun_index=0;gun_index<ARRAY_SIZE(ShmPowerSharing->Connection_Info[idx].connectorInfo);gun_index++)
				ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected = 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].isSocketConnected)
		{
			result += 1;
		}
	}

	if(ShmPowerSharing->connectedConnectionQty != result)
	{
		DEBUG_INFO("Connection quantity: %d\n", result);
		ShmPowerSharing->connectedConnectionQty = result;
	}

	return result;
}

int conn_getConectedConnector(void)
{
	int result = 0;

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

	if(ShmPowerSharing->connectedConnectorQty != result)
	{
		DEBUG_INFO("Connected connector quantity: %d\n", result);
		ShmPowerSharing->connectedConnectorQty = result;
	}

	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);
			refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_HEARTBEAT]);
			result = PASS;
			break;
		}
	}

	return result;
}

struct timespec conn_getStatusStarttime(int socketFd)
{
	struct timespec result;
	refreshStartTimer(&result);

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

	return result;
}

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

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_GET_STATUS]);
			result = PASS;
			break;
		}
	}

	return result;
}

struct timespec conn_setCapacityStarttime(int socketFd)
{
	struct timespec result;
	refreshStartTimer(&result);

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

	return result;
}

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

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_SET_CAPACITY]);
			result = PASS;
			break;
		}
	}

	return result;
}

int conn_update_status(int socketFd, Connector_Info *connectorInfo, uint8_t connectorCount)
{
	int result = FAIL;
	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			ShmPowerSharing->Connection_Info[idx].connectorCount = connectorCount;
			for(uint8_t gun_index;gun_index<connectorCount;gun_index++)
			{
				if(!ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected && connectorInfo[gun_index].isGunConnected)
				{
					ShmPowerSharing->isDetectNewConnected = YES;
				}

				if((ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected != connectorInfo[gun_index].isGunConnected?YES:NO) ||
				   (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent != connectorInfo[gun_index].availableSharingCurrent) ||
				   (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent != connectorInfo[gun_index].presentOutputCurrent))
				{
					switch(connectorInfo[gun_index].connectorType)
					{
						case CONNECTOR_TYPE_AC:
							DEBUG_INFO("Conn-%d gun_%d type: AC signle phase.\n", idx, gun_index);
							break;
						case CONNECTOR_TYPE_AC_THREE:
							DEBUG_INFO("Conn-%d gun_%d type: AC three phase.\n", idx, gun_index);
							break;
						case CONNECTOR_TYPE_DC:
							DEBUG_INFO("Conn-%d gun_%d type: DC.\n", idx, gun_index);
							break;
					}

					DEBUG_INFO("Conn-%d gun_%d connected: %s\n", idx, gun_index, (connectorInfo[gun_index].isGunConnected?"Yes":"No"));
					DEBUG_INFO("Conn-%d gun_%d available current: %d\n", idx, gun_index, connectorInfo[gun_index].availableSharingCurrent);
					DEBUG_INFO("Conn-%d gun_%d preset output current: %d\n", idx, gun_index, connectorInfo[gun_index].presentOutputCurrent);
					DEBUG_INFO("==================================\n");
				}

				ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType = connectorInfo[gun_index].connectorType;
				ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected = connectorInfo[gun_index].isGunConnected;
				ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent = connectorInfo[gun_index].presentOutputCurrent;
			}

			result = PASS;
		}
	}

	return result;
}

int conn_getOnHandCurrent(void)
{
	int result = 0;
	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].isSocketConnected)
		{
			for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
			{
				result += (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType == CONNECTOR_TYPE_AC_THREE?
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent*3:
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent);
			}
		}
	}
	result = checkChargingProfileLimit() - result;

	if(ShmPowerSharing->onHandCurrent != result)
	{
		DEBUG_INFO("Total available current: %d\n", result);
		ShmPowerSharing->onHandCurrent = result;
	}

	return result;
}

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

	out->size = 4;
	out->buffer[0] = 0x55;
	out->buffer[1] = 0x00;
	out->buffer[2] = SHARING_CMD_GET_STATUS;
	out->buffer[out->size-1] = chksumCal(out);

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

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

	for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
	{
		if(ShmPowerSharing->Connection_Info[idx].socketFd == socketFd)
		{
			out->size = ShmPowerSharing->Connection_Info[idx].connectorCount*2 + 5;
			out->buffer[0] = 0x55;
			out->buffer[1] = ShmPowerSharing->Connection_Info[idx].connectorCount*2 + 1;
			out->buffer[2] = SHARING_CMD_SET_CAPACITY;

			out->buffer[3] = ShmPowerSharing->Connection_Info[idx].connectorCount;
			for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
			{
				out->buffer[4+(gun_index*2)] = (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent>>8)&0xff;
				out->buffer[4+(gun_index*2)+1] = (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent>>0)&0xff;
			}
		}
	}
	out->buffer[out->size-1] = chksumCal(out);
	//dM(out->buffer, out->size, FALSE);
}

int tcpSocketServer(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);

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


#ifdef FUNC_MODULE_POWER_SHARING_APPEND_LOG
	DEBUG_INFO("==========[ tcpSocketServer begin ]==========\n");
#endif

	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);
		getClientp(clientSockFd);

		if(clientSockFd > 0)
		{
			if(ShmPowerSharing->connectedConnectionQty < CONNECTION_LIMIT)
			{
				// Fork a child process to handle the new conn
				if(fork()==0)
				{
					uint8_t idxStep = 0;
					uint8_t socketEnable = YES;
					struct timeval	tv;
					tv.tv_sec = 0;
					tv.tv_usec = 500000;
					setsockopt(clientSockFd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
					// Assign socket handle as available handle in conn info pool
					dupFd = dup2(clientSockFd, conn_getDupFd());
					conn_register(dupFd);

					while(socketEnable)
					{
						if((input.size = recv(dupFd, input.buffer, sizeof(input.buffer), 0)) > 0)
						{
							//dM(input.buffer, input.size, YES);

							if(isValidCheckSum(&input))
							{
								Connector_Info connectorInfo[4];
								conn_updateHeartBeatTime(dupFd);

								memset(output.buffer, 0x00, ARRAY_SIZE(output.buffer));
								switch(input.buffer[2])
								{
									case SHARING_CMD_GET_STATUS:
										for(uint8_t gun_index=0;gun_index<input.buffer[3];gun_index++)
										{
											connectorInfo[gun_index].connectorType = input.buffer[4+(gun_index*6)];
											if((connectorInfo[gun_index].connectorType == CONNECTOR_TYPE_AC) || (connectorInfo[gun_index].connectorType == CONNECTOR_TYPE_AC_THREE))
											{
												connectorInfo[gun_index].isGunConnected = ((2<=input.buffer[4+(gun_index*6)+1]) && (input.buffer[4+(gun_index*6)+1]<=7)?TRUE:FALSE);
											}
											else
											{
												connectorInfo[gun_index].isGunConnected = input.buffer[4+(gun_index*6)+1];
											}
											connectorInfo[gun_index].availableSharingCurrent = (input.buffer[4+(gun_index*6)+2]<<8) | input.buffer[4+(gun_index*6)+3];
											connectorInfo[gun_index].presentOutputCurrent = (input.buffer[4+(gun_index*6)+4]<<8) | input.buffer[4+(gun_index*6)+5];
										}

										conn_update_status(dupFd, &connectorInfo[0], input.buffer[3]);

										break;

									case SHARING_CMD_SET_CAPACITY:
										if(!input.buffer[3])
											DEBUG_INFO("Set connection-%d available current fail \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("Client disSocketConnected.\n");
							conn_reject(dupFd);
							socketEnable = NO;
							close(dupFd);
							close(clientSockFd);
							fflush(stdout);
						}
						else if(input.size == -1)
						{
							// Server slave handler
							switch(idxStep)
							{
								case 0:
									if((getDiffSecNow(conn_getStatusStarttime(dupFd)) >= INTERVAL_SPEC_POLLING_CMD))
									{
										create_cmd_getStatus(&output);
										conn_getStatusStarttimeUpdate(dupFd);
										send(clientSockFd, output.buffer, output.size, 0);
									}

									idxStep++;
									break;
								default:
									if((getDiffSecNow(conn_setCapacityStarttime(dupFd)) >= INTERVAL_SPEC_POLLING_CMD))
									{
										create_cmd_SetAvailableCurrent(&output, dupFd);
										conn_setCapacityStarttimeUpdate(dupFd);
										send(clientSockFd, output.buffer, output.size, 0);
									}

									idxStep = 0;
									break;
							}
						}
					}


					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 = 4;
				output.buffer[0] = 0x55;
				output.buffer[1] = 0x00;
				output.buffer[2] = SHARING_CMD_CONNECTION_FULL;
				output.buffer[3] = chksumCal(&output);
				send(clientSockFd, output.buffer, output.size, 0);
				close(clientSockFd);
			}
		}

		usleep(500000);
	}

	return FAIL;
}

//==========================================
// Client routine
//==========================================
int tcpSocketClient(void)
{
	int 				sockfd;
	struct sockaddr_in 	info;
	struct hostent 		*ghbn;
	struct timeval 		tv;
	uint8_t 			socketEnable;
	uint8_t				cntSocketErr;

	struct Message		input;
	struct Message		output;

	bzero(&info,sizeof(info));
	if(ShmSysConfigAndInfo->SysConfig.isEnableLocalPowerSharing == 1)
	{
		ghbn = gethostbyname((char*)"127.0.0.1");
	}
	else
	{
		if(strlen((char*)ShmSysConfigAndInfo->SysConfig.PowerSharingServerIP) > 0)
			ghbn = gethostbyname((char*)ShmSysConfigAndInfo->SysConfig.PowerSharingServerIP);
		else
			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_CLIENT("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_CLIENT("Fail to create a socket.");
		return 0;
	}

	if(connect(sockfd, (struct sockaddr *)&info,sizeof(info)) ==-1)
	{
		DEBUG_ERROR_CLIENT("Connection error.\n");
		ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = OFF;
		socketEnable = OFF;
	}
	else
	{
		DEBUG_INFO_CLIENT("Connect success.\n");
		tv.tv_sec = 0;
		tv.tv_usec = 500000;
		setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
		socketEnable = ON;
		cntSocketErr = 0;
		ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = 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, YES);

			if(isValidCheckSum(&input))
			{
				switch(input.buffer[2])
				{
					case SHARING_CMD_GET_STATUS:
						output.size = 5 + (gunTotalNumber*6);
						output.buffer[0] = 0x55;
						output.buffer[1] = 1 + (gunTotalNumber*6);
						output.buffer[2] = input.buffer[2];
						output.buffer[3] = gunTotalNumber;
						for(uint8_t gun_index=0;gun_index<gunTotalNumber;gun_index++)
						{
							uint8_t tempIndex;

							output.buffer[4+(gun_index*6)] = (gunType[gun_index]==GUN_TYPE_AC?(ShmSysConfigAndInfo->SysConfig.AcPhaseCount==3?CONNECTOR_TYPE_AC_THREE:CONNECTOR_TYPE_AC):CONNECTOR_TYPE_DC);

							switch(gunType[gun_index])
							{
								case GUN_TYPE_CHAdeMO:
									if(ShmSysConfigAndInfo->SysConfig.ModelName[8] != '0')
									{
										tempIndex = ((gun_index==2) ? 1: 0);
									}
									else
									{
										tempIndex = gun_index;
									}

									for (int index = 0; index < CHAdeMO_QUANTITY; index++)
									{
										if (ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index].Index == tempIndex)
										{
											output.buffer[4+(gun_index*6)+1] = ShmSysConfigAndInfo->SysInfo.ChademoChargingData[tempIndex].ConnectorPlugIn;
											output.buffer[4+(gun_index*6)+2] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>8)&0xff;
											output.buffer[4+(gun_index*6)+3] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>0)&0xff;
											output.buffer[4+(gun_index*6)+4] = ((int)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[gun_index].PresentChargingPower/220)>>8)&0xff;
											output.buffer[4+(gun_index*6)+5] = ((int)(ShmSysConfigAndInfo->SysInfo.ChademoChargingData[gun_index].PresentChargingPower/220)>>0)&0xff;
										}
									}
									break;

								case GUN_TYPE_CCS:
									if(ShmSysConfigAndInfo->SysConfig.ModelName[8] != '0')
									{
										tempIndex = ((gun_index==2) ? 1: 0);
									}
									else
									{
										tempIndex = gun_index;
									}

									for (int index = 0; index < CCS_QUANTITY; index++)
									{
										if (ShmSysConfigAndInfo->SysInfo.CcsChargingData[index].Index == tempIndex)
										{
											output.buffer[4+(gun_index*6)+1] = ShmSysConfigAndInfo->SysInfo.CcsChargingData[tempIndex].ConnectorPlugIn;
											output.buffer[4+(gun_index*6)+2] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>8)&0xff;
											output.buffer[4+(gun_index*6)+3] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>0)&0xff;
											output.buffer[4+(gun_index*6)+4] = ((int)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[gun_index].PresentChargingPower/220)>>8)&0xff;
											output.buffer[4+(gun_index*6)+5] = ((int)(ShmSysConfigAndInfo->SysInfo.CcsChargingData[gun_index].PresentChargingPower/220)>>0)&0xff;
										}
									}
									break;

								case GUN_TYPE_GBT:
									if(ShmSysConfigAndInfo->SysConfig.ModelName[8] != '0')
									{
										tempIndex = ((gun_index==2) ? 1: 0);
									}
									else
									{
										tempIndex = gun_index;
									}

									for (int index = 0; index < GB_QUANTITY; index++)
									{
										if (ShmSysConfigAndInfo->SysInfo.GbChargingData[index].Index == tempIndex)
										{
											output.buffer[4+(gun_index*6)+1] = ShmSysConfigAndInfo->SysInfo.GbChargingData[tempIndex].ConnectorPlugIn;
											output.buffer[4+(gun_index*6)+2] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>8)&0xff;
											output.buffer[4+(gun_index*6)+3] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>0)&0xff;
											output.buffer[4+(gun_index*6)+4] = ((int)(ShmSysConfigAndInfo->SysInfo.GbChargingData[gun_index].PresentChargingPower/220)>>8)&0xff;
											output.buffer[4+(gun_index*6)+5] = ((int)(ShmSysConfigAndInfo->SysInfo.GbChargingData[gun_index].PresentChargingPower/220)>>0)&0xff;
										}
									}
									break;

								case GUN_TYPE_DO:
									tempIndex = gun_index;

									for (int index = 0; index < GENERAL_GUN_QUANTITY; index++)
									{
										if (ShmSysConfigAndInfo->SysInfo.ConnectorInfo[index].GeneralChargingData.Index == tempIndex)
										{
											output.buffer[4+(gun_index*6)+1] = ShmSysConfigAndInfo->SysInfo.ConnectorInfo[tempIndex].GeneralChargingData.ConnectorPlugIn;
											output.buffer[4+(gun_index*6)+2] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>8)&0xff;
											output.buffer[4+(gun_index*6)+3] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>0)&0xff;
											output.buffer[4+(gun_index*6)+4] = ((int)(ShmSysConfigAndInfo->SysInfo.ConnectorInfo[gun_index].GeneralChargingData.PresentChargingPower/220)>>8)&0xff;
											output.buffer[4+(gun_index*6)+5] = ((int)(ShmSysConfigAndInfo->SysInfo.ConnectorInfo[gun_index].GeneralChargingData.PresentChargingPower/220)>>0)&0xff;
										}
									}
									break;

								case GUN_TYPE_AC:
									if(ShmSysConfigAndInfo->SysConfig.ModelName[0]=='D')
									{
										tempIndex = 2;
									}
									else
									{
										tempIndex = gun_index;
									}
									for (int index = 0; index < AC_QUANTITY; index++)
									{
										if (ShmSysConfigAndInfo->SysInfo.AcChargingData[index].Index == tempIndex)
										{
											output.buffer[4+(gun_index*6)+1] = ShmSysConfigAndInfo->SysInfo.AcChargingData[tempIndex].PilotState;
											output.buffer[4+(gun_index*6)+2] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>8)&0xff;
											output.buffer[4+(gun_index*6)+3] = (ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]>>0)&0xff;
											output.buffer[4+(gun_index*6)+4] = ((int)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargingCurrent>>8)&0xff;
											output.buffer[4+(gun_index*6)+5] = ((int)ShmSysConfigAndInfo->SysInfo.AcChargingData[gun_index].PresentChargingCurrent>>0)&0xff;
										}
									}

									break;
								default:
									break;
							}
						}

						output.buffer[output.size-1] = chksumCal(&output);

						break;

					case SHARING_CMD_SET_CAPACITY:
						output.size = 5;
						output.buffer[0] = 0x55;
						output.buffer[1] = 0x01;
						output.buffer[2] = input.buffer[2];
						output.buffer[3] = 0x01;
						output.buffer[output.size-1] = chksumCal(&output);

						for(uint8_t gun_index=0;gun_index<input.buffer[3];gun_index++)
						{
							if(ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index] != ((input.buffer[4+(gun_index*2)]<<8) | (input.buffer[4+(gun_index*2)+1])))
							{
								ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index] = ((input.buffer[4+(gun_index*2)]<<8) | (input.buffer[4+(gun_index*2)+1]));
								DEBUG_INFO_CLIENT("Get available current from server: %d\n", ShmSysConfigAndInfo->SysInfo.localSharingInfo.AvailableShargingCurrent[gun_index]);
							}
						}

						break;

					default:
						DEBUG_WARN_CLIENT("Receive unknown command.\n");
						output.size = 4;
						output.buffer[0] = 0x55;
						output.buffer[1] = 0x00;
						output.buffer[2] = SHARING_CMD_UNKNOWN;
						output.buffer[3] = chksumCal(&output);
						break;
				}
			}
			else
			{
				DEBUG_WARN_CLIENT("Receive command check sum error.\n");
				output.size = 4;
				output.buffer[0] = 0x55;
				output.buffer[1] = 0x00;
				output.buffer[2] = SHARING_CMD_CHKSUM_ERROR;
				output.buffer[3] = chksumCal(&output);
			}

			dM(output.buffer, output.size, NO);
			send(sockfd, output.buffer, output.size, 0);
		}
		else if(input.size == 0)
		{
			DEBUG_INFO_CLIENT("DisSocketConnected.\n");
			fflush(stdout);

			socketEnable = OFF;
			ShmSysConfigAndInfo->SysInfo.localSharingInfo.isConnectedSharingServer = OFF;
		}
		else if(input.size == -1)
		{
			if(cntSocketErr > 5)
			{
				socketEnable = OFF;
				DEBUG_ERROR_CLIENT("Socket error occur\n");
			}
			else
			{
				cntSocketErr++;
			}
		}
		usleep(500000);
	}
	close(sockfd);

	return FAIL;
}

//==========================================
// Local loading balance check
//==========================================
#ifdef MODIFY_MODPS_BALANCE_CHECK_LOOP

Connector_Info* GetIsetMaxGun(void)
{
	struct CONNECTION_INFO* pDev = NULL;
	Connector_Info* pGun = NULL;
	uint16_t* pIset = NULL;
	uint16_t* pIout = NULL;
	uint16_t Imin = 0;
	uint16_t IsetMax = 6;
	Connector_Info* pGunIsetMax = NULL;

	for (uint8_t i = 0; i < CONNECTION_LIMIT; i++)
	{
		pDev = &ShmPowerSharing->Connection_Info[i];
		if (!pDev->isSocketConnected)
		{
			continue;
		}
		for (uint8_t j = 0; j < pDev->connectorCount; j++)
		{
			pGun = &pDev->connectorInfo[j];
			if (pGun->isGunConnected)
			{
				pIset = &pGun->availableSharingCurrent;
				pIout = &pGun->presentOutputCurrent;
				Imin = pGun->connectorType==CONNECTOR_TYPE_DC ? SHARE_MIN_DC : SHARE_MIN_AC;
				if (*pIset > Imin && *pIout > Imin && *pIset > IsetMax)
				{
					IsetMax = *pIset;
					pGunIsetMax = pGun;
				}
			}
		}
	}

//	if (pGunIsetMax)
//	{
//		DEBUG_INFO("GunIsetMax: %d / %d (%d)\n", pGunIsetMax->presentOutputCurrent,
//				pGunIsetMax->availableSharingCurrent, pGunIsetMax->connectorType);
//	}
	return pGunIsetMax;
}

Connector_Info* GetIsetMinGun(void)
{
	struct CONNECTION_INFO* pDev = NULL;
	Connector_Info* pGun = NULL;
	uint16_t* pIset = NULL;
	uint16_t* pIout = NULL;
	uint16_t Imin = 0;
	uint16_t IsetMin = 1000;
	Connector_Info* pGunIsetMin = NULL;

	for (uint8_t i = 0; i < CONNECTION_LIMIT; i++)
	{
		pDev = &ShmPowerSharing->Connection_Info[i];
		if (!pDev->isSocketConnected)
		{
			continue;
		}
		for (uint8_t j = 0; j < pDev->connectorCount; j++)
		{
			pGun = &pDev->connectorInfo[j];
			if (pGun->isGunConnected)
			{
				pIset = &pGun->availableSharingCurrent;
				pIout = &pGun->presentOutputCurrent;
				Imin = pGun->connectorType==CONNECTOR_TYPE_DC ? SHARE_MIN_DC : SHARE_MIN_AC;
				if (*pIset >= Imin && *pIout >= Imin && *pIset < IsetMin)
				{
					IsetMin = *pIset;
					pGunIsetMin = pGun;
				}
			}
		}
	}

//	if (pGunIsetMin)
//	{
//		DEBUG_INFO("GunIsetMin: %d / %d (%d)\n", pGunIsetMin->presentOutputCurrent,
//				pGunIsetMin->availableSharingCurrent, pGunIsetMin->connectorType);
//	}
	return pGunIsetMin;
}

int balance_check_loop(void)
{
	DEBUG_INFO("==========[ balance_check_loop begin ]==========\n");
	for(;;)
	{
		// Get connection info
		conn_getConectedQuantity();
		conn_getOnHandCurrent();
		conn_getConectedConnector();

		// Check conn heart beat
		for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		{
			if(ShmPowerSharing->Connection_Info[idx].isSocketConnected &&
			   (getDiffSecNow(ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_HEARTBEAT]) > TIMEOUT_SPEC_HEARTBEAT))
			{
				DEBUG_INFO("SocketFd-%d heart beat is over %d seconds.\n", ShmPowerSharing->Connection_Info[idx].socketFd, TIMEOUT_SPEC_HEARTBEAT);
				for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
					ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected = FALSE;
				ShmPowerSharing->Connection_Info[idx].isSocketConnected = FALSE;
			}
		}

		// Check available power
		if(ShmPowerSharing->isDetectNewConnected || (ShmPowerSharing->onHandCurrent < 0))
		{
			for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
			{
				for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
				{
					if(ShmPowerSharing->Connection_Info[idx].isSocketConnected &&
					   ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected)
					{
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType==CONNECTOR_TYPE_DC?SHARE_MIN_DC:SHARE_MIN_AC);
					}
					else
					{
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = 0;
					}
				}
			}

			if(ShmPowerSharing->onHandCurrent < 0)
				DEBUG_INFO("On hand current < 0 re-allocate available current to each connection.\n");

			if(ShmPowerSharing->isDetectNewConnected)
			{
				DEBUG_INFO("Detect gun connected re-allocate available current to each connection.\n");
				ShmPowerSharing->isDetectNewConnected = NO;
			}
		}

		struct CONNECTION_INFO* pDev = NULL;
		Connector_Info* pGun = NULL;
		uint16_t* pIset = NULL;
		uint16_t* pIout = NULL;
		uint16_t Imin = 0;

		for(uint8_t idx = 0; idx < CONNECTION_LIMIT; idx++)
		{
			pDev = &ShmPowerSharing->Connection_Info[idx];
			for(uint8_t gun_index = 0; gun_index < pDev->connectorCount; gun_index++)
			{
				pGun = &pDev->connectorInfo[gun_index];
				pIset = &pGun->availableSharingCurrent;
				pIout = &pGun->presentOutputCurrent;
				Imin = (pGun->connectorType == CONNECTOR_TYPE_DC ? SHARE_MIN_DC : SHARE_MIN_AC);

				if(pDev->isSocketConnected && pGun->isGunConnected)
				{
					if((getDiffSecNow(pGun->tmrCheckCapacity) >= INTERVAL_SPEC_CHECK_CAPACITY))
					{
						int16_t Idiff = *pIset - *pIout;
						if(Idiff > 3)
						{
							int32_t IsetAdd = - (Idiff >> 1);
							uint16_t IsetNext = *pIset + IsetAdd;
							if(IsetNext >= Imin)
								*pIset = IsetNext;
							else
								*pIset = Imin;
						}
						else if((abs(Idiff) <= 1) && (ShmPowerSharing->onHandCurrent > 0))
						{
							int AvgCurr = ShmPowerSharing->onHandCurrent /
								(ShmPowerSharing->connectedConnectorQty == 0 ? 1 : ShmPowerSharing->connectedConnectorQty);
							*pIset += (pGun->connectorType == CONNECTOR_TYPE_AC_THREE ? AvgCurr / 3 : AvgCurr);
						}
						else
						{
						}

						Connector_Info* pGunIsetMax = GetIsetMaxGun();
						Connector_Info* pGunIsetMin = GetIsetMinGun();
						if (pGun == pGunIsetMax && pGunIsetMax == pGunIsetMin)
						{
							if (pGun->connectorType == CONNECTOR_TYPE_AC_THREE)
							{
								if (ShmPowerSharing->onHandCurrent >= 3)
								{
									*pIset += 1;
									conn_getOnHandCurrent();
								}
							}
							else
							{
								for (uint8_t i = 1; i <= 1; i++)
								{
									if (ShmPowerSharing->onHandCurrent > 0)
									{
										*pIset += 1;
										conn_getOnHandCurrent();
									}
									else
									{
										break;
									}
								}
							}
						}
						else if (pGun == pGunIsetMin)
						{
							if (pGun->connectorType == CONNECTOR_TYPE_AC_THREE)
							{
								if (ShmPowerSharing->onHandCurrent >= 3)
								{
									*pIset += 1;
									conn_getOnHandCurrent();
								}
							}
							else
							{
								for (uint8_t i = 1; i <= 1; i++)
								{
									if (ShmPowerSharing->onHandCurrent > 0)
									{
										*pIset += 1;
										conn_getOnHandCurrent();
									}
									else
									{
										break;
									}
								}
							}
						}
						else if (pGun == pGunIsetMax)
						{
							if (pGun->connectorType == CONNECTOR_TYPE_AC_THREE)
							{
								*pIset -= 1;
								conn_getOnHandCurrent();
							}
							else
							{
								*pIset -= 1;
								conn_getOnHandCurrent();
							}
						}
						refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].tmrCheckCapacity);
					}
				}
				else
				{
					if(*pIset != 0)
					{
						DEBUG_INFO("Dupfd-%d on conn-%d available current reset to 0A\n", ShmPowerSharing->Connection_Info[idx].socketFd, idx);
					}
					*pIset = 0;
				}
			}
		}

		usleep(100000);
	}

	return FAIL;
}
#else //MODIFY_MODPS_BALANCE_CHECK_LOOP

int balance_check_loop(void)
{
	for(;;)
	{
		// Get connection info
		conn_getConectedQuantity();
		conn_getOnHandCurrent();
		conn_getConectedConnector();

		// Check conn heart beat
		for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		{
			if(ShmPowerSharing->Connection_Info[idx].isSocketConnected &&
			   (getDiffSecNow(ShmPowerSharing->Connection_Info[idx].timer[POWERSHARING_TMR_IDX_HEARTBEAT]) > TIMEOUT_SPEC_HEARTBEAT))
			{
				DEBUG_INFO("SocketFd-%d heart beat is over %d seconds.\n", ShmPowerSharing->Connection_Info[idx].socketFd, TIMEOUT_SPEC_HEARTBEAT);
				for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
					ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected = FALSE;
				ShmPowerSharing->Connection_Info[idx].isSocketConnected = FALSE;
			}
		}

		// Check available power
		if(ShmPowerSharing->isDetectNewConnected || (ShmPowerSharing->onHandCurrent < 0))
		{
			for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
			{
				for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
				{
					if(ShmPowerSharing->Connection_Info[idx].isSocketConnected &&
					   ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected)
					{
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType==CONNECTOR_TYPE_DC?SHARE_MIN_DC:SHARE_MIN_AC);
					}
					else
					{
						ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = 0;
					}
				}
			}

			if(ShmPowerSharing->onHandCurrent < 0)
				DEBUG_INFO("On hand current < 0 re-allocate available current to each connection.\n");

			if(ShmPowerSharing->isDetectNewConnected)
			{
				DEBUG_INFO("Detect gun connected re-allocate available current to each connection.\n");
				ShmPowerSharing->isDetectNewConnected = NO;
			}
		}

		for(uint8_t idx=0;idx<CONNECTION_LIMIT;idx++)
		{
			for(uint8_t gun_index=0;gun_index<ShmPowerSharing->Connection_Info[idx].connectorCount;gun_index++)
			{
				if(ShmPowerSharing->Connection_Info[idx].isSocketConnected &&
				   ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].isGunConnected)
				{
					if((getDiffSecNow(ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].tmrCheckCapacity) >= INTERVAL_SPEC_CHECK_CAPACITY))
					{
						if((ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent-ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent) > 3)
						{
							if((ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent - ((ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent-ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent)>>1)) >= (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType==CONNECTOR_TYPE_DC?SHARE_MIN_DC:SHARE_MIN_AC))
								ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent -= (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent-ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent)>>1;
							else
								ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType==CONNECTOR_TYPE_DC?SHARE_MIN_DC:SHARE_MIN_AC);
						}
						else if((abs(ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].presentOutputCurrent-ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent) <= 1) &&
								(ShmPowerSharing->onHandCurrent > 0))

						{
							ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent += (ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].connectorType==CONNECTOR_TYPE_AC_THREE?(ShmPowerSharing->onHandCurrent/(ShmPowerSharing->connectedConnectorQty==0?1:ShmPowerSharing->connectedConnectorQty))/3:ShmPowerSharing->onHandCurrent/(ShmPowerSharing->connectedConnectorQty==0?1:ShmPowerSharing->connectedConnectorQty));
						}
						else
						{}
						refreshStartTimer(&ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].tmrCheckCapacity);
					}
				}
				else
				{
					if(ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent != 0)
					{
						DEBUG_INFO("Dupfd-%d on conn-%d available current reset to 0A\n", ShmPowerSharing->Connection_Info[idx].socketFd, idx);
					}
					ShmPowerSharing->Connection_Info[idx].connectorInfo[gun_index].availableSharingCurrent = 0;
				}
			}
		}

		usleep(100000);
	}

	return FAIL;
}
#endif //MODIFY_MODPS_BALANCE_CHECK_LOOP

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

	Init_FW_Info();
	DEBUG_INFO("\n\n");
	DEBUG_INFO("Latest Firmware Version : [ %s ]\n", Version_And_Date[0]);
	DEBUG_INFO("Latest Upgrade Date : [ %s ]\n", Version_And_Date[1]);

	// Enable server if rotary switch not slave mode
	if(ShmSysConfigAndInfo->SysConfig.isEnableLocalPowerSharing == 1)
	{
		// TCP socket server start
		if(fork() == 0)
		{
			if(tcpSocketServer() == FAIL)
			{
				DEBUG_ERROR("TCP socket server down.\n");
				return 0;
			}
		}

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

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

		usleep(100000);
	}

	return FAIL;
}