/*
 * eth.c
 *
 *  Created on: 22-03-2019
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "eth.h"
#include "mkstream.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>

struct udp_pcb *listen_pcb_udp;
struct udp_pcb *bcast_pcb_udp;
struct udp_pcb *stream_pcb_udp;
struct pbuf *udp_p_bcast = NULL;
struct pbuf *udp_p_stream = NULL;
struct pbuf *udp_ping = NULL;
struct ip_addr unicast_ip;

//#pragma LOCATION(str,0xcd000000) // @ non-cached memory above 128MB
/*volatile*/ struct stream_frame str[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE] __attribute__((aligned(4)));
volatile u8 str_bits[ETH_MAX_RECV_STREAMS][ETH_STREAM_BUF_SIZE] __attribute__((aligned(4)));
volatile u32 str_pos[ETH_MAX_RECV_STREAMS] __attribute__((aligned(4)));

volatile u8 ping_seq_sent=0;
volatile u32 ping_time=0;
volatile u8 ping_adr=0;

#define ETH_NO_TZ_CHECKSUM // don't check the checksum inside a TZ frame

struct str_eth_frame r87l_tx_packet __attribute__((aligned(4)));

const u8 zprae_multicast_pool[] = {0x01,0x00,0x5e,'Z','P','6'};

void ethernet_recv(char *buf)
{
	struct stream_frame *s = (struct stream_frame *)buf;
	struct ping_info *ping = (struct ping_info *)(buf+(sizeof(struct stream_frame)));

	if(s->addr<ETH_MAX_RECV_STREAMS)
	{
		memcpy((u8 *)&str[s->addr][str_pos[s->addr]],(u8*)s,sizeof(struct stream_frame));
		str_bits[s->addr][str_pos[s->addr]++]=0;
		str_pos[s->addr]%=ETH_STREAM_BUF_SIZE;

		if((ping->adr & 0x7F) < ETH_MAX_STREAMS) // Additional PING NFO
		{
			if(ping->adr & 0x80) // ping reply
			{
				if(ping_seq_sent==ping->seq)
				{
					u32 pwm = PWM1_GET(PWM1_TBCNT);

					if(pwm>=ping->pwmval)
						ping_time = pwm-ping->pwmval;
					else
						ping_time = pwm+65536-ping->pwmval;
				}
			}
			else				  // ping request
			{
				ping_nfo.adr=ping_adr | 0x80;
				ping_nfo.seq=ping->seq;
				ping_nfo.pwmval=ping->pwmval;
				ping_nfo.tstamp=bus_an_cur_timestamp;
			}
		}
	}
}

int ethernet_initlog(void *arguments, void *logic)
{
	struct eth_args *args = (struct eth_args *)arguments;
	struct eth_logic *log = (struct eth_logic *)logic;
	u8 i;

	for(i=0;i<6;i++)
		r87l_tx_packet.src_mac[i]=eth.hwaddr[i];

	for(i=0;i<6;i++)
		r87l_tx_packet.dst_mac[i]=zprae_multicast_pool[i];

	r87l_tx_packet.dst_mac[5]=args->params.multicast_id;

	emac_hash_add(EMAC_0_BASE,r87l_tx_packet.dst_mac);

	r87l_tx_packet.lenh=0;
	r87l_tx_packet.lenl=sizeof(struct stream_frame)+sizeof(struct ping_info)+4; //4 bytes cksum
	r87l_tx_packet.padding1=0;
	r87l_tx_packet.str.hdr[0]='T';
	r87l_tx_packet.str.hdr[1]='Z';

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

	if(set_pointer_in_ptr(args->io.stream_ptr_in,(u32 *)&log->stream_in))
		return -1;

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

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

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

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

	if(set_pointer_out_ptr(args->io.stream0_ptr_out,(u32 *)&log->stream_out[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream0_act_out,&log->stream_act[0]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream1_ptr_out,(u32 *)&log->stream_out[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream1_act_out,&log->stream_act[1]))
		return -1;
	if(set_pointer_out_ptr(args->io.stream2_ptr_out,(u32 *)&log->stream_out[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.stream2_act_out,&log->stream_act[2]))
		return -1;

	if(set_bit_ptr_struct(args->io.link0_out,&log->link[0]))
		return -1;
	if(set_float_ptr(args->io.stream0_errs_float_out,&log->stream_errs[0]))
		return -1;

	if(set_bit_ptr_struct(args->io.link1_out,&log->link[1]))
		return -1;
	if(set_float_ptr(args->io.stream1_errs_float_out,&log->stream_errs[1]))
		return -1;

	if(set_bit_ptr_struct(args->io.link2_out,&log->link[2]))
		return -1;
	if(set_float_ptr(args->io.stream2_errs_float_out,&log->stream_errs[2]))
		return -1;

	if(set_float_ptr(args->io.ping_float_out,&log->ping_time))
		return -1;

	if(set_float_ptr(args->io.stream0_synt_errs_float_out,&log->stream_synt_errs[0]))
		return -1;
	if(set_float_ptr(args->io.stream1_synt_errs_float_out,&log->stream_synt_errs[1]))
		return -1;
	if(set_float_ptr(args->io.stream2_synt_errs_float_out,&log->stream_synt_errs[2]))
		return -1;

	log->s_delay=args->params.sample_delay/LOOP_CYCLE_MS;
	if(args->params.sample_delay%LOOP_CYCLE_MS)
		log->s_delay++;

	log->s_delay*=(SAMPLES_PER_MS*LOOP_CYCLE_MS);

	if(log->s_delay>=((ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS)))
		log->s_delay=(ETH_STREAM_BUF_SIZE-1)*(SAMPLES_PER_MS*LOOP_CYCLE_MS);

	ping_nfo.adr=0xFF;

	return 0;
}

void ethernet(void *arguments, void *logic)
{
	struct eth_logic *log = (struct eth_logic *)logic;
	struct eth_args *args = (struct eth_args *)arguments;

	u8 i,k;
#ifndef ETH_NO_TZ_CHECKSUM
	u8 j,cksum;
#endif
	u16 find_stamp;
	u8 already_incremented[ETH_MAX_STREAMS]={0,0,0};
    //struct str_eth_frame *eth_str = (struct str_eth_frame *)r87l_tx_packet;

	if(check_struct(&log->clr))
	{
		for(i=0;i<ETH_MAX_STREAMS;i++)
		{
			*log->stream_errs[i]=0;
			*log->stream_synt_errs[i]=0;
		}
	}

	for(i=0;i<ETH_MAX_STREAMS;i++)
	{
		if(log->prev_pos[i]!=str_pos[i])
		{
			if(log->link_up_cnt[i]>LINK_UP_TIME)
			{
				if(!check_struct(&log->link[i]))
					*log->stream_errs[i]=0;
				set_struct(&log->link[i]);
			}
			else
				log->link_up_cnt[i]+=LOOP_CYCLE_MS;

			while(log->prev_pos[i]!=str_pos[i])
			{
				u16 prev_tstamp=(!log->prev_pos[i]?str[i][ETH_STREAM_BUF_SIZE-1].sample_stamp:str[i][log->prev_pos[i]-1].sample_stamp) & 0x7FFF;
				u16 next_tstamp=(str[i][log->prev_pos[i]].sample_stamp)&0x7FFF;

				if((prev_tstamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)!=next_tstamp)
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->prev_pos[i]++;
				log->prev_pos[i]%=ETH_STREAM_BUF_SIZE;
			}

			log->link_timeout_cnt[i]=0;
			log->prev_pos[i]=str_pos[i];
		}
		else
		{
			if(log->link_timeout_cnt[i]>LINK_TIMEOUT_TIME)
			{
				clear_struct(&log->link[i]);
				log->link_up_cnt[i]=0;
			}
			else
			{
				if(log->link_timeout_cnt[i]>(1*LOOP_CYCLE_MS)) // at least 2ms between samples
				{
					if((log->prev_timestamp+(SAMPLES_PER_MS*LOOP_CYCLE_MS))%(SAMPLES_PER_MS*1000)==bus_an_cur_timestamp)
						*log->stream_errs[i]+=1.0f;
					already_incremented[i]=1;
				}
				log->link_timeout_cnt[i]+=LOOP_CYCLE_MS;
			}
		}
	}

	if(bus_an_cur_timestamp>=log->s_delay/*ETH_BUF_DELAY*/)
		find_stamp=bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY;
	else
		find_stamp=(SAMPLES_PER_MS*1000)+bus_an_cur_timestamp-log->s_delay;//ETH_BUF_DELAY;

	for(i=0;i<ETH_MAX_STREAMS;i++)
	{
		if(!check_struct(&log->link[i]))
		{
			clear_struct(&log->stream_act[i]);
			continue;
		}
		for(k=0;k<ETH_STREAM_BUF_SIZE;k++)
		{
			if((args->params.bits & ETH_NO_SYNC || str[i][k].sample_stamp == find_stamp) && !(str_bits[i][k] & STATUS_FRAME_READ))
			{
#ifndef ETH_NO_TZ_CHECKSUM
				cksum=CKSUM_IV;
				for(j=0;j<sizeof(struct stream_frame) - 1;j++)
					cksum+=*((u8*)&str[i][k]+j);
#endif
				str_bits[i][k]|=STATUS_FRAME_READ;

#ifndef ETH_NO_TZ_CHECKSUM
				if(cksum == str[i][k].cksum)
#endif
				{
					memcpy((u8*)&log->stream_out[i],(u8*)&str[i][k],sizeof(struct stream_frame));
					break;
				}
			}
		}

		if(k<ETH_STREAM_BUF_SIZE)
			set_struct(&log->stream_act[i]);
		else
		{
			clear_struct(&log->stream_act[i]);
			if(!already_incremented[i]) // wyrzucono bledy od synT z licznikow
				*log->stream_synt_errs[i]+=1.0f;
		}
	}

	if(eth.flags & ETH_INITIALIZED) // eth active
	{
		ping_adr=log->stream_in->addr;

		if(check_struct(&log->ping))
		{
			if(log->tcnt>=5000)
			{
				ping_nfo.adr=ping_adr;
				ping_nfo.seq++;
				ping_seq_sent=ping_nfo.seq;
				ping_time=0xFFFFFFFF;
				ping_nfo.pwmval=PWM1_GET(PWM1_TBCNT);
				ping_nfo.tstamp=bus_an_cur_timestamp;
			}
		}

		if(check_struct(&log->stream_en))
		{
			memcpy((u8*)&r87l_tx_packet.str,(u8 *)log->stream_in,sizeof(struct stream_frame));

			if(ping_nfo.adr != 0xFF) // need send ping request or reply
				memcpy((u8*)&r87l_tx_packet.ping,(u8*)&ping_nfo,sizeof(struct ping_info));
			else
				r87l_tx_packet.ping.adr=0xFF;

			ping_nfo.adr=0xFF;
			omapl138EthSendPacket((u8*)&r87l_tx_packet,sizeof(struct str_eth_frame));
		}
		else
		{
			if(ping_nfo.adr != 0xFF) // need to send ping request or reply
			{
				memcpy((u8*)&r87l_tx_packet.str,(u8 *)log->stream_in,sizeof(struct stream_frame));
				memcpy((u8*)&r87l_tx_packet.ping,(u8*)&ping_nfo,sizeof(struct ping_info));
				ping_nfo.adr=0xFF;
				omapl138EthSendPacket((u8*)&r87l_tx_packet,sizeof(struct str_eth_frame));
			}
		}

		//my stream
		memcpy((u8 *)&str[log->stream_in->addr][str_pos[log->stream_in->addr]],(u8*)log->stream_in,sizeof(struct stream_frame));
		str_bits[log->stream_in->addr][str_pos[log->stream_in->addr]++]=0;
		str_pos[log->stream_in->addr]%=ETH_STREAM_BUF_SIZE;

		if(ping_time!=log->ping_time_prev)
		{
			*log->ping_time = ping_time==0xFFFFFFFF?999999.0f:(float)ping_time*0.000213219616f;
			log->ping_time_prev=ping_time;
		}


		if(log->tcnt >= 5000)
			log->tcnt=0;
		else
			log->tcnt+=LOOP_CYCLE_MS;
	}

	check_and_set_struct((eth.flags & ETH_INITIALIZED),&log->alive);
	log->prev_timestamp=bus_an_cur_timestamp;
}
