csu3_am335x/EVSE/Modularization/Module_ProduceUtils.c

/*
 * Module_ProduceUtils.c
 *
 *  Created on: 2020-02-10
 *      Author: Folus Wen
 */
#include    <sys/types.h>
#include    <sys/stat.h>
#include 	<sys/time.h>
#include 	<sys/timeb.h>
#include 	<sys/ipc.h>
#include 	<sys/shm.h>
#include 	<sys/mman.h>
#include 	<sys/socket.h>
#include 	<netinet/in.h>

#include 	<unistd.h>
#include 	<stdarg.h>
#include    <stdio.h>		/*標準輸入輸出定義*/
#include    <stdlib.h>		/*標準函數庫定義*/
#include    <unistd.h>		/*Unix 標準函數定義*/
#include    <fcntl.h>		/*檔控制定義*/
#include    <termios.h>		/*PPSIX 終端控制定義*/
#include    <errno.h>		/*錯誤號定義*/
#include 	<errno.h>
#include 	<string.h>
#include	<time.h>
#include	<ctype.h>
#include	"define.h"
#include	"Module_ProduceUtils.h"

#define Debug
#define ARRAY_SIZE(A)			(sizeof(A) / sizeof(A[0]))
#define PASS					1
#define FAIL					0
#define ON						1
#define OFF						0
#define MtdBlockSize			0x600000

#define LISTEN_PORT				8234
#define	CONNECTION_LIMIT		1

#define PROTOCOL_ADDR			0

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;
struct OCPP16Data				*ShmOCPP16Data;

int StoreLogMsg(const char *fmt, ...)
{
	char Buf[4096+256];
	char buffer[4096];
	time_t CurrentTime;
	struct tm *tm;
	va_list args;

	va_start(args, fmt);
	int rc = vsnprintf(buffer, sizeof(buffer), fmt, args);
	va_end(args);

	memset(Buf,0,sizeof(Buf));
	CurrentTime = time(NULL);
	tm=localtime(&CurrentTime);
	sprintf(Buf,"echo \"[%04d.%02d.%02d %02d:%02d:%02d] - %s\" >> /Storage/SystemLog/[%04d.%02d]ProduceUtils_SystemLog",
			tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,
			buffer,
			tm->tm_year+1900,tm->tm_mon+1);
#ifdef SystemLogMessage
	system(Buf);
#endif

	printf("[%04d.%02d.%02d %02d:%02d:%02d] - %s", tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec, buffer);

	return rc;
}

int DiffTimeb(struct timeb ST, struct timeb ET)
{
	//return milli-second
	unsigned int StartTime,StopTime;

	StartTime=(unsigned int)ST.time;
	StopTime=(unsigned int)ET.time;
	return (StopTime-StartTime)*1000+ET.millitm-ST.millitm;
}

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

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

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

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

   	return result;
}

//==========================================
// Common routine
//==========================================
void trim(char *s)
{
    int i=0, j, k, l=0;

    while((s[i]==' ')||(s[i]=='\t')||(s[i]=='\n'))
        i++;

    j = strlen(s)-1;
    while((s[j]==' ')||(s[j]=='\t')||(s[j]=='\n'))
        j--;

    if(i==0 && j==strlen(s)-1) { }
    else if(i==0) s[j+1] = '\0';
    else {
        for(k=i; k<=j; k++) s[l++] = s[k];
        s[l] = '\0';
    }
}

void substr(char *dest, const char* src, unsigned int start, unsigned int cnt)
{
	strncpy(dest, src + start, cnt);
	dest[cnt] = 0;
}

int StoreUsrConfigData(struct SysConfigData *UsrData)
{
	int result = PASS;
	int fd,wrd;
	unsigned int i,Chk;
	unsigned char *ptr, *BufTmp;

	Chk=0;
	ptr=(unsigned char *)UsrData;
	if((BufTmp=malloc(MtdBlockSize))!=NULL)
	{
		memset(BufTmp,0,MtdBlockSize);
		memcpy(BufTmp,ptr,sizeof(struct SysConfigData));
		for(i=0;i<MtdBlockSize-4;i++)
			Chk+=*(BufTmp+i);
		memcpy(	BufTmp+MtdBlockSize-4,&Chk,4);
		fd = open("/dev/mtdblock10", O_RDWR);
		if (fd>0)
		{
			wrd=write(fd, BufTmp, MtdBlockSize);
			close(fd);
			if(wrd>=MtdBlockSize)
			{
				fd = open("/dev/mtdblock11", O_RDWR);
				if (fd>0)
				{
					wrd=write(fd, BufTmp, MtdBlockSize);
    					close(fd);
				   	if(wrd<MtdBlockSize)
					{
						DEBUG_ERROR("write /dev/mtdblock11(backup) NG\r\n");
				   		result = FAIL;
					}
				}
				else
				{
					DEBUG_ERROR("open /dev/mtdblock11(backup) NG\r\n");
					result = FAIL;
				}
			}
			else
			{
		    		DEBUG_ERROR("write /dev/mtdblock10 NG\r\n");
		    		result = FAIL;
			}
		}
		else
		{
			DEBUG_ERROR("open /dev/mtdblock10 NG\r\n");
			result = FAIL;
		}
	}
	else
	{
		DEBUG_ERROR("alloc BlockSize NG\r\n");
    		result = FAIL;
	}
	if(BufTmp!=NULL)
		free(BufTmp);

	return result;
}

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

	for(int idx = 0;idx<((message[4] | message[5]<<8)>1024?1024:(message[4] | message[5]<<8));idx++)
	{
		chksum ^= message[6+idx];
	}

	return ((chksum == message[6+((message[4] | message[5]<<8)>1024?1024:(message[4] | message[5]<<8))]) ? PASS : FAIL);
}

//==========================================
// Main process
//==========================================
int main(void)
{
	int 				sockFd = 0;
	int 				clientSockFd = 0;
	uint8_t 			inputBuffer[2048] = {};
	uint8_t 			outBuffer[2048] = {};
	uint8_t				cmdBuf[128];
	uint8_t				chksum;
	int8_t 				read_size;
	int8_t				tx_size;
	struct sockaddr_in 	serverInfo, clientInfo;
	socklen_t 			addrlen = sizeof(clientInfo);

	struct timeb csuTime;
	struct tm *tmCSU;


	if(InitShareMemory() == FAIL)
	{
		DEBUG_ERROR("InitShareMemory NG\n");

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

	sockFd = socket(AF_INET , SOCK_STREAM , 0);
	if(sockFd == -1)
	{
		DEBUG_ERROR("InitSocketServer NG\n");
		sleep(5);
		return 0;
	}

	bzero(&serverInfo,sizeof(serverInfo));
	serverInfo.sin_family = PF_INET;
	serverInfo.sin_addr.s_addr = htonl(INADDR_ANY);
	serverInfo.sin_port = htons(LISTEN_PORT);
	bind(sockFd, (struct sockaddr *)&serverInfo, sizeof(serverInfo));
	listen(sockFd, CONNECTION_LIMIT);

	// Main loop
	for(;;)
	{
		clientSockFd = accept(sockFd, (struct sockaddr*) &clientInfo, &addrlen);
		DEBUG_INFO("Client connect in.\r\n");

		while((read_size = recv(clientSockFd, inputBuffer, sizeof(inputBuffer), 0)) > 0)
		{
			if(isValidCheckSum(inputBuffer))
			{
				chksum = 0;
				memset(outBuffer, 0x00, sizeof(outBuffer));

				switch(inputBuffer[3])
				{
					case CMD_QUERY_FW_VER:
						DEBUG_INFO("Query firmware version: %s.\r\n", (char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev);
						tx_size= 7 + (strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev));
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_FW_VER;
						outBuffer[4] = strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev)&0xff;
						outBuffer[5] = (strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev)>>0x08) & 0xff;
						for(uint8_t idx=0;idx<strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev);idx++)
						{
							outBuffer[6+idx] = ShmSysConfigAndInfo->SysInfo.CsuRootFsFwRev[idx];
						}

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[6+(outBuffer[4] | (outBuffer[5]<<8))] = chksum;

						break;
					case CMD_QUERY_HW_VER:
						DEBUG_INFO("Query hardware version: %s.\r\n", (char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev);
						tx_size = 7 + strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev);
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_HW_VER;
						outBuffer[4] = strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev) & 0xff;
						outBuffer[5] = (strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev)>>0x08) & 0xff;
						for(uint8_t idx=0;idx<strlen((char*)ShmSysConfigAndInfo->SysInfo.CsuHwRev);idx++)
						{
							outBuffer[6+idx] = (char)ShmSysConfigAndInfo->SysInfo.CsuHwRev[idx];
						}

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[6+(outBuffer[4] | (outBuffer[5]<<8))] = chksum;
						break;
					case CMD_QUERY_PRESENT_INPUTVOLTAGE:
						DEBUG_INFO("Query present input voltage.\r\n");
						DEBUG_INFO("  -Input R: %.2f.\r\n", ShmSysConfigAndInfo->SysInfo.InputVoltageR);
						DEBUG_INFO("  -Input S: %.2f.\r\n", ShmSysConfigAndInfo->SysInfo.InputVoltageS);
						DEBUG_INFO("  -Input T: %.2f.\r\n", ShmSysConfigAndInfo->SysInfo.InputVoltageT);
						tx_size = 14;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_PRESENT_INPUTVOLTAGE;
						outBuffer[4] = 0x07;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x00;
						outBuffer[7] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageR*10)>>0) & 0xff);
						outBuffer[8] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageR*10)>>8) & 0xff);
						outBuffer[9] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageS*10)>>0) & 0xff);
						outBuffer[10] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageS*10)>>8) & 0xff);
						outBuffer[11] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageT*10)>>0) & 0xff);
						outBuffer[12] = (((int)(ShmSysConfigAndInfo->SysInfo.InputVoltageT*10)>>8) & 0xff);

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[13] = chksum;
						break;
					case CMD_QUERY_PRESENT_OUTPUTVOLTAGE:
						break;
					case CMD_QUERY_FAN_SPEED:
						break;
					case CMD_QUERY_TEMPERATURE:
						DEBUG_INFO("Query temperature.\r\n");
						DEBUG_INFO("  -SystemAmbientTemp: %d\r\n", ShmSysConfigAndInfo->SysInfo.SystemAmbientTemp);
						DEBUG_INFO("  -CcsConnectorTemp: %d\r\n", ShmSysConfigAndInfo->SysInfo.CcsConnectorTemp);
						tx_size = 15;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_TEMPERATURE;
						outBuffer[4] = 0x08;
						outBuffer[5] = 0x00;
						outBuffer[6] = (((ShmSysConfigAndInfo->SysInfo.SystemAmbientTemp+60)>>0) & 0xff);
						outBuffer[7] = (((ShmSysConfigAndInfo->SysInfo.CcsConnectorTemp+60)>>0) & 0xff);
						outBuffer[8] = 0x00;
						outBuffer[9] = 0x00;
						outBuffer[10] = 0x00;
						outBuffer[11] = 0x00;
						outBuffer[12] = 0x00;
						outBuffer[13] = 0x00;

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[14] = chksum;
						break;
					case CMD_QUERY_AUX_POWERVOLTAGE:
						break;
					case CMD_QUERY_RELAY_OUTPUT:

						break;
					case CMD_QUERY_GFD_ADC:
						break;
					case CMD_QUERY_GPIO_INPUT:
						break;
					case CMD_QUERY_ALARM_LOG:
						break;
					case CMD_QUERY_SN:
						ftime(&csuTime);
						tmCSU = localtime(&csuTime.time);
						DEBUG_INFO("Query serial number: %s\r\n", (char*)ShmSysConfigAndInfo->SysConfig.SerialNumber);
						tx_size = 15 + strlen((char*)ShmSysConfigAndInfo->SysConfig.SerialNumber);
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_SN;
						outBuffer[4] = (strlen((char*)ShmSysConfigAndInfo->SysConfig.SerialNumber)+8)&0xff;
						outBuffer[5] = ((strlen((char*)ShmSysConfigAndInfo->SysConfig.SerialNumber)+8)>>0x08) & 0xff;
						outBuffer[6] = ((tmCSU->tm_year+1900)/1000)+'0';
						outBuffer[7] = (((tmCSU->tm_year+1900)%1000)/100)+'0';
						outBuffer[8] = (((tmCSU->tm_year+1900)%100)/10)+'0';
						outBuffer[9] = ((tmCSU->tm_year+1900)%10) +'0';
						outBuffer[10] = ((tmCSU->tm_mon+1)/10) +'0';
						outBuffer[11] = ((tmCSU->tm_mon+1)%10) +'0';
						outBuffer[12] = (tmCSU->tm_mday/10) +'0';
						outBuffer[13] = (tmCSU->tm_mday%10) +'0';

						for(uint8_t idx=0;idx<strlen((char*)ShmSysConfigAndInfo->SysConfig.SerialNumber);idx++)
						{
							outBuffer[14+idx] = (char)ShmSysConfigAndInfo->SysConfig.SerialNumber[idx];
						}

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[6+(outBuffer[4] | (outBuffer[5]<<8))] = chksum;
						break;
					case CMD_QUERY_MODEL_NAME:
						DEBUG_INFO("Query model name: %s\r\n", (char*)ShmSysConfigAndInfo->SysConfig.ModelName);
						tx_size = 7 + strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName);
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_MODEL_NAME;
						outBuffer[4] = strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName)&0xff;
						outBuffer[5] = (strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName)>>0x08) & 0xff;
						for(uint8_t idx=0;idx<strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName);idx++)
						{
							outBuffer[6+idx] = (char)ShmSysConfigAndInfo->SysConfig.ModelName[idx];
						}

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[6+(outBuffer[4] | (outBuffer[5]<<8))] = chksum;
						break;
					case CMD_QUERY_PARAMETER:
						break;
					case CMD_QUERY_RTC:
						DEBUG_INFO("Query system time.\r\n");
						ftime(&csuTime);
						tmCSU = localtime(&csuTime.time);

						tx_size = 21;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_RTC;
						outBuffer[4] = 0x0e;
						outBuffer[5] = 0x00;
						outBuffer[6] = ((tmCSU->tm_year+1900)/1000)+'0';
						outBuffer[7] = (((tmCSU->tm_year+1900)%1000)/100)+'0';
						outBuffer[8] = (((tmCSU->tm_year+1900)%100)/10)+'0';
						outBuffer[9] = ((tmCSU->tm_year+1900)%10) +'0';
						outBuffer[10] = ((tmCSU->tm_mon+1)/10) +'0';
						outBuffer[11] = ((tmCSU->tm_mon+1)%10) +'0';
						outBuffer[12] = (tmCSU->tm_mday/10) +'0';
						outBuffer[13] = (tmCSU->tm_mday%10) +'0';
						outBuffer[14] = (tmCSU->tm_hour/10) +'0';
						outBuffer[15] = (tmCSU->tm_hour%10) +'0';
						outBuffer[16] = (tmCSU->tm_min/10) +'0';
						outBuffer[17] = (tmCSU->tm_min%10) +'0';
						outBuffer[18] = (tmCSU->tm_sec/10) +'0';
						outBuffer[19] = (tmCSU->tm_sec%10) +'0';

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[20] = chksum;
						break;
					case CMD_QUERY_4G_REVISION:
						DEBUG_INFO("Query 4G module revision: %s\r\n", (char*)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer);
						tx_size = 7 + strlen((char*)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer);
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] = inputBuffer[1];
						outBuffer[3] = CMD_QUERY_4G_REVISION;
						outBuffer[4] = strlen((char*)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer)&0xff;
						outBuffer[5] = (strlen((char*)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer)>>0x08) & 0xff;
						for(uint8_t idx=0;idx<strlen((char*)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer);idx++)
						{
							outBuffer[6+idx] = (char)ShmSysConfigAndInfo->SysConfig.TelecomInterface.TelcomSoftwareVer[idx];
						}

						for(uint16_t idx=0;idx<(outBuffer[4] | (outBuffer[5]<<8));idx++)
						{
						  chksum ^= outBuffer[6 + idx];
						}
						outBuffer[6+(outBuffer[4] | (outBuffer[5]<<8))] = chksum;
						break;
					case CMD_QUERY_AC_STATUS:
						break;
					case CMD_QUERY_AC_ALARM:
						break;
					case CMD_QUERY_BLE_CONFIG_DATA:
						break;
					case CMD_QUERY_POWER_CONSUMPTION:
						break;
					case CMD_QUERY_GUN_PLUGIN_TIMES:
						break;
					case CMD_CONFIG_FAN_SPEED:
						break;
					case CMD_CONFIG_SERIAL_NUMBER:
						memset(ShmSysConfigAndInfo->SysConfig.SerialNumber, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.SerialNumber));
						memset(ShmSysConfigAndInfo->SysConfig.SystemId, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.SystemId));

						for(uint16_t idx=0;idx<((inputBuffer[4] | (inputBuffer[5]<<8))-8);idx++)
						{
							ShmSysConfigAndInfo->SysConfig.SerialNumber[idx] = inputBuffer[14+idx];
						}
						ShmSysConfigAndInfo->SysConfig.SerialNumber[(inputBuffer[4] | (inputBuffer[5]<<8))-8] = '\0';
						memcpy(ShmSysConfigAndInfo->SysConfig.SystemId, ShmSysConfigAndInfo->SysConfig.ModelName, strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName));
						memcpy(&ShmSysConfigAndInfo->SysConfig.SystemId[strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName)], ShmSysConfigAndInfo->SysConfig.SerialNumber, (inputBuffer[4] | (inputBuffer[5]<<8)));

						tx_size = 8;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] =  inputBuffer[1];
						outBuffer[3] = CMD_CONFIG_SERIAL_NUMBER;
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x01;
						outBuffer[7] = outBuffer[6];

						DEBUG_INFO("Config serial number: %s\r\n", (char*)ShmSysConfigAndInfo->SysConfig.SerialNumber);

						break;
					case CMD_CONFIG_MODEL_NAME:
						memset(ShmSysConfigAndInfo->SysConfig.SerialNumber, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.SerialNumber));
						memset(ShmSysConfigAndInfo->SysConfig.SystemId, 0x00, ARRAY_SIZE(ShmSysConfigAndInfo->SysConfig.SystemId));

						for(uint16_t idx=0;idx<(inputBuffer[4] | (inputBuffer[5]<<8));idx++)
						{
							ShmSysConfigAndInfo->SysConfig.ModelName[idx] = inputBuffer[6+idx];
						}
						ShmSysConfigAndInfo->SysConfig.ModelName[(inputBuffer[4] | (inputBuffer[5]<<8))] = '\0';
						memcpy(ShmSysConfigAndInfo->SysConfig.SystemId, ShmSysConfigAndInfo->SysConfig.ModelName, strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName));
						memcpy(&ShmSysConfigAndInfo->SysConfig.SystemId[strlen((char*)ShmSysConfigAndInfo->SysConfig.ModelName)], ShmSysConfigAndInfo->SysConfig.SerialNumber, (inputBuffer[4] | (inputBuffer[5]<<8)));

						tx_size = 8;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] =  inputBuffer[1];
						outBuffer[3] = CMD_CONFIG_MODEL_NAME;
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x01;
						outBuffer[7] = outBuffer[6];
						DEBUG_INFO("Config model name: %s\r\n", (char*)ShmSysConfigAndInfo->SysConfig.ModelName);
						break;
					case CMD_CONFIG_RELAY_OUTPUT:
						break;
					case CMD_CONFIG_GPIO_OUTPUT:
						break;
					case CMD_CONFIG_RTC:
						DEBUG_INFO("Config system time.\r\n");
						tx_size = 8;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] =  inputBuffer[1];
						outBuffer[3] = CMD_CONFIG_RTC;
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = 0x01;
						outBuffer[7] = 0x01;

						sprintf((char*)cmdBuf, "date -u -s \"%04d-%02d-%02d %02d:%02d:%02d\"", (inputBuffer[6]-'0')*1000 + (inputBuffer[7]-'0')*100 + (inputBuffer[8]-'0')*10 + (inputBuffer[9]-'0'),
																						(inputBuffer[10]-'0')*10+ (inputBuffer[11]-'0'),
																						(inputBuffer[12]-'0')*10 + (inputBuffer[13]-'0'),
																						(inputBuffer[14]-'0')*10 + (inputBuffer[15]-'0'),
																						(inputBuffer[16]-'0')*10 + (inputBuffer[17]-'0'),
																						(inputBuffer[18]-'0')*10 + (inputBuffer[19]-'0'));
						system((char*)cmdBuf);
						system("hwclock -w -u");
						system("hwclock -s");

						break;
					case CMD_CONFIG_AC_LED:
						break;
					case CMD_CONFIG_CURRENT_LINIT:
						break;
					case CMD_CONFIG_MCU_MODE:
						break;
					case CMD_CONFIG_MCU_RESET_REQUEST:
						break;
					case CMD_CONFIG_BREATHE_LED_TIMING:
						break;
					case CMD_CONFIG_SAVE_CONFIGURATION:
						tx_size = 8;
						outBuffer[0] = 0xaa;
						outBuffer[1] = PROTOCOL_ADDR;
						outBuffer[2] =  inputBuffer[1];
						outBuffer[3] = CMD_CONFIG_SAVE_CONFIGURATION;
						outBuffer[4] = 0x01;
						outBuffer[5] = 0x00;
						outBuffer[6] = (StoreUsrConfigData(&ShmSysConfigAndInfo->SysConfig)!=0x01?0x00:0x01);
						outBuffer[7] = outBuffer[6];
						DEBUG_INFO("Save configuration\r\n");

						if((outBuffer[6] == PASS))
						{
							sprintf((char*)ShmOCPP16Data->Reset.Type, "Soft");
							ShmOCPP16Data->MsMsg.bits.ResetReq = ON;
						}

						break;
					case CMD_UPDATE_START:
						break;
					case CMD_UPDATE_ABOARD:
						break;
					case CMD_UPDATE_TRANSFER:
						break;
					default:
						break;
				}
			}
			else
			{
				tx_size = 9;
				outBuffer[0] = 0xaa;
				outBuffer[1] = PROTOCOL_ADDR;
				outBuffer[2] = inputBuffer[1];
				outBuffer[3] = inputBuffer[3];
				outBuffer[4] = 0x01;
				outBuffer[5] = 0x00;
				outBuffer[6] = 0x00;
				outBuffer[7] = 0x00;
			}

			send(clientSockFd, outBuffer, tx_size, 0);
		}

		if(read_size == 0)
		{
			DEBUG_INFO("Client disconnected.\r\n");
			fflush(stdout);
		}
		else if(read_size == -1)
		{
			DEBUG_ERROR("Socket recv failed.\r\n");
		}

		sleep(1);
	}
}