csu3_am335x/EVSE/Projects/PlugIt360/Apps/Module_Modbus.c

/*
 * Module_Modbus.c
 *
 *  Created on: 2020年3月11日
 *      Author: 7564
 */

#include 	<sys/time.h>
#include 	<sys/timeb.h>
#include    <sys/types.h>
#include    <sys/stat.h>
#include 	<sys/types.h>
#include 	<sys/ioctl.h>
#include 	<sys/socket.h>
#include 	<sys/ipc.h>
#include 	<sys/shm.h>
#include 	<sys/shm.h>
#include 	<sys/mman.h>
#include 	<linux/wireless.h>
#include 	<arpa/inet.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 	<ifaddrs.h>
#include 	<stdbool.h>
#include	"../../define.h"
#include 	"modbus.h"

#define ARRAY_SIZE(A)		(sizeof(A) / sizeof(A[0]))
#define PASS				1
#define FAIL				-1
#define YES					1
#define NO					0

#define MODBUS_SERVER_PORT     502
#define MODBUS_DEBUG           OFF

modbus_t						*ctx;
int								server_socket;
modbus_mapping_t 				*mb_mapping;
int 							header_length;
uint8_t         				bits[MODBUS_MAX_READ_BITS] = {0};
uint16_t        				regs[MODBUS_MAX_READ_REGISTERS] = {0};

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;

unsigned char _gunCount = CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY;
struct ChargingInfoData *chargingInfo[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY];

void PRINTF_FUNC(char *string, ...);

int StoreLogMsg(const char *fmt, ...);
#define DEBUG_INFO(format, args...) StoreLogMsg("[%s:%d][%s][Info] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_WARN(format, args...) StoreLogMsg("[%s:%d][%s][Warn] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_ERROR(format, args...) StoreLogMsg("[%s:%d][%s][Error] "format, __FILE__, __LINE__, __FUNCTION__, ##args)

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]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);
	system(Buf);

	return rc;
}

void PRINTF_FUNC(char *string, ...)
{
	va_list args;
	char buffer[4096];
	va_start(args, string);
	vsnprintf(buffer, sizeof(buffer), string, args);
	va_end(args);

	if (DEBUG)
		PRINTF_FUNC("%s \n", buffer);
	else
		DEBUG_INFO("%s \n", buffer);
}

//=================================
// Save data to share memory Function
//=================================
bool FindChargingInfoData(byte target, struct ChargingInfoData **chargingData)
{
	for (byte index = 0; index < CHAdeMO_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index];
			return true;
		}
	}

	for (byte index = 0; index < CCS_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.CcsChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.CcsChargingData[index];
			return true;
		}
	}

	for (byte index = 0; index < GB_QUANTITY; index++)
	{
		if (ShmSysConfigAndInfo->SysInfo.GbChargingData[index].Index == target)
		{
			chargingData[target] = &ShmSysConfigAndInfo->SysInfo.GbChargingData[index];
			return true;
		}
	}

	return false;
}

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

    return result;
}

void Initialization()
{
	bool isPass = false;
	while(!isPass)
	{
		isPass = true;
		for (byte _index = 0; _index < _gunCount; _index++)
		{
			if (!FindChargingInfoData(_index, &chargingInfo[0]))
			{
				DEBUG_ERROR("EvComm (main) : FindChargingInfoData false \n");
				isPass = false;
				break;
			}
		}
	}
}
//================================================
// Main process
//================================================
int InitModbus()
{
	int result = PASS;

	ctx = modbus_new_tcp(NULL, MODBUS_SERVER_PORT);
	server_socket = modbus_tcp_listen(ctx, 1);
	mb_mapping = modbus_mapping_new(MODBUS_MAX_READ_BITS, MODBUS_MAX_WRITE_BITS, MODBUS_MAX_READ_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS);
	if (mb_mapping == NULL)
	{
		PRINTF_FUNC("mb_mapping error.\r\n");
		modbus_free(ctx);
		result = FAIL;
	}
	modbus_set_debug(ctx, MODBUS_DEBUG);

	return result;
}

void updateInfo()
{
	// Start or Stop Charging
	if (mb_mapping->tab_registers[0] == YES)
		ShmSysConfigAndInfo->SysInfo.StartToChargingFlag = YES;
	else
		ShmSysConfigAndInfo->SysInfo.StartToChargingFlag = NO;

	// Hard Reset
	if (mb_mapping->tab_registers[1] == YES && chargingInfo[0]->SystemStatus == S_IDLE)
	{
		sleep(3);
		system("reboot -f");
	}

	// State
	mb_mapping->tab_registers[2] = chargingInfo[0]->SystemStatus;

	// Plug Status
	mb_mapping->tab_registers[3] = chargingInfo[0]->ConnectorPlugIn;

	// Max Current
	mb_mapping->tab_registers[4] = (ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent >> 8) & 0xFF;
	mb_mapping->tab_registers[5] = ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent & 0xFF;

	// Soc
	mb_mapping->tab_registers[10] = chargingInfo[0]->EvBatterySoc;

	// Output Pow
	mb_mapping->tab_registers[11] = ((int)chargingInfo[0]->PresentChargingPower >> 24) & 0xFF;
	mb_mapping->tab_registers[12] = ((int)chargingInfo[0]->PresentChargingPower >> 16) & 0xFF;
	mb_mapping->tab_registers[13] = ((int)chargingInfo[0]->PresentChargingPower >> 8) & 0xFF;
	mb_mapping->tab_registers[14] = chargingInfo[0]->PresentChargingPower;

	// Output Vol
	mb_mapping->tab_registers[20] = ((int)chargingInfo[0]->PresentChargingVoltage >> 24) & 0xFF;
	mb_mapping->tab_registers[21] = ((int)chargingInfo[0]->PresentChargingVoltage >> 16) & 0xFF;
	mb_mapping->tab_registers[22] = ((int)chargingInfo[0]->PresentChargingVoltage >> 8) & 0xFF;
	mb_mapping->tab_registers[23] = chargingInfo[0]->PresentChargingVoltage;

	// Output Cur
	mb_mapping->tab_registers[25] = ((int)chargingInfo[0]->PresentChargingCurrent >> 24) & 0xFF;
	mb_mapping->tab_registers[26] = ((int)chargingInfo[0]->PresentChargingCurrent >> 16) & 0xFF;
	mb_mapping->tab_registers[27] = ((int)chargingInfo[0]->PresentChargingCurrent >> 8) & 0xFF;
	mb_mapping->tab_registers[28] = chargingInfo[0]->PresentChargingCurrent;

	// Remain Time
	mb_mapping->tab_registers[30] = (chargingInfo[0]->PresentChargedDuration  >> 24) & 0xFF;
	mb_mapping->tab_registers[31] = (chargingInfo[0]->PresentChargedDuration  >> 16) & 0xFF;
	mb_mapping->tab_registers[32] = (chargingInfo[0]->PresentChargedDuration >> 8) & 0xFF;
	mb_mapping->tab_registers[33] = chargingInfo[0]->PresentChargedDuration;

	// Charging Time
	mb_mapping->tab_registers[35] = (chargingInfo[0]->RemainChargingDuration  >> 24) & 0xFF;
	mb_mapping->tab_registers[36] = (chargingInfo[0]->RemainChargingDuration  >> 16) & 0xFF;
	mb_mapping->tab_registers[37] = (chargingInfo[0]->RemainChargingDuration >> 8) & 0xFF;
	mb_mapping->tab_registers[38] = chargingInfo[0]->RemainChargingDuration;
}

int main(void)
{
	PRINTF_FUNC("Psu Task boot .... \n");
	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("InitShareMemory NG\n");
		#endif
		if(ShmStatusCodeData != NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = 1;
		}
		sleep(5);
		return 0;
	}

	Initialization();
	int rc;
	uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];

	if(InitModbus() == FAIL)
	{
		PRINTF_FUNC("InitModbus NG\r\n");
		return FAIL;
	}
	else
	{
		PRINTF_FUNC("Modbus TCP initial OK.\r\n");
	}

	//=================================
	// Modbus TCP loop
	//=================================
	for(;;)
	{
		modbus_tcp_accept(ctx, &server_socket);

		while((rc = modbus_receive(ctx, query)) > 0)
		{
			modbus_reply(ctx, query, rc, mb_mapping);
			// Update EVSE info
			updateInfo();

			usleep(100000);
		}

		if(rc == -1)
			PRINTF_FUNC("Client disconnect...\r\n");
		else
			PRINTF_FUNC("Client communication error...\r\n");
	}

	return FAIL;
}