// $RCSfile: time_tsk.c,v $Revision: 1.8 $Date: 2005/03/11 15:08:57
#include "allnodes.h"
#include "node1.h"
#include <stdlib.h>
#include "configura.h"
#include "tmtc_if.h"
#include "pubfuncs.h"
#include "cmd_seq.h"
#include "err_hdl.h"
#include "init1553.h"
#include "MilConf.h"
#include "time_tsk.h"
#include "MM_21020.h"


/* ----------------------- Local vars, macros and functions  ------------------------------- */
#define ONE_SECOND 0x1312D00 /* Number of clocks in one second (20 MHz) */
void get_time(struct time_struct_at_sync * p_time);
static unsigned int CDMS_current_time[4];
static unsigned int CDMS_previous_time[4];

/* ----------------------- Globals ------------------------------- */
struct time_struct_at_sync Timekeeper;

//---time_tsk -------------------------------------------------------------------------//
void time_tsk (void)
{
	CDMS_current_time[0]=0;
	CDMS_current_time[1]=0;
	CDMS_current_time[2]=0;
	CDMS_current_time[3]=0;
	
	//Read data from rx at current sa into current position
	MilBlockWrite(MilRTConf, Tx_data_han8->m_AbsAddr,  (int *) &CDMS_current_time, 4);
	
//	the following to be confirmed!!!! ADG
	MilBlockRead( MilRTConf, Rx_data_han8->m_AbsAddr, (int *) &CDMS_previous_time, 4);

	while (1)
	{
		if(KS_EventTestW(TS_EVENT) != RC_OK) continue; // Event set by ISR

		/// Retransmit the time received, as acknowledge
		MilBlockRead( MilRTConf, Rx_data_han8->m_AbsAddr, (int *) &CDMS_current_time, 4);
		MilBlockWrite( MilRTConf, Tx_data_han8->m_AbsAddr, (int *) &CDMS_current_time, 4);

		andmask ( CDMS_previous_time, 4, 0x0000FFFF);
		andmask ( CDMS_current_time, 4, 0x0000FFFF);

		if 	( memcmp ( CDMS_previous_time, CDMS_current_time, 4 ) != 0 )
		{
			adicpy( CDMS_previous_time , CDMS_current_time, 4);
			
			// The following critical section is to prevent interrupts inbetween p_time building
			// copies the 1553 time stamp into the appropriate location
			__asm("bit clr mode1 0x1000;NOP;NOP;"); //	disables all interrupts
				Timekeeper.clock_at_sync = KS_HighTimerRead();  //HighTimerRead at synch
				Timekeeper.seconds       = (CDMS_current_time[1] << 16) + CDMS_current_time[2];
				Timekeeper.fractions     =  CDMS_current_time[3];
			__asm("bit set mode1 0x1000;NOP;NOP;"); //	enables all interrupts
		}
	}
}

//-------- get_TS --------------------------------------------------------------
void get_TS (PACKD_TIME_STAMP * ts_48)
{
	struct time_struct_at_sync time_of_packet;
	
	get_time(&time_of_packet);
	ts_48->seconds_high = (time_of_packet.seconds >> 16 )	& 0xFFFF;
	ts_48->seconds_low  = (time_of_packet.seconds       )	& 0xFFFF;
	ts_48->fraction     = (time_of_packet.fractions     )	& 0xFFFF;
}

/******************************************************************************
*Function name : get_time

*Purpose:
*	This function computes the ICU internal time. Every second the ICU receives
*	the time from the Bus Controller and the ICU internal clock using the
*	KS_HighTimerRead()(both activities are performed in a critical section 
*	in which all relevant interrupts are disabled)
*	and writes the two times into the Timekeeper global variable.
*	The procedure get_time reads again the ICU internal clock and 
*	computes how many cycles (assuming a clock of 20 MHz) elapsed from the last 
*	sync received (expressed in 1/65536 seconds).
*	In case a new sync is received during the reading of Timekeeper, the global
*	variable is read again

*Syntax:
*	get_time(struct time_struct_at_sync * p_time);

*Input:
*	p_time: the pointer to the structure containing the DPU time

*Output:
*	p_time: on output it contains
*		  ->clock_at_sync (the number of clocks at the last sync received)
*		  ->seconds (32 bits with the number of seconds)
*		  ->fractions (16 bits with fraction of seconds in 1/65536)

*Return:
*	none

******************************************************************************/

void get_time (struct time_struct_at_sync * p_time)
{
    unsigned int number_of_clocks;
    unsigned int seconds;
    int temp;
    float fraction;
    div_t result;

	p_time->clock_at_sync = Timekeeper.clock_at_sync;
	p_time->seconds       = Timekeeper.seconds;
	p_time->fractions     = Timekeeper.fractions;

    /* The following condition ensures that the DPU has not received a new synchro
      during these operations. In case, the whole process is repeated (only once,
      since it is not possible that two synchro are received) 
	  the new clock at sync is stored in the updated Timekeeper.clock_at_sync*/
    if (p_time->clock_at_sync != Timekeeper.clock_at_sync)
    {
		p_time->clock_at_sync = Timekeeper.clock_at_sync;
		p_time->seconds       = Timekeeper.seconds;
		p_time->fractions     = Timekeeper.fractions;
    }

//  the following to handle the hightimer wraparound.
//  Note that the Hightimer output is an int. 

	temp = KS_HighTimerRead();

	if (temp >= p_time->clock_at_sync)
	{
		number_of_clocks = temp - p_time->clock_at_sync;
	}
	else
	{
        number_of_clocks = (unsigned int)(~p_time->clock_at_sync + temp + 1); //PACS
//		number_of_clocks = (((0xFFFFFFFF - p_time->clock_at_sync) + temp) + 1);
	}	
					
	//  If elapsed time greater that 1 sec, 
	//  update number of seconds and number of clocks:
	if ( number_of_clocks >= ONE_SECOND )
	{
		// Here we use div() because we are sure that both are positive 
		result = div(number_of_clocks,ONE_SECOND);
		seconds = result.quot;
		number_of_clocks = result.rem;
	}
	else
	{
		seconds = 0;
	}

	/*
	Now number_of_clocks is less than 1 second. It is in fraction of 20 MHz,
	but we want it in 65536th of seconds. So we perform
	number_of_clocks*65536/20*10^6 or number_of_clocks*256/78125
	*/
	fraction = (float)(number_of_clocks) * 0.0032768;

	p_time->fractions += (unsigned int)fraction;
	if ( p_time->fractions > 0xFFFF )
	{
		p_time->fractions &= 0xFFFF;
		seconds++;
	}
	p_time->seconds += seconds;

/*
	p_time->fractions += (unsigned int)fraction;
	seconds += (p_time->fractions >>16);
	p_time->fractions &= 0xFFFF;
	p_time->seconds += seconds;
*/
	return;
}