// $RCSfile: cmd_exec.c,v $Revision: 1.38 $Date: 2005/03/11 15:08:57
//! Essentially a library files: contains functions that are called by cmd_seq (and by other tasks) to perform various activities
#include "cmd_exec.dox"
#include "allnodes.h"
#include "node1.h"
#include <stdlib.h>
#include "configura.h"
#include "main.h"
#include "hs0.h"
#include "pubfuncs.h"
#include "time_tsk.h"
#include "tables.h"
#include "err_hdl.h"
#include "hk_ask.h"
#include "ls.h"
#include "data_hdl.h"
#include "hs_hdl.h"
#include "hs_lib.h"
#include "MM_21020.h"
#include "ls_hdl.h"
#include "cmd_seq.h"
#include "vm_lib.h"
#include "MM_misc.h"
#include "cmd_exec.h"
#include "hs1.h"

//---- abort_measurement -------------------------------------------------------
//! Puts a start abort command in fifo HS_HDL_QUEUE towards task hs_hdl
void abort_measurement (void)
{
	HS_HDL_MSG msg;  // structure of command for hs_hdl
	
	if ( abort_is_running ) return; // there is already an abort attempt ongoing. Drop the command.

	abort_is_running = 1;  // prevent double abort
	
	current_command_number++;  // this is to stop the current command
	KS_SemaGroupSignal(Sema_Wait_Group);  // signal these semaphores to interrupt any sleep state of tasks hs_hdl and ls_hdl
	completion_update(NULL);  // this resets the global variables used for completion issues
	
	// flush the hs_hdl queue
	while (KS_FIFOGet(HS_HDL_QUEUE, &msg) == RC_OK) if (msg.block_loaded == 1) release_block(&(msg.block));
	
	// prepare the message
	msg.block_loaded = 0;
	msg.operation_id = START_ABORT;  // set the operation id
	msg.command_number = current_command_number;  // set the cmd number
	msg.exec_attempts = 0; // no exec attempt yet

	if (KS_FIFOPut(HS_HDL_QUEUE, (void *) &msg) == RC_FAIL)  // send to task hs_hdl
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_PUT_HS_HDL_QUEUE, 0, NULL);
		
	return;		
}

//----------- obs_ident --------------------------------------------------------
//! Writes ObsID & BBID coming with the TC packet into the corresponding global variables, and resets the transfer counters
void obs_ident (TC_packet * packet)
{
	unsigned int old_obs_id;
	
	old_obs_id = Observation_ID;
	Observation_ID	= PACK(packet->data[TC_OBS_IDX], packet->data[TC_OBS_IDX+1]);
	Cur_BuildingBlock_ID = PACK(packet->data[TC_BBID_IDX], packet->data[TC_BBID_IDX+1]);	
	if ( old_obs_id != Observation_ID) reset_transfer_cnt();  // reset the frames counters of the four spectrometers
}

//----- instrument_status ------------------------------------------------------
//! updates global variables SubsysStatus, Active_spectr according to the SS status word contained in the TC(8,4,2,0)
void instrument_status (TC_packet * packet)
{
	int Active_spectr_prev, reset_fifo_mask = 0;

	Active_spectr_prev = Active_spectr;  // store the previous value of the global variable Active_spectr
	Active_spectr = 0;  // reset the global variable
	
	// update global variable SubsysStatus with the value from the TC
	SubsysStatus = packet->data[TC_SSSTAT_IDX];  
	
	// update variable used by hk ask to poll subsystems
	HK_SubsysStatus = SubsysStatus;
	
	// update global variable Active_spectr: it contains the same information as SubsysStatus,
	// but restricted to the four spectrometers and coded in a different way (the way the
	// IRQ mask register requires it)
	if ((SubsysStatus & HRS_H_MASK) != 0) Active_spectr |= FIFO2_SS_MASK;
	if ((SubsysStatus & HRS_V_MASK) != 0) Active_spectr |= FIFO3_SS_MASK;
	if ((SubsysStatus & WBS_H_MASK) != 0) Active_spectr |= FIFO0_SS_MASK;
	if ((SubsysStatus & WBS_V_MASK) != 0) Active_spectr |= FIFO1_SS_MASK;

	// now, for each spectrometer that was off and has now been switched on, we have to:
	// 1) reset the corresponding hw fifo (to do this we prepare the reset_fifo_mask)
	// 2) set to zero the corresponding variable fifo_x_pending_frames (to avoid possible problems)
	if ((Active_spectr & FIFO2_SS_MASK) && ( ! (Active_spectr_prev & FIFO2_SS_MASK) ))
	{
		reset_fifo_mask |= HRS_H_VALIDITY_MASK;
		fifo_2_pending_frames = 0;
	}
	if ((Active_spectr & FIFO3_SS_MASK) && ( ! (Active_spectr_prev & FIFO3_SS_MASK) ))
	{
		reset_fifo_mask |= HRS_V_VALIDITY_MASK;
		fifo_3_pending_frames = 0;
	}
	if ((Active_spectr & FIFO0_SS_MASK) && ( ! (Active_spectr_prev & FIFO0_SS_MASK) ))
	{
		reset_fifo_mask |= WBS_H_VALIDITY_MASK;
		fifo_0_pending_frames = 0;
	}
	if ((Active_spectr & FIFO1_SS_MASK) && ( ! (Active_spectr_prev & FIFO1_SS_MASK) ))
	{
		reset_fifo_mask |= WBS_V_VALIDITY_MASK;
		fifo_1_pending_frames = 0;
	}
	
	// if necessary, reset the hw fifos 
	if (reset_fifo_mask != 0) reset_FIFO_by_Mask(reset_fifo_mask);
	
	// now, through the IRQ mask register, we enable only the interrupts corresponding to SS that are on
	*IRQMASK = Active_spectr & 0x78;
}
 
//---------------- configure_ss ------------------------------------------------
//! this function executes configure subsystems TC(8,4,12); it generates a TM(8,6)
//! report packet (essentially an echo of the TC) and then sends towards task
//! ls the 32-bit commands read from the tc.
int configure_ss (TC_packet * packet)
{
	unsigned int j, len, cmd_len, activity_ID;
	unsigned int conf_echo_data[120];  // 78+6 should be enough in correct cases
	static unsigned int ls_commands_array[50];  // static array to hold ls commands to be forwarded to ss
		// its dimension has been chosen as the maximum expected number of ls-commands parameters in the tc
	
	// extract aid, bbid and length of data from the tc packet
	activity_ID = packet->data[TC_FUNACT_IDX] & 0x0FF;  // LS byte of the word
	Cur_BuildingBlock_ID = PACK(packet->data[TC_BBID_IDX], packet->data[TC_BBID_IDX + 1]);
	len = (packet->packet_length - 13)>>1;  // length in 16-bit words of the data following the BBID in the TC packet
	// subtract 14 -1; 14 bytes are for: data field header (4), crc (2), FID (1), AID (1), SID (2), BBID (4)
	
	cmd_len = len/2;  // number of 32-bit parameters (ie ls commands) contained in the TC
	// note: this number is as expected because we have already checked it by calling
	// check_TC_length() in function TC_acceptance(); then is also safe to divide by 2,
	// (len is even, no rounding will occur)

	// now prepare some data to be put into the report tm packet
	conf_echo_data[0] = packet->data[TC_FUNACT_IDX];
	conf_echo_data[1] = packet->data[TC_STRUC_IDX];  // sid of report = sid of tc
	conf_echo_data[2] = Observation_ID >>16;
	conf_echo_data[3] = Observation_ID & 0x0FFFF;
	UNPACK(&conf_echo_data[4], Cur_BuildingBlock_ID);
	
	// build the command vector
	for (j = 0; j < cmd_len; j++) ls_commands_array[j] = PACK(packet->data[TC_TABL_IDX + 2*j], packet->data[TC_TABL_IDX + 2*j+1]);  // set up the 32 bit word

	// perform some configuration specific actions
	switch( activity_ID )
	{
		case TC_FUNMAN_CFLCU_NOMIN_AID:
			// check if the LCU checksum is to be added
			compute_and_add_LCU_CRC(&ls_commands_array[1]);
			break;
		case TC_FUNMAN_CFFCU_POWER_AID:
			// possibly restart error count 
			if (ls_commands_array[0] == 0xCF080001 ) hk_restart_FCU_err_count(0);
			if (ls_commands_array[1] == 0xCF090001 ) hk_restart_FCU_err_count(1);
			if (ls_commands_array[2] == 0xCF0A0000 ) hk_restart_FCU_err_count(2);
			break;
	}			
	
	// send the commands 
	for (j = 0; j < cmd_len; j++) if (ls_put_msg_hp(&ls_commands_array[j], NULL, 0, NULL) != RC_OK)  // send the command to ls
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_ERROR_LS_HP_QUEUE, 0, NULL);  // exec failure
		return RC_FAIL;
	}
	
	// build the report
	for (j = 0; j < cmd_len; j++) 
	{
		conf_echo_data[6 + 2*j] = (ls_commands_array[j] & 0xFFFF0000) >> 16;
		conf_echo_data[6 + 2*j + 1] = ls_commands_array[j] & 0x0000FFFF;
	}
	
	// send the report
	generate_report(APID1024, TM_FS_TYPE, TM_FS_SUBT, 6+len, conf_echo_data);  // produce a TM(8,6) report packet
	
	return RC_OK;                           
}
   
//-------------- configure_spectroscopy ----------------------------------------
// this function executes a configure spectroscopy TC, FID = 11, AID = 17; this
// TC contains 29 16-bit parameters that must be copied into the two arrays
// Spectroscopy_table[] and Conf_Spectroscopy_table[]; we check that HIF_T_ACC_WBS
// is a multiple of 10 plus 5; a function status report packet containing echo data
// is generated.
int configure_spectroscopy (TC_packet * packet)
{
	unsigned int i;
	unsigned int conf_echo_data[35];  // array to store data for the echo report
								// 6 (from fid to bbid)  + 29 (echo parameters)
	
	// check that parameter HIF_T_ACC_WBS is a multiple of 10 plus 5
	if (( IFSI_MOD ( (packet->data[10] - 5) , 10) ) != 0)
	{
		enqueue_nok(packet, TC_ACCREP_FAILURE, NOK_CMDEX_ILLEGAL_WBS_ACC_TIME, 0, NULL);  // acceptance failure
		return RC_FAIL;
	}
	// note: we don't need to check the number of 16-bit parameters, because that
	// check has already been performed by function check_TC_length() in TC_acceptance() 

	// now the TC can be accepted (generation of tm(1,1) packet)
	enqueue_ok(packet, TC_ACCREP_SUCCESS);  // acceptance ack

	// update BBID variable
	Cur_BuildingBlock_ID = PACK(packet->data[TC_BBID_IDX], packet->data[TC_BBID_IDX + 1]);

	// now prepare some data to be put into the report tm packet
	conf_echo_data[0] = packet->data[TC_FUNACT_IDX];
	conf_echo_data[1] = packet->data[TC_STRUC_IDX];  // sid of report = sid of tc
	conf_echo_data[2] = Observation_ID >> 16;
	conf_echo_data[3] = Observation_ID & 0x0FFFF;
	UNPACK(&conf_echo_data[4], Cur_BuildingBlock_ID);
		
	// now copy the 29 16-bit parameters of the TC:
	// into the first 29 positions of Spectroscopy_table[] and Conf_Spectroscopy_table[] to perform configuration
	// and into local array conf_echo_data[] to prepare the data for the report
	for (i = 0; i < 29; i++) 
		conf_echo_data[6 + i] = Conf_Spectroscopy_table[i] = Spectroscopy_table[i] = packet->data[TC_TABL_IDX + i];
		
	generate_report(APID1024, TM_FS_TYPE, TM_FS_SUBT, 35, conf_echo_data);  // produce a TM(8,6) report packet
	return RC_OK;
}
   
//---- send_single_command -----------------------------------------------------
//! Enqueues towards task LS the single command coming with the TC packet, and produces an echo tm packet.
int send_single_command (TC_packet * packet) 
{
	static unsigned int Cmd;                    
    unsigned int single_cmd_report[8];  // the data for the report is stored into this array
	
	Cur_BuildingBlock_ID = PACK(packet->data[TC_BBID_IDX], packet->data[TC_BBID_IDX + 1]);
	
	// now prepare the data to be put into the report tm packet
	single_cmd_report[0] = packet->data[TC_FUNACT_IDX];
	single_cmd_report[1] = 0;  // the SID is assumed to be zero
	single_cmd_report[2] = Observation_ID >> 16;
	single_cmd_report[3] = Observation_ID & 0x0FFFF;
	UNPACK(&single_cmd_report[4], Cur_BuildingBlock_ID);
	single_cmd_report[6] = packet->data[TC_SINGCMD_IDX];
	single_cmd_report[7] = packet->data[TC_SINGCMD_IDX + 1];

	generate_report(APID1024, TM_FS_TYPE, TM_FS_SUBT, 8, single_cmd_report);  // tm(8,6)
	
	// now prepare the command and send it
	Cmd = PACK(packet->data[TC_SINGCMD_IDX], packet->data[TC_SINGCMD_IDX + 1]);  // set up the 32 bit word
	if (ls_put_msg_hp(&Cmd, NULL, 0, NULL) != RC_OK)
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_ERROR_LS_HP_QUEUE, 0, NULL);  // exec failure
		return RC_FAIL;
	}
	else
 	{	// we assume completion here
		enqueue_ok(packet, TC_EXEC_COMPLETED);
		return RC_OK;
	}
}

//------------ forward_TCpacket_to_ls_hdl --------------------------------------
//! Passes a command to ls_hdl, with the TC packet attached
void forward_TCpacket_to_ls_hdl (TC_packet * packet, int oper_id)
{
	LS_HDL_MSG msg, msg2;  // structure of command for ls_hdl
	unsigned int *p1, *p2, i;
	
	current_command_number++;  // increment the label
	KS_SemaGroupSignal(Sema_Wait_Group);  // signal these semaphores to interrupt any sleep state of tasks hs_hdl and ls_hdl
	if (oper_id != START_VERIFY_LCU_MODE1) completion_update(packet);  // completion management
	
	// get a block from the TC pool
	if (get_block(&msg.block, TC_POOL, TC_BLOCK_LEN_BYTES) == RC_FAIL)
	{
		enqueue_exec_fail(EXF_CMDEX_GET_BLOCK_TC_POOL, 0, NULL);
		return;
	}
	
	// copy the TC packet into the block just allocated
	p1 = (unsigned int *) msg.block.pointer_to_data;
	p2 = (unsigned int *) packet;
	for (i = 0; i < TC_BLOCK_LEN_WORDS; i++)
		p1[i] = p2[i];
	
	// flush the LS_HDL_QUEUE fifo
	while (KS_FIFOGet(LS_HDL_QUEUE, &msg2) == RC_OK) if (msg2.block_loaded == 1) release_block(&(msg2.block));
	
	msg.command_number = current_command_number; // this is the prog number of this command
	msg.operation_id = oper_id;  // set the operation id
	msg.block_loaded = 1;  // this signals that a block has been allocated
	
	if (KS_FIFOPut(LS_HDL_QUEUE, (void *) &msg) == RC_FAIL)  // send to task ls_hdl
	{
		enqueue_exec_fail(EXF_CMDEX_PUT_LS_HDL_QUEUE, 0, NULL);
		release_block(&msg.block);
		return;
	}
}
             
//----- forward_TCpacket_to_hs_hdl ---------------------------------------------
//! Passes a command to task hs_hdl, with the TC packet attached
//! It also changes the current_command_number, to stop other pending commands
//! and flushes the hs_hdl queue
void forward_TCpacket_to_hs_hdl (TC_packet * packet, int command_aid)
{
	HS_HDL_MSG msg, msg2;  // structure of command for hs_hdl
	unsigned int *p1, *p2, i;
	
	// prepare for command execution
	current_command_number++;  // this is the prog number of this command
	KS_SemaGroupSignal(Sema_Wait_Group);  // signal these semaphores to interrupt any sleep state of tasks hs_hdl and ls_hdl
	completion_update(packet);  // completion management
	
	// get a block from the TC pool
	if (get_block(&msg.block, TC_POOL, TC_BLOCK_LEN_BYTES) == RC_FAIL)
	{
		enqueue_exec_fail(EXF_CMDEX_GET_BLOCK_TC_POOL, 0, NULL);
		return;
	}
	
	// copy the TC packet into the block just allocated
	p1 = (unsigned int *) msg.block.pointer_to_data;
	p2 = (unsigned int *) packet;
	for (i = 0; i < TC_BLOCK_LEN_WORDS; i++)
		p1[i] = p2[i];
		
	// flush the hs_hdl queue.
	while (KS_FIFOGet(HS_HDL_QUEUE, &msg2) == RC_OK) if (msg2.block_loaded == 1) release_block(&(msg2.block));
	
	// prepare the message
	msg.operation_id = command_aid;  // set the operation id
	msg.command_number = current_command_number;  // set the cmd number
	msg.exec_attempts = 0;  // no exec attempt yet
	msg.block_loaded = 1;  // this signals that a block has been allocated
		
	if (KS_FIFOPut(HS_HDL_QUEUE, (void *) &msg) == RC_FAIL)  // send to task hs_hdl
	{
		enqueue_exec_fail(EXF_CMDEX_PUT_HS_HDL_QUEUE, 0, NULL);
		release_block(&msg.block);
		return;
	}
}

//--------------- enable_time_verification -------------------------------------
//! Writes the appropriate TM packet whith the current time stamp, then enqueues it toward tmtc_if task
int enable_time_verification (TC_packet * packet)
{                                            
	// Time verification has a special type of packet
	// w/o data field, only data field header   
	K_BLOCK block;
	HK_TM_MSG message;  // for the queue towards tm_tc
	struct TM_packet *tm_rep;
	unsigned int i;
	int N_param, time_data[4];
	
	// Get a new free block from pool to put TM data
	if (get_block(&block, HK_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) 
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, EXF_CMDEX_GET_BLOCK_HK_POOL, 0, NULL); 
		return RC_FAIL;
	}

	N_param = 3;
	time_data[2] = (Timekeeper.fractions) & 0xFFFF;
	time_data[1] = ((Timekeeper.seconds) & 0xFFFF) + 1;
	time_data[0] = ((Timekeeper.seconds) >> 16) & 0xFFFF;
	if (time_data[1] > 0xFFFF)
	{
		time_data[1] &= 0xFFFF;
		time_data[0]++;
	}

	// Write a TM_packet structure into the block
	tm_rep = (struct TM_packet *) block.pointer_to_data;
	// Write TM data
	for (i = 0; i < N_param; i++) tm_rep->data[i] = time_data[i];

	prepare_TM_packet(&block, TM_DFHF_REGULAR, APID1024, ((6+N_param)<<1)-1, TM_TYPE_TIME, TM_SUBTY_TIME);    
	
	// now prepare the message for the queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS; 
	// the following is useless in case of packing 16:
	message.length = N_param + 5;  // length in words of the application data field without crc
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	
	if (KS_FIFOPut(HK_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_PUT_HK_TM_QUEUE, 0, NULL);
		release_block(&block);
		return RC_FAIL;
	}
	return RC_OK;	
}
                                                
//------- perform_connection_test ----------------------------------------------
//! Writes the appropriate TM packet, then enqueues it Toward tmtc_if task in order to be sent to ground.
int perform_connection_test (TC_packet * packet)           
{                                            
	EVNT_TM_MSG message;  // queue to tm_tc                                 
	K_BLOCK block;                              
                                             
	// No applcation data for this Type-Subtype.
	// Perform the action: Send TM Service Type 17,
	// APID 1024, Type 17, subtype 2. No data.  

	// check against event queue overflow
	if ( ! authorise_event() ) return RC_FAIL;
     
	// Get a new free block from pool to put TM data
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) 
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_GET_EV_POOL, 0, NULL);
		return RC_FAIL;
	}
                                             
	prepare_TM_packet(&block, TM_DFHF_REGULAR, APID1024, TM_CONN_TEST_LEN, TM_CTEST_TYPE, TM_CTEST_SUBT);
     
	// Now prepare the message for the queue to tm_tc
	message.block = block;                      
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS; // offset to data
	message.length = 0;                         
	message.count = 0;                          
	message.packing = PACKING_16; // 16 bit data
	
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_PUT_EV_TM_QUEUE, 0, NULL);
		release_block(&message.block);
		return RC_FAIL;
	}
	return RC_OK;                        
}

//----------------- reset_function ---------------------------------------------
//! Perform a software reset
void reset_function (unsigned int act_id)
{
	// write the mask that signals a sw reset
	*((unsigned int pm * )BOOT_SEQ_COUNTER ) |= MASK_FOR_APID_SSC;
	
	// sleep 200 msec (spr 1984)
	KS_TaskSleep(200);  

	__asm("bit clr mode1 0x1000;NOP;NOP;"); //	disables all interrupts
	// if AID is 4, then array aj_DmInttab[] must be copied from data memory to 
	// program memory to enable the boot software start
	if (act_id == TC_FUNMAN_CALL_BOOT_AID) from_DM_to_PM((unsigned int*) 0, aj_DmInttab, 256);
	__asm("i1 = 0x82000000; r3 = 0x60; dm(i1,m6) = r3;");
	__asm("jump(0x08)(db); nop; nop;");
}

/******************************************************************************
*******************************************************************************
					TRANSMISSION CONTROL
*******************************************************************************
******************************************************************************/

#define TC_N_ENABLE_IDX 	 0  //!< absolute index in the array data (field of the structure TC_packet) for the number of en/dis operations in the TC
#define TC_TY_SUB_EN_IDX	 1  //!< relative index in the array data (field of the structure TC_packet) for the type and subtype information
#define TC_PID_EN_IDX		 2  //!< relative index in the array data (field of the structure TC_packet) for the pid (packet id) information

//-------------- pkt_enable ----------------------------------------------------
//! enables/disables (depending on the value of the argument flag: 1 enables, 0 disables)
// packets specified by (type, subtype, packet_id); if packet_id is zero, 
// all TM packets with (type, subtype) must be enabled/disabled.
void pkt_enable (TC_packet * packet, int flag)
{
	int i, j, NP, err_counter = 0;
	
	NP = packet->data[TC_N_ENABLE_IDX];  // number of enable/disable operations the TC packet is asking for
	// consistency check NP/length of the TC packet
	if ((NP*4 + 7) != packet->packet_length)
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_TRX_CTRL_LENGTH_CHECK, 1, &NP);
		return;
	}
	
	for (i = 0; i < NP; i++)
	{
		int Type_Found = FALSE;
		int SubType_Found = FALSE;
		int PID_Found = FALSE;
		int type, subtype, pid;
	
		type = (packet->data[i*2 + TC_TY_SUB_EN_IDX] & 0xFF00)>>8;
		subtype = (packet->data[i*2 + TC_TY_SUB_EN_IDX] & 0x0FF);
		pid = packet->data[i*2 + TC_PID_EN_IDX];
		
		for (j = 0; j < DIM_PKT_ENAB_VECT; j++) 
		{
			if (type == pkt_enab[j].type)
			{
				Type_Found = TRUE;
				if (subtype == pkt_enab[j].subtype)
				{
	    			SubType_Found = TRUE;
	    			if (pid == pkt_enab[j].pktid)
	    			{
	    				PID_Found = TRUE;
	    				pkt_enab[j].enab = flag;
	    				break;  // we can go out of the for loop on j
	    			}
	    			if (pid == 0)
	    			{
	    				PID_Found = TRUE;
	    				pkt_enab[j].enab = flag; 
	    			}
	    		}  // end if on subtype
			}  // end if on type
		}  // end for over j

		// if there have been problems, signal them
		if (PID_Found == FALSE)
		{	
			int params[3] = {type, subtype, pid};
			err_counter ++;
			if (SubType_Found == FALSE)
			{
				if (Type_Found == FALSE)
				{
					generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_TRX_CTRL_INVALID_TYPE, 3, params);
					continue;
				}
				generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_TRX_CTRL_INVALID_SUBTYPE, 3, params);	
				continue;
			}
			generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_TRX_CTRL_INVALID_PID, 3, params);
		}	
	}  // end of for over i
	if (err_counter > 0) 
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_TRX_CTRL_TC_WRONG_DATA, 0, NULL);  // execution failure 
	else 
		enqueue_ok(packet, TC_EXEC_COMPLETED);  // completion ack
}

//------------- pkt_enable_report ----------------------------------------------
//! produces a TM packet containing the list of (type, subtype, packet_id) combinations
// whose transmission is enabled; the packet is sent to the EVENT_TM_QUEUE. 
int pkt_enable_report (TC_packet * packet)
{
	struct TM_packet * tm_packet; 
	K_BLOCK block;
	EVNT_TM_MSG message;
	int i, l;

	// check against event queue overflow
	if ( ! authorise_event() ) return RC_FAIL;
	
	// Get a new free block from pool to put TM data
	if (get_block(&block, HK_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) 
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_GET_EV_POOL, 0, NULL);
		return RC_FAIL;
	}
	tm_packet = (struct TM_packet *) block.pointer_to_data;
	tm_packet->data[TC_N_ENABLE_IDX] = 0;
	
	for (i = 0, l = 1; i < DIM_PKT_ENAB_VECT; i++)
	{
		if (pkt_enab[i].enab == ENABLED)
		{
			tm_packet->data[l] = ((pkt_enab[i].type <<8) | pkt_enab[i].subtype);
			tm_packet->data[l+1] = pkt_enab[i].pktid;       
			tm_packet->data[TC_N_ENABLE_IDX]++; 
			l += 2; 
		}  // end if on pkt_enab[i].enab  
	}  // end for on i
	
	prepare_TM_packet(&block, TM_DFHF_REGULAR, APID1024, 2*l + 11, PACKET_TX_CONTROL, PACKET_GEN_REPORT);

	// Now prepare the message for the queue
	message.block = block;
	message.packing = PACKING_16;
	message.count = 0;
	
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		enqueue_nok(packet, TC_EXEC_FAILURE, NOK_CMDEX_PUT_EV_TM_QUEUE, 0, NULL);
		release_block(&message.block);
		return RC_FAIL;
	}
	return RC_OK;	
}
//------ LCU_safe_and_block ---------------------------------------------------------
void LCU_safe_and_block (void)
{
	static unsigned int HL_standby = 0xF0020202;

	if (ls_put_msg_hp(&HL_standby, NULL, 0, NULL) != RC_OK)  generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_FIFOPUT_LS_HP_QUEUE, 0, NULL);
	if (ls_put_msg_hp(NULL, NULL, BLOCK_LCU_REQUEST, NULL) != RC_OK)  generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_CMDEX_FIFOPUT_LS_HP_QUEUE, 0, NULL);
}