/*
 * goose_out.c
 *
 *  Created on: 08-02-2020
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

#include "tdefs.h"
#include "misc.h"

#include "helper.h"
#include "eth.h"
#include "goose.h"
#include "goose_out.h"

#include <ti/ipc/Notify.h>

#include "comm.h"
#include "../ethernet/ports/am1808/include/lwiplib.h"
#include "../ethernet/emac.h"
#include "../ethernet/hw_types.h"
#include "ports/am1808/include/netif/sitaraif.h"
#include <ti/sysbios/hal/Hwi.h>
#include <string.h>
#include <stdio.h>

int goose_out_initlog(void *arguments, void *logic)
{
	struct goose_out_args *args = (struct goose_out_args *)arguments;
	struct goose_out_logic *log = (struct goose_out_logic *)logic;
	u8 i;
	int values[6];
	u8 dstaddr[6];
	struct goose_frame_normal *gfr = (struct goose_frame_normal *)log->goose_out_frame;
	struct goose_frame_vlan *gfr_vlan = (struct goose_frame_vlan *)log->goose_out_frame;
	struct goose_frame2 *g;

	for(i=0;i<16;i++)
		if(set_bit_ptr_struct(args->io.state_in[i],&log->state_in[i]))
			return -1;

	if(set_bit_ptr_struct(args->io.test_in,&log->test))
		return -1;

	if(set_bit_ptr_struct(args->io.ndscom_in,&log->ndscom_in))
		return -1;

	log->goose_id=(char*)&args->params.strings_start;
	u8 len = strlen(log->goose_id) + 1;
	if(len%4)
		len+=4-(len%4);

	log->goose_dataset=log->goose_id+len;
	len = strlen(log->goose_dataset) + 1;
		if(len%4)
			len+=4-(len%4);

	log->goose_cbref=log->goose_dataset+len;
	len = strlen(log->goose_cbref) + 1;
		if(len%4)
			len+=4-(len%4);

	log->goose_dstaddr=log->goose_cbref+len;
	len = strlen(log->goose_dstaddr) + 1;
		if(len%4)
			len+=4-(len%4);

	if( 6 == sscanf( log->goose_dstaddr, "%x-%x-%x-%x-%x-%x%*c",
	    &values[0], &values[1], &values[2],
	    &values[3], &values[4], &values[5] ) )
	{
	    for( i = 0; i < 6; ++i )
	        dstaddr[i] = (u8) values[i];
	}
	else
		return -1;

	log->stnum_cur=1;
	log->sqnum_cur=1;

	// prepare frame headers
	memset(log->goose_out_frame,0,sizeof(log->goose_out_frame));
	memcpy(gfr->dst_mac,dstaddr,6);
	memcpy(gfr->src_mac,eth.hwaddr,6);

	if(args->params.bits & GOOSE_OUT_VLAN_EN)
	{
		gfr_vlan->ethertype=GOOSE_ETHERTYPE_LE;
		gfr_vlan->tpid=0x0081; // 0x8100 LE coded
		gfr_vlan->tci=((args->params.priority & 0x07)<<5)|((args->params.vlan_no & 0x0F00)>>8)|((args->params.vlan_no & 0x00FF)<<8);
		g=(struct goose_frame2 *)&gfr_vlan->goose;
	}
	else
	{
		gfr->ethertype=GOOSE_ETHERTYPE_LE;
		g=(struct goose_frame2 *)&gfr->goose;
	}

	g->appid=htons(args->params.appid);
	g->pdu_tag=GOOSE_TAG_PDU;
	g->pdu_taglen=0x81;
	log->len=11;

	u32 dcnt = 0;

	g->pdu_data[dcnt++]=GOOSE_TAG_GOCBREF;
	g->pdu_data[dcnt++]=strlen(log->goose_cbref);
	memcpy(&g->pdu_data[dcnt],log->goose_cbref,strlen(log->goose_cbref));
	dcnt+=strlen(log->goose_cbref);

	g->pdu_data[dcnt++]=GOOSE_TAG_TTL;
	g->pdu_data[dcnt++]=2;
	log->ttl=&g->pdu_data[dcnt];
	*(u16 *)&g->pdu_data[dcnt] = htons(args->params.ttl);
	dcnt+=2;

	g->pdu_data[dcnt++]=GOOSE_TAG_DATSET;
	g->pdu_data[dcnt++]=strlen(log->goose_dataset);
	memcpy(&g->pdu_data[dcnt],log->goose_dataset,strlen(log->goose_dataset));
	dcnt+=strlen(log->goose_dataset);

	g->pdu_data[dcnt++]=GOOSE_TAG_ID;
	g->pdu_data[dcnt++]=strlen(log->goose_id);
	memcpy(&g->pdu_data[dcnt],log->goose_id,strlen(log->goose_id));
	dcnt+=strlen(log->goose_id);

	g->pdu_data[dcnt++]=GOOSE_TAG_T;
	g->pdu_data[dcnt++]=8;
	log->tstamp_secs=&g->pdu_data[dcnt];


	for(i=0;i<4;i++)
		g->pdu_data[dcnt++]=(((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec)>>((3-i)*8));

	log->tstamp_frac=&g->pdu_data[dcnt];
	u32 stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER;
	for(i=0;i<4;i++)
		g->pdu_data[dcnt++]=stamp>>((3-i)*8);

	g->pdu_data[dcnt++]=GOOSE_TAG_STNUM;
	g->pdu_data[dcnt++]=4;
	log->stnum=&g->pdu_data[dcnt];

	for(i=0;i<4;i++)
		g->pdu_data[dcnt++]=(log->stnum_cur>>((3-i)*8));

	g->pdu_data[dcnt++]=GOOSE_TAG_SQNUM;
	g->pdu_data[dcnt++]=4;

	log->sqnum=&g->pdu_data[dcnt];
	for(i=0;i<4;i++)
		g->pdu_data[dcnt++]=(log->sqnum_cur>>((3-i)*8));

	g->pdu_data[dcnt++]=GOOSE_TAG_SIMULATION;
	g->pdu_data[dcnt++]=1;
	log->simulation=&g->pdu_data[dcnt];
	g->pdu_data[dcnt++]=0;

	g->pdu_data[dcnt++]=GOOSE_TAG_CONFREV;
	g->pdu_data[dcnt++]=4;
	for(i=0;i<4;i++)
		g->pdu_data[dcnt++]=(args->params.cfg_rev>>((3-i)*8));

	g->pdu_data[dcnt++]=GOOSE_TAG_NDSCOM;
	g->pdu_data[dcnt++]=1;
	log->ndscom=&g->pdu_data[dcnt];
	g->pdu_data[dcnt++]=0;

	g->pdu_data[dcnt++]=GOOSE_TAG_NUMDATSETENTRIES;
	g->pdu_data[dcnt++]=1;
	g->pdu_data[dcnt++]=16;

	g->pdu_data[dcnt++]=GOOSE_TAG_ALLDATA;
	g->pdu_data[dcnt++]=16*3; // 16 states * 3 bytes each
	for(i=0;i<16;i++)
	{
		g->pdu_data[dcnt++]=GOOSE_DATA_BOOLEAN;
		g->pdu_data[dcnt++]=1;
		log->state[i]=&g->pdu_data[dcnt];
		g->pdu_data[dcnt++]=0;
	}

	g->pdu_len+=dcnt;
	log->len+=dcnt;
	g->len=htons(log->len);


	if(log->len>=(GOOSE_MAX_FR_OUT_SIZE-18))
		return -1;

	log->first_packet=1;

	return 0;
}


void goose_out(void *arguments, void *logic)
{
	struct goose_out_logic *log = (struct goose_out_logic *)logic;
	struct goose_out_args *args = (struct goose_out_args *)arguments;
	u8 i;
	u32 stamp;
	u8 st_changed;


	st_changed=0;

	for(i=0;i<16;i++)
	{
		*log->state[i]=check_struct(&log->state_in[i])?1:0;
		if(*log->state[i]!=log->state_prev[i])
		{
			st_changed=1;
			log->state_prev[i]=*log->state[i];
		}
	}

	if(st_changed||log->first_packet)
	{
		log->retrans_cnt=0;
		log->retrans_cycle=0;

		stamp=((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_sec:cur_time.tv_sec);
		for(i=0;i<4;i++)
			*(log->tstamp_secs+i)=(stamp>>((3-i)*8));

		stamp = ((timesync_bits & CFG_TSYNC_USE_SWCLK)?cur_time_sw.tv_usec:cur_time.tv_usec)*GOOSE_TIMESTAMP_MS_MULTIPLIER;
		for(i=0;i<4;i++)
			*(log->tstamp_frac+i)=stamp>>((3-i)*8);
	}

	if(!log->retrans_cnt)
	{
		*log->simulation=check_struct(&log->test)?1:0;
		*log->ndscom=check_struct(&log->ndscom_in)?1:0;

		if(st_changed && !log->first_packet)
		{
			log->stnum_cur++;
			log->sqnum_cur=0;
		}

		for(i=0;i<4;i++)
			*(log->stnum+i)=log->stnum_cur>>((3-i)*8);

		for(i=0;i<4;i++)
			*(log->sqnum+i)=log->sqnum_cur>>((3-i)*8);

		log->sqnum_cur++;

		log->retrans_cnt = (2<<log->retrans_cycle);
		if(log->retrans_cnt>=(args->params.ttl>>1))
		{
			log->retrans_cnt=(args->params.ttl>>1);
			*log->ttl = (args->params.ttl)>>8;
			*(log->ttl+1) = (args->params.ttl);
		}
		else
		{
			*log->ttl = (log->retrans_cnt<<1)>>8;
			*(log->ttl+1) = (log->retrans_cnt<<1);
			log->retrans_cycle++;
		}

		if(args->params.bits & GOOSE_OUT_VLAN_EN)
			omapl138EthSendPacket(log->goose_out_frame,(log->len+18)>64?(log->len+18):64);
		else
			omapl138EthSendPacket(log->goose_out_frame,(log->len+14)>64?(log->len+14):64);

		log->first_packet=0;

	}

	if(log->retrans_cnt>=LOOP_CYCLE_MS)
		log->retrans_cnt-=LOOP_CYCLE_MS;
	else
		log->retrans_cnt=0;

}