// $RCSfile: tmtc_if.c,v $Revision: 1.18 $Date: 2005/03/11 15:08:57
/*------------------- includes ---------------------------------------------*/
#include "allnodes.h"
#include "node1.h"
#include <stdlib.h>
#include "configura.h"
#include "MilDef.h"
#include "tmtc_if.h"
#include "conf1553.h"
#include "pubfuncs.h"
#include "tables.h"
#include "time_tsk.h"
#include "err_hdl.h"
#include "MM_21020.h"
#include "MM_misc.h"
#include "init1553.h"

/*------------------- local functions ---------------------------------------------*/
void UpLoad_Packet_demux (TM_MSG * , int);
void UpLoad_Packet16 (TM_MSG * , int);
void UpLoad_Packet24 (TM_MSG * , int);
int check_pkt_enable (TM_MSG *);
void get_counter (int *);

/*------------------- global variables ---------------------------------------------*/
unsigned int ICU_Prime_Redundant;

// tmtc : Virtuoso Task for 1553 communication. SpaceCraft-InterFace.
/*------------------- task tmtc ---------------------------------------------*/
void tmtc (void)
{
	TM_MSG msgReader;  // check if global is better
	while (1)
	{
		KS_EventTestW(ISR_1553_EVENT);
		// Start of TM UpLoader
		while (FreePackDPRAM > 0)
			if (KS_FIFOGet(EVENT_TM_QUEUE, &msgReader) != RC_FAIL)
			{
				((int*)(msgReader.block.pointer_to_data))[0] |= ICU_Prime_Redundant; // update APID
				if ( check_pkt_enable(&msgReader) == ENABLED)  // packet trx control
					UpLoad_Packet_demux(&msgReader, EVT);
				else 
					if ( msgReader.count == 0 )
						release_block( &(msgReader.block) );
			}	
			else
				if (KS_FIFOGet(HK_TM_QUEUE, &msgReader) != RC_FAIL)
				{
					((int*)(msgReader.block.pointer_to_data))[0] |= ICU_Prime_Redundant; // update APID
					if ( check_pkt_enable(&msgReader) == ENABLED)  // packet trx control
						UpLoad_Packet_demux(&msgReader, HK);
					else 
						if ( msgReader.count == 0 ) 
							release_block( &(msgReader.block) );
				}
				else
					if (KS_FIFOGet(SD_TM_QUEUE, &msgReader) != RC_FAIL)
					{
						if ( check_pkt_enable(&msgReader) == ENABLED)  // packet trx control
							UpLoad_Packet_demux(&msgReader, SCI);
						else 
							if ( msgReader.count == 0 ) 
								release_block( &(msgReader.block) );
					}
					else
						break;  // in this case there are no entries in the fifos
		// End of TM UpLoader

		if (FreeCmndDPRAM  < MaxCmndDPRAM)
			DownLoad_Packet((TC_packet*)NULL);
	}  // end while (1)
}  // end of function tmtc


//////////////////////////////////////////////////////////////////////////////
/* UpLoad_Packet_demux :
/		   try to send a packet.
/		   it presumes that there's place on 1553CBDP_RAM
/		   by calling checkFreeDPRAM()-check before entry
*/

/*--------------------- UpLoad_Packet_demux --------------------------------------------*/
void UpLoad_Packet_demux (TM_MSG * cmsg, int msgtype )
{
	if (msgtype == EVT)
		UpLoad_Packet16(cmsg, msgtype);
	else
		switch (0x0000ff & cmsg->packing)
		{
			case 1:
				UpLoad_Packet16(cmsg, msgtype);
				break;
			case 2:
				UpLoad_Packet24(cmsg, msgtype);
				break;
			default:
				// runtime error (the parameters are the first 18 bytes of the tm packet)
				generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_TMTC_INVALID_PACKING_FIELD, 9, (int*) cmsg->block.pointer_to_data);
				// release the pool memory block if necessary
				if (cmsg->count == 0) release_block( &(cmsg->block) );
				break;
		}
}

/*--------------------- UpLoad_Packet16 --------------------------------------------*/
void UpLoad_Packet16 (TM_MSG * cmsg, int msgtype)
{
	int * hBuffer;
	int * dBuffer;
	int * cBuffer;
	int p_total, dhead;
	int pktlength_b, pktlength_w;
	struct TM_packet tmp_TM_packet;
	PACKD_TIME_STAMP * p_time;
	
    if ((cmsg->packing >> 16) == SCIP)
	{
		// packet is science data
		
		// number of word of this msg //9=3(SPH)+5(DFH)+1(CRC)//9+SDH+data len
		p_total = cmsg->length + 9 + ((cmsg->packing >> 8) & 0xff);
		pktlength_b = ((p_total-3) * 2) - 1;  // number of bytes of this msg

		hBuffer = (int*)(cmsg->block.pointer_to_data);
		dBuffer = (int*)(cmsg->block.pointer_to_data + cmsg->offset);
		cBuffer = (int*)(&tmp_TM_packet);
		dhead = 8 + ((cmsg->packing >> 8) & 0xff);  //SPH+DFH+SDH
		
		// refresh time stamp (spr462)
		p_time = (PACKD_TIME_STAMP *) &(hBuffer[5]);
		get_TS( p_time );
		
		///conversion (16 broken, 16 contiguous) cycle begin 
		adicpy (cBuffer, hBuffer, dhead);
		adicpy (cBuffer + dhead, dBuffer, cmsg->length);
		///conversion (16 broken, 16 contiguous) cycle end

		get_counter(cBuffer);  // this function writes the packet sequence counter
		cBuffer[2] = pktlength_b;
		if ((msgtype == SCI) && (cBuffer[8]>8))  // science packet with SID>8 (not a start frame, but a data report)
		{
			cBuffer[ 15 ] = cBuffer[ 14 ] - cmsg->count - 1;
			cBuffer[ 16 ] = cmsg->first_ch;  // ADG: new cmd specs.
        }

        UpLoad_Packet(cBuffer);

		if (cmsg->count == 0)
			release_block( &(cmsg->block) );
	}
	else  
	{
		// packet is not science
    	hBuffer = (int*) cmsg->block.pointer_to_data;
		cBuffer = (int*) &tmp_TM_packet;  
		// total number of 16-bit words in the packet
		pktlength_w = (hBuffer[2] + 7) >> 1;  // pktlength_w = ((packet lenght+1)+6)/2

		adicpy(cBuffer, hBuffer, pktlength_w);  // copy of the packet 

		get_counter(cBuffer);  // this function writes the packet sequence counter

		UpLoad_Packet(cBuffer);

		if (cmsg->count == 0)
			release_block( &(cmsg->block) );
    }  
}

/*--------------------- UpLoad_Packet24 --------------------------------------------*/
void UpLoad_Packet24 (TM_MSG * cmsg, int msgtype)
{
	unsigned int i, j, index, SID;
	int * hBuffer;
	int * dBuffer;
	int pktlength_b;
	int pktlength_w;
	int dhead;
	int * cBuffer;
	struct TM_packet tmp_TM_packet;
	PACKD_TIME_STAMP * p_time;

    dhead = 8 + ((cmsg->packing >> 8) & 0xff);  //SPH+DFH+SDH
	hBuffer = (int*)(cmsg->block.pointer_to_data);
	dBuffer = (int*)(cmsg->block.pointer_to_data + cmsg->offset);
	cBuffer = (int*)(&tmp_TM_packet);

	// refresh time stamp (spr462)
	p_time = (PACKD_TIME_STAMP *) &(hBuffer[5]);
	get_TS( p_time );
		
	// conversion cycle (16/16) begin
	adicpy (cBuffer, hBuffer, dhead);

	SID = cBuffer[8];

	// conversion cycle (24/16) begin
	cBuffer+= dhead;	//pointer to begin of data section
	index = ((cmsg->length) &0xFFFFFFFE );		// {0x00AABBCC,0x00DDEEFF} --> {0X0000AABB,0X0000CCDD,0X0000EEFF}
	for (i=0,j=0;i<index;){
			cBuffer[j++]	=	dBuffer[i]>>8;				//0x0000AABB
			cBuffer[j]		=	(dBuffer[i++]<<8)&0xFF00;	//0x0000CC00
			cBuffer[j++]	|=	(dBuffer[i]>>16)&0X00FF;	//0x0000CCDD
			cBuffer[j++]	=	dBuffer[i++] & 0xffff;		//0x0000EEFF
		}
	if ((cmsg->length)& 0x00000001)
		{
			cBuffer[j++]	=	dBuffer[i]>>8;				//0x0000AABB // added >>8
			cBuffer[j++]	=	(dBuffer[i++]<<8)&0xFF00;		//0x0000CC00 // added j++
		}		 // now j rapresents the totals of word16bit - 8
	if ( ( msgtype == SCI ) && ( SID < 9 ) )  // start frame not a data report)
		{
			cBuffer[j++] = dBuffer[i] & 0xffff;    
		}

	cBuffer-=dhead;

	pktlength_w = j + dhead + 1;
	pktlength_b = ((pktlength_w-3) * 2) - 1;
	// conversion cycle (24/16) begin

	get_counter(cBuffer);  // this function writes the packet sequence counter
	cBuffer [2] = pktlength_b;
	if ( ( msgtype == SCI ) && ( SID > 8 ) )  // science packet with SID>8 (not a start frame, but a data report)
	{
		cBuffer[ 16 ] = cmsg->first_ch;    //ADG: new cmd specs.
		cBuffer[ 15 ] = cBuffer[ 14 ] - cmsg->count - 1;
	}
	UpLoad_Packet(cBuffer);

	if ( cmsg->count == 0 )
		release_block( &(cmsg->block) );
}

//--------------- check_pkt_enable ---------------------------------------------
//! checks if the TM packet associated with the structure pointed by argument cmsg is enabled for 
// transmission: returns 1 (ENABLED) if enabled, 0 (DISABLED) if not.
int check_pkt_enable (TM_MSG * cmsg)
{	
	struct TM_packet * tm_pack;
	unsigned int type, subtype, pack_id, i, param[3];
	
	// extraction of type and subtype
	tm_pack = (struct TM_packet *) cmsg->block.pointer_to_data;
	type = tm_pack->data_field_header[0] & 0x0FF;  // type of the TM packet
	subtype = (tm_pack->data_field_header[1] >> 8) & 0x0FF;  // subtype of the TM packet
		
	switch (type)  // extraction of the packet id, which depends on the type
	{
		case HK_DIAG_REPORT:  // type 3
		case EVENT_REPORT:  // type 5
		case FUNC_STATUS_REPORT:  // type 8
		case SCIENCE_DATA:  // type 21
			pack_id = tm_pack->data[0];
			break;
		default:
			pack_id = 0;
	}
	
	for (i = 0; i < DIM_PKT_ENAB_VECT; i++) 
		if ((type == pkt_enab[i].type) && (subtype == pkt_enab[i].subtype) && (pack_id == pkt_enab[i].pktid))
			return (pkt_enab[i].enab);
			
	// combination (type, subtype, pack_id) has not been found in the array pkt_enab[] : issue an error
	param[0] = type;
	param[1] = subtype;
	param[2] = pack_id;
	generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_TMTC_TRX_CTRL_PKT_NOT_FOUND, 3, param);	
	// combination (type, subtype, pack_id) not found: we decide to let the TM packet pass
 	return ENABLED;
}

// ----------------------- get_counter -------------------------------------
//! For each of the possible APID values, counts the number of TM packets 
// effectively transmitted; the sequence number is written into the second 
// 16-bit word of the TM packet.
static unsigned int Source_counter[8] = {0, 0, 0, 0, 0, 0, 0, 0};		//!< APID couters
void get_counter (int * cBuff)
{
	unsigned int apid;
	
	apid = cBuff[0] & 0x07FF;  // extract apid from first 16-bit word of the TM packet
	// and then writes the correct sequence counter into the second word
	// and store the counter for restoring it after a reset
	switch (apid)
	{
		case 1024 :		// default apid (prime/redundant)
		case 1025 :
			cBuff[1] |= IFSI_MOD((Source_counter[0]++) , MOD14);
			* ( (unsigned int pm * ) BOOT_SEQ_COUNTER ) = Source_counter[0]-1;
			break;
		case 1026 : 	// hk apid (prime/redundant)
		case 1027 :
			cBuff[1] |= IFSI_MOD((Source_counter[1]++) , MOD14);
			* ( (unsigned int pm * ) OBSW_APID_2 ) = Source_counter[1]-1;
			break;
		case 1028 :  // science from HRS-H
			cBuff[1] |= IFSI_MOD((Source_counter[2]++) , MOD14);
			* ( (unsigned int pm * ) OBSW_APID_3 ) = Source_counter[2]-1;
			break;	
		case 1029 :  // science from HRS-V
			cBuff[1] |= IFSI_MOD((Source_counter[3]++) , MOD14);
			* ( (unsigned int pm * ) OBSW_APID_4 ) = Source_counter[3]-1;
			break;
		case 1030 :  // science from WBS-H
			cBuff[1] |= IFSI_MOD((Source_counter[4]++) , MOD14);
			* ( (unsigned int pm * ) OBSW_APID_5 ) = Source_counter[4]-1;
			break;	
		case 1031 :  // science from WBS-V 
			cBuff[1] |= IFSI_MOD((Source_counter[5]++) , MOD14);
			* ( (unsigned int pm * ) OBSW_APID_6 ) = Source_counter[5]-1;
			break;
		default :  // error: invalid APID
			generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_TMTC_APID_IN_GET_COUNTER, 1, &apid);
			break;
	}
}

//----------------------- init_APID_400_counter -------------------------------------------
// ! Inits the counter for packets with APID 400
void init_APID_counter(unsigned int * counters)
{
	Source_counter[0] = counters[0];	// base apid   
	Source_counter[1] = counters[4];	// hk apid
	Source_counter[2] = counters[5];	// science hrs h apid
	Source_counter[3] = counters[6];	// science hrs v  apid
	Source_counter[4] = counters[7];	// science wbs h  apid
	Source_counter[5] = counters[8];	// science wbs v apid
}

//-----------------------  adicpy  -------------------------------------------
// ! copies len words (32bit) from src to dst
// Note that,for compatibility reasons, the declaration of this function is in mem21020.h
void adicpy (unsigned int * p_dst, unsigned int * p_src, unsigned int len)
{
	while(len--)  *p_dst++=*p_src++;
}