/*
 * goose.c
 *
 *  Created on: 16-07-2020
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "eth.h"
#include "goose.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 <ti/sysbios/hal/Cache.h>
#include <string.h>
#include <stdio.h>

//TODO: stnum i seqnum dokladniej analizowac?
// 		dorobic lub sprawdzac wyjscia QUALITY?

//#pragma LOCATION(gqueue,0xcb800000) // @ non-cached memory above 128MB
struct goose_queue gqueue = { .cur = 0, .next = 0, .lost = 0 };

void iec61850_goose_recv(struct goose_frame *gfr,int len,char type)
{
	if(!(gqueue.status[gqueue.next] & (GOOSE_TO_PROCESS)))
	{
		if(htons(gfr->len)==len && len<=GOOSE_MAX_FR_SIZE)
		{
			memcpy(gqueue.buf[gqueue.next],(char*)gfr,len);
			gqueue.status[gqueue.next]|=GOOSE_TO_PROCESS;
			gqueue.next++;
			gqueue.next%=GOOSE_QUEUE_DEPTH;
		}
	}
	else
		gqueue.lost++;
}

int goose_initlog(void *arguments, void *logic)
{
	struct goose_args *args = (struct goose_args *)arguments;
	struct goose_logic *log = (struct goose_logic *)logic;
	u8 i;
	int values[6];
	u8 dstaddr[6];

	if(set_float_ptr(args->io.recv_cnt_float_out,&log->recv_cnt))
		return -1;

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

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

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

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

	gqueue.lost=0;

	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;

	emac_hash_add(EMAC_0_BASE,dstaddr);


	return 0;
}


void goose(void *arguments, void *logic) // not used, replaced by goose_drv.c
{
	struct goose_logic *log = (struct goose_logic *)logic;
	struct goose_args *args = (struct goose_args *)arguments;
	struct goose_frame *gfr;
	u8 *pdu;
	u8 *pdu_prev;
	u8 matched_crit;
	int len,pdu_len;
	int bytes_left, bytes_left_prev;
	int err;
	u8 tag;
	int alldata_len;
	int bytes_left_alldata;
	u8 *pdu_alldata;
	u8 data_cnt;
	u8 i;
	u32 sqnum,stnum,ttl,cfg_rev;

	//*log->recv_cnt=log->processed;
	//*log->lost=gqueue.lost;

	if(log->ttl)
		log->ttl--;

	check_and_set_struct(!log->ttl,&log->expired);

	while(gqueue.status[gqueue.cur] & (GOOSE_TO_PROCESS))
	{

		err = 0;
		gfr = (struct goose_frame *)gqueue.buf[gqueue.cur];

		if(htons(gfr->appid)==args->params.appid)
		{
			matched_crit = 0;

			bytes_left=htons(gfr->len)-8;
			pdu = (u8 *)&gfr->pdu_tag;

			while(bytes_left>=2 && !err)
			{
				len = asn_parse(&pdu,&bytes_left,&tag);

				if(len<0)
				{
					err=1;
					break;
				}

				if(tag==GOOSE_TAG_PDU)
				{
					pdu_len = len;
					pdu_prev = pdu;
					bytes_left_prev = bytes_left;
					bytes_left = pdu_len;

					while(bytes_left>=2 && !err)
					{
						len = asn_parse(&pdu,&bytes_left,&tag);
						if(len<0)
						{
							err=1;
							break;
						}

						switch(tag)
						{
							case GOOSE_TAG_GOCBREF:
								// process data (compare LEN bytes with STR)
								if(len!=strlen(log->goose_cbref) || memcmp(log->goose_cbref,pdu,len))
									err=1;
								else
									matched_crit++;

								pdu+=len;
								bytes_left-=len;
								break;

							case GOOSE_TAG_DATSET:
								// process data (compare LEN bytes with STR)
								if(len!=strlen(log->goose_dataset) || memcmp(log->goose_dataset,pdu,len))
									err=1;
								else
									matched_crit++;

								pdu+=len;
								bytes_left-=len;
								break;

							case GOOSE_TAG_CONFREV: //dodac bytes left-=len ?!
								if(len>4)
									err=1;
								else
								{
									cfg_rev=0;

									for(i=0;i<len;i++)
									{
										cfg_rev|=*pdu;
										pdu++;

										if((i+1)<len)
											cfg_rev<<=8;
									}

									if(args->params.cfg_rev==cfg_rev)
										matched_crit++;
									else
										err=1;
								}
								break;

							case GOOSE_TAG_TTL: //dodac bytes left-=len ?!
								if(len>4)
									err=1;
								else
								{
									ttl=0;

									for(i=0;i<len;i++)
									{
										ttl|=*pdu;
										pdu++;

										if((i+1)<len)
											ttl<<=8;
									}

									matched_crit++;
								}

								break;

							case GOOSE_TAG_STNUM: //dodac bytes left-=len ?!
								if(len>4)
									err=1;
								else
								{
									stnum=0;

									for(i=0;i<len;i++)
									{
										stnum|=*pdu;
										pdu++;

										if((i+1)<len)
											stnum<<=8;
									}

									matched_crit++;
								}
								break;

							case GOOSE_TAG_SQNUM: //dodac bytes left-=len ?!
								if(len>4)
									err=1;
								else
								{
									sqnum=0;

									for(i=0;i<len;i++)
									{
										sqnum|=*pdu;
										pdu++;

										if((i+1)<len)
											sqnum<<=8;
									}

									matched_crit++;
								}
								break;

							case GOOSE_TAG_ID:
								// process data (compare LEN bytes with STR)
								if(len!=strlen(log->goose_id) || memcmp(log->goose_id,pdu,len))
									err=1;
								else
									matched_crit++;

								pdu+=len;
								bytes_left-=len;
								break;

							case GOOSE_TAG_ALLDATA:
								data_cnt = 0;
								alldata_len = len;
								bytes_left_alldata = bytes_left;
								pdu_alldata = pdu;
								bytes_left=alldata_len;

								while(bytes_left>=2 && !err)
								{
									len = asn_parse(&pdu,&bytes_left,&tag);
									if(len<0)
									{
										err=1;
										break;
									}

									switch(tag)
									{
										case GOOSE_DATA_STRUCT:
											break;
										default:
											for(i=0;i<8;i++)
												if(data_cnt==args->params.data_index[i] && tag==GOOSE_DATA_BOOLEAN)
												{
													log->values&=~(1<<i);
													log->values|=*pdu?(1<<i):0;
													matched_crit++;
												}

											pdu+=len;
											bytes_left-=len;
											data_cnt++;
											break;
									}
								}

								pdu=pdu_alldata+alldata_len;
								bytes_left=bytes_left_alldata-alldata_len;
								break;

							default:
								pdu+=len;
								bytes_left-=len;
								break;
						}
					}

					if(!err && matched_crit==15) // matched goose cfgrev,cbref,datset,ttl,sqnum,stnum,goid+8 states
					{

						//check sqnum/stnum here
						if((sqnum>=(log->sqnum+1) && stnum==log->stnum)
							|| stnum>=log->stnum+1
							|| !log->ttl)
						{
							//log->processed++;
							*log->recv_cnt=(*log->recv_cnt)+1;
							log->stnum=stnum;
							log->sqnum=sqnum;
							log->ttl=ttl;

							for(i=0;i<8;i++)
								check_and_set_struct(log->values & (1<<i),&log->state[i]);
						}
						break;
					}

					pdu=pdu_prev+pdu_len;
					bytes_left=bytes_left_prev-pdu_len;
				}
				else
				{
					pdu+=len;
					bytes_left-=len;
				}
			}
		}

		gqueue.status[gqueue.cur]=0;
		gqueue.cur=((gqueue.cur+1)%GOOSE_QUEUE_DEPTH);
	}
}
