/*****************************************************************************/
/*  Module       : TestUtility                                               */
/*  C.I. No.     :                                                           */
/*  Revision     :                                                           */
/*  Date         :                                                           */
/*  Belonging to :                                                           */
/*               :                                                           */
/*  File Name    : TS_Test_p.c                                               */
/*  Program Type :                                                           */
/*  Sub-modules  :                                                           */
/*                                                                           */
/*****************************************************************************/
/*  S W   D e v e l o p m e n t   E n v i r o n m e n t                      */
/*                                                                           */
/*  Host system  :                                                           */
/*  SW Compiler  :                                                           */
/*  Author                                                                   */
/*               :                                                           */
/*****************************************************************************/
/*  U p d a t i n g                                                          */
/*                                                                           */
/*  Revision     :                                                           */
/*                                                                           */
/*                                                                           */
/*****************************************************************************/
 

/*============================= Include File ================================*/

#include <src/INFN/LU_SourceFileID_INFN.h>
#define __FILEID__ _TS_Test_INFN__c
#include <src/INFN/PRH_ParamHandler_INFN_auto.h>
#include <src/INFN/PRH_ParamHandler_INFN.h>
#include <src/INFN/LU_LogUtility_INFN.h>

LU_DECL_MASK();


#include <src/TestUtility/Test/TS_Test_p.h>
#include <src/ModeManager/ModeCoordinator/MC_ModeCoordinator_p.h>
#include <src/FileManager/MMSUManager/FT_MMSUManager_p.h>
#include <src/FileManager/FileSystem/FS_FileSystem_p.h>
#include <src/MCMDManager/MCMDDispatcher/MD_MCMDDispatcher_p.h>
#include <src/MCMDManager/MCMDArea/MA_MCMDArea_p.h>
#include <src/TM_TCManager/TMTCManager/TM_TMTCManager_p.h>
#include <src/SRAMManager/SRAMPageManager/SM_SRAMPageManager_p.h>
#include <src/SRAMManager/SRAMPageManager/SM_SRAMPageManager_op.h>
#include <src/HKManager/HistoryArea/HA_HistoryArea_p.h>


#include <src/INFN/LU_SourceFileID_INFN.h>
/*==================================== define ===============================*/

#define DISABLE      0
#define ENABLE       1
#define LENGTH_OUT_CMDS 0x0100

#define BITE_FILLER  0x0204

#define  HA_EVENT_00    0
#define  HA_EVENT_01    1
#define  HA_EVENT_02    2
#define  HA_EVENT_03    3
#define  HA_EVENT_04    4
#define  HA_EVENT_05    5
#define  HA_EVENT_06    6
#define  HA_EVENT_07    7
#define  HA_EVENT_08    8
#define  HA_EVENT_09    9
#define  HA_EVENT_10    10
#define  HA_EVENT_11    11
#define  HA_EVENT_12    12
#define  HA_EVENT_13    13
#define  HA_EVENT_14    14
#define  HA_EVENT_15    15

typedef unsigned char       UBYTE;
typedef unsigned short int  UWORD;
typedef unsigned long  int  UDWORD;

static unsigned int teststate;
static unsigned int pkts;
MA_HEADER_MCMD McMd;
static MsgTsk        MaintMsg;
static MsgTsk        ActiveMsg;
static MsgTsk        TestMsg;
static MsgTsk        CalMsg;
static MsgTsk        ExpMsg;

/*================================ Prototype ================================*/


void TS_TmTc(void);

/*============================== Local Variables ============================*/

static unsigned int TmValue[85];
static unsigned int val;
static unsigned int sensore;

/* array for test the memory load output */
static TM_TC_ML TmMemoryLoad[16];

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

/*============= S W _ T E S T   I N T E R N A L   F U N C T I O N ===========*/

/*****************************************************************************/
/*  prototype
 * 
 *
 *  Description
 *  
 *
 *  Input parameters:   
 *                      
 *                      
 *
 *  Output parameters:  rtems_status_code
 *
 */
/*****************************************************************************/

status_code TS_piTestInit (void)
{
status_code    status;
unsigned int   OldPriority;
unsigned int   transiz;
unsigned char* pRc;

    status   =SUCCESSFUL;
    status  |=OS_piTaskReady(TS_TASK,TS_tkTest);
    status  |=OS_piTaskPriority(TS_TASK,20,&OldPriority);
    return (status);
}


/*****************************************************************************/
/*  TS_tkTest
 *
 *
 *  Input parameters:  NONE
 *
 *  Output parameters:  NONE
 *
 */
/*****************************************************************************/

task TS_tkTest (task_argument unused)
{
status_code        status;
unsigned int       MsgSize;
unsigned int       counter;
unsigned int       level;
unsigned int       i;
unsigned int       length;
MsgTsk             RxMsg;
MsgTsk             SndMsg;


volatile unsigned short confStatRegVal[100]; 
unsigned char   buf[100]; 


SD_DAQ_WRESULT  pageInfo; 
unsigned int    loop; 
unsigned int    pageCounter; 
unsigned int    tst_value; 

  counter =1;

/*    
    SndMsg.Code    =TS_TM_TC;
    SndMsg.LlInfo  =0; 
    status =TS_piSndMsgTest (&SndMsg); 
*/    

  length = 16;
  for (i=0; i<length; i++)
  {
      TmMemoryLoad[i].Code  = 0x19;  /* or 0x1a for ML 2 */
      TmMemoryLoad[i].Value = (i*0x1234)&0xFFFF;
  }

  val = 1;
  sensore = TM_CC_DIAG_ACQ_1;
  while( FOREVER )
  {
    // Check mailbox queue
    RxMsg.Code =NULL_MSG_TS;
    if ((status =OS_piMsgQueueReceive (TS_MAILBOX,(void*)&RxMsg,
           &MsgSize,WAIT, NO_TIMEOUT)) == SUCCESSFUL)
    {
      switch (RxMsg.Code)
      {
        case TS_TM_TC:
          while( counter )
          {
            TS_TmTc();
            counter--;
          }
        break;

        case TS_GO_SELECT_MODE:
        {
          unsigned int      timetagFlag;
          unsigned int      Mode;
          unsigned int      CurrentState;
          unsigned int      cnt;
          unsigned char     pippo2[LENGTH_OUT_CMDS];
          MA_HEADER_MCMD*   PtrMcmd;

          PtrMcmd = (MA_HEADER_MCMD*)RxMsg.Info;
          /* Delete MCMD from partition */
          Mode  =(unsigned int)*(((unsigned short*)PtrMcmd->PtrMCMD)+OFFSET_HEADER);
          Mode &= 0xFF;
          timetagFlag =( *(((unsigned short*)PtrMcmd->PtrMCMD)+1)&TIMETAG_MASK ) ?
                                              	       TIMETAG_MCMD : IMMEDIATE_MCMD;
          MA_piDelBufferMCMD(PtrMcmd->PtrMCMD,timetagFlag);
          HA_piLogHistoryEntry2 (PtrMcmd->Type,status);

          pippo2[0x00]=0xFA;
          pippo2[0x01]=0xFE;
          pippo2[0x02]=0xDE;
          pippo2[0x03]=0x1A;
          pippo2[0x04]='O';
          pippo2[0x05]='B';
          pippo2[0x06]='T';
          pippo2[0x07]='_';
          pippo2[0x08]='C';
          pippo2[0x09]='N';
          pippo2[0x0A]='T';
          pippo2[0x0B]=0x1E;
          /* fill the body of the command data                     */
          for (cnt = 0x0C; cnt < LENGTH_OUT_CMDS; pippo2[cnt++]=0xAA);
          /* copy the data in the calibration page 2 (es)          */
          PM_pi_CopyToSRAM (SM_CALEXP2, pippo2, sizeof(pippo2));
          /* select the calibration page 2                         */
          PM_pi_UseCalibrationPage (SM_CALEXP2);

          switch(Mode)
          {
            case 1 : //MM_INFN_SUBMODEA0
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_MAINTENANCE_STATE)
                MC_piSetState(MC_MAINTENANCE_STATE);
              break;
            case 2 : // MM_INFN_SUBMODEA1
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_MAINTENANCE_STATE)
                MC_piSetState(MC_MAINTENANCE_STATE);
              break;
            case 3 : // MM_INFN_SUBMODEA2:
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_MAINTENANCE_STATE)
                MC_piSetState(MC_MAINTENANCE_STATE);
              break;
            case 4 : // MM_INFN_SUBMODEB1:
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_ACTIVE_STATE)
              {
                MC_piSetState(MC_ACTIVE_STATE);
              }
              FT_piChangeAcqMode(SM_EXP64/*it was SM_EXPERIMENT*/);
              PM_pi_SetExpPktCpyFlag(FALSE);
              break;
            case 5 : // MM_INFN_SUBMODEB2:
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_ACTIVE_STATE)
              {
                MC_piSetState(MC_ACTIVE_STATE);
              }
              FT_piChangeAcqMode(SM_EXP64);
              PM_pi_SetExpPktCpyFlag(FALSE);
              break;
            case 6 : // MM_INFN_SUBMODEB3:
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_ACTIVE_STATE)
              {
                MC_piSetState(MC_ACTIVE_STATE);
              }
              FT_piChangeAcqMode(SM_DAQSETUP);
              break;
            case 7 : // MM_INFN_SUBMODEB4:
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_ACTIVE_STATE)
	            {
                MC_piSetState(MC_ACTIVE_STATE);
                FT_piChangeAcqMode(SM_DAQSETUP);
              }
              break;
            case 8 : // TEST low level transfers (LABEN test mode)
              TS_try(TS_TEST_SRAM);
              break;
            case 9 : // TEST Memory Module ON (LABEN test mode)
              TS_try(TS_MEM_TEST);
              break;
            case 10 : // set acquisition freq in erc32 Memory to 1/10 (LABEN test mode)
              MC_piGetState(&CurrentState);
              if(CurrentState != MC_ACTIVE_STATE)
              {
                MC_piSetState(MC_ACTIVE_STATE);
              }
              FT_piChangeAcqMode(SM_EXP64/*it was SM_EXPERIMENT*/);
              PM_pi_SetExpPktCpyFlag(FALSE);
              break;
            case 11 : // write data via PCMCIA (LABEN test mode)
              TS_try(TS_WRITE_FILE);
              break;
            default :
              break;
          }
        }
        break;

        case TS_MEM_TEST: 
        {
          FT_piModuleTest();
        } break;
        case TS_TEST_SRAM:
        {
          unsigned int patternIndex;
          unsigned int patternIndexVerify;
          unsigned int errors;
          unsigned int pattern[TS_MAX_FIX_PATTERN] = {0x00000000,0x00000055,0x000000AA,0x000000FF};
          unsigned int address;
          unsigned int pageAddress;
          unsigned int SRAMAddress;
          unsigned int value;
          unsigned int pageDim;
          unsigned int lengthSRAM = 0x40000; /* 256kB        */
          unsigned int page;
          unsigned int modAddress;
          unsigned int cnt;
          unsigned int repeat;
          unsigned int timeOutCMD;
          TI_TIME      obt;
          unsigned int numCharPrinted = 0;
          static unsigned int bufTest[TS_ERROR_BUFFER_DIM];

 
          /* packet counter for header                       */
          cnt = 0;
          /* number of errors find                           */
          errors = 0;
          /* current pattern index used for write pages      */
          patternIndex = 0;
          /* current pattern index used for read pages       */
          patternIndexVerify = TS_MAX_PATTERN;

          for (repeat = 0; repeat < TS_NUM_OF_CYCLE; repeat++)
          {
            /* write 256Kb of SRAM with a fixed page size      */
            /* increase the page dimension  */
            for (pageDim = 0x0400; pageDim <= lengthSRAM; pageDim <<= 1 )
            {
              /* write a collection of pages                   */
              for (page = 0; page < (lengthSRAM/pageDim); page++)
              {
                SD_piSetSRAMRWAddress(page*pageDim);
 
                /* Retrive OBT */
                TI_piGetTimeInfo_ms(&obt);
                /* write header  */
                SD_piWriteSRAM(PAM_HRD1);
                SD_piWriteSRAM(PAM_HRD2);
                SD_piWriteSRAM(PAM_HRD3);
                SD_piWriteSRAM(PAM_HRD4);
                /* Write the OBT            */
                SD_piWriteSRAM((BYTE)((obt>>24) & 0x000000FF));
                SD_piWriteSRAM((BYTE)((obt>>16) & 0x000000FF));
                SD_piWriteSRAM((BYTE)((obt>>8 ) & 0x000000FF));
                SD_piWriteSRAM((BYTE)( obt      & 0x000000FF));
 
                /* Write the packet counter */
                SD_piWriteSRAM((BYTE)((cnt>>16) & 0x000000FF));
                SD_piWriteSRAM((BYTE)((cnt>>8 ) & 0x000000FF));
                SD_piWriteSRAM((BYTE)( cnt++    & 0x000000FF));
                /* write packet id #2 */
                SD_piWriteSRAM(PAM_HRD_PHY_EVENT);

                for (pageAddress = TS_PKT_HEADER_LENGTH; pageAddress < pageDim; pageAddress++)
                {
                  /* write patterns in SRAM: 00, 55, AA, FF, Addresses */
                  if (patternIndex < TS_MAX_FIX_PATTERN)
                    SD_piWriteSRAM(pattern[patternIndex]);
                  else
                    SD_piWriteSRAM(pageAddress * 3);
                }

                patternIndex++;
                if (patternIndex >= TS_MAX_PATTERN)
                   patternIndex = 0;

              } /* for (page = 0;...                         */

              /* read SRAM                                     */

              /* save the data in SRAM in to memory module     */
              FT_piSavePage(0, lengthSRAM -1, FS_HK);

              /* wait until the buffer on CMD I/F is empty     */
              timeOutCMD = 0;
              while ((SD_piGetCMDCounterLeft() != 0) && (timeOutCMD < TS_TIMEOUT_CMD_LOOP))
              {
                OS_piTaskSuspend(1);
                timeOutCMD++;
              }

              /* send out the data in SRAM throgh CMD I/F      */
              FT_piSavePage(0, lengthSRAM -1, FS_CMD);

              for (SRAMAddress = 0; SRAMAddress < lengthSRAM; SRAMAddress++)
              {
 
                /* evaluate the address into virtual page           */
                pageAddress = SRAMAddress % pageDim;
                if (pageAddress == 0)
                {
                  SD_piSetSRAMRWAddress(SRAMAddress);
                  patternIndexVerify++;
                  if (patternIndexVerify >= TS_MAX_PATTERN)
                    patternIndexVerify = 0;
                }
 
                /* read a value from SRAM                           */
                value = SD_piReadSRAM();

                /* skip the header check and verify the packet data */
                if ((pageAddress >= TS_PKT_HEADER_LENGTH) &&
                        (numCharPrinted + TS_NEXT_REPORT_LENGTH < TS_ERROR_BUFFER_DIM))
                {
                  if (patternIndexVerify < TS_MAX_FIX_PATTERN)
                  {
                    /* check fixed pattern */
                    if ((pattern[patternIndexVerify] & 0x000000FF) != value)
                    {
                      errors++;
                      address = SD_piGetSRAMRWAddress();
                      bufTest[numCharPrinted++] = address -1;
                      bufTest[numCharPrinted++] = value;
                    }
                  }
                  else
                  {
                    /* check variable pattern */
                    if (((SRAMAddress * 3) & 0x000000FF) != value)
                    {
                      errors++;
                      address = SD_piGetSRAMRWAddress();
                      bufTest[numCharPrinted++] = address -1;
                      bufTest[numCharPrinted++] = value;
                    }
                  }
                }
              }

              OS_piTaskSuspend(TS_TE_TIME_FOR_ACQUIRE_CMD);
              sprintf(buf," Page number: %d - Wait for acquiring data", cnt);
              TS_piWriteOnUART(buf);
            } /* "for" that increase the page dimension */
          } /* "for" that repeat the test n times for store 11.25 MB*/
          TS_piWriteOnUART("Test Ended");
        } break;
        
        case TS_WRITE_FILE:
        {
          static unsigned char data[TS_DATA_LENGTH];
          unsigned int  arrayIndex;
          TI_TIME       obt;

          data[0x00] = PAM_HRD1;
          data[0x01] = PAM_HRD2;
          data[0x02] = PAM_HRD3;
          data[0x03] = PAM_HRD4;

          /* Retrive OBT */
          TI_piGetTimeInfo_ms(&obt);

          /* Write the OBT            */
          data[0x04] = ((obt>>24) & 0x000000FF);
          data[0x05] = ((obt>>16) & 0x000000FF);
          data[0x06] = ((obt>>8 ) & 0x000000FF);
          data[0x07] = ( obt      & 0x000000FF);

          /* Write a fun counter      */
          data[0x08] = 'E';
          data[0x09] = 'R';
          data[0x0a] = 'C';

          data[0x0B] = PAM_HRD_PHY_EVENT;
          
          /* write a simple data body */
          for (arrayIndex = TS_PKT_HEADER_LENGTH; 
               arrayIndex < TS_DATA_LENGTH; 
               data[arrayIndex++] = 0xAA);
          
          /* write the packet         */
          PM_pi_StorePacket (TS_DATA_LENGTH, (void*)data, FS_HK);
          
        }break;
 
	      case 1010:
	      {
          MsgTsk SndMsg;
          TI_TIME Time_begin, Time_end;
          int loop, delay, cmd;
          void*           p;
          volatile unsigned short *pIR, *pCS;
          unsigned int             index;
          unsigned short           Format,
                                 * pFormat;
 
          sprintf(buf,"Starting case 10 ");
          puts(buf);
 
          status = TI_piGetTimeInfo_ms (&Time_begin);
          sprintf(buf,"Time_begin=%ld", Time_begin);
          puts(buf);
          SndMsg.Code              =TM_CYCLIC_ACQUISITION;
          SndMsg.LlInfo            =0;
          //TM_piSndMsgTMTCManager (&SndMsg);
          status = TI_piGetTimeInfo_ms (&Time_end);
          sprintf(buf,"Time_end=%ld", Time_end);
          puts(buf);
 
          sprintf(buf,"end case 10: \n");
          puts(buf);
          break;
        }

        default:
        break;
      } // switch
    }   // if
  }     // while
}

/*****************************************************************************/
/*  TS_piSndMsgTest
 *
 *
 *  Input parameters:  NONE
 *
 *  Output parameters:  NONE
 *
 */
/*****************************************************************************/

status_code TS_piSndMsgTest (MsgTsk* SndMsg)
{
status_code status;

    status =OS_piMsgQueueSend(TS_MAILBOX,(void*)SndMsg,sizeof(MsgTsk));
    return (status);
}




/*****************************************************************************/
/*  TS_piPutLog
 *
 *
 *  Input parameters:  NONE
 *
 *  Output parameters:  NONE
 *
 */
/*****************************************************************************/
status_code TS_piPutLog (MsgTsk* msg) {
  UBYTE          EntryType;
  static unsigned char  buf[100],str[100],str2[100];
  unsigned int   entry;
  unsigned char  code;
  unsigned int   time;
  unsigned int   rep1,rep2,rep3,line,fileid;

#if 0
  // console_outbyte_polled(0,msg->LlInfo*sizeof(UWORD));
  //console_outbyte_polled(1,msg->LlInfo*sizeof(UWORD));
  for(entry=0;entry<msg->LlInfo*sizeof(UWORD);entry++) {
    console_outbyte_polled(0,msg->Info[entry]);
    //console_outbyte_polled(1,msg->Info[entry]);
  }
#endif
    
  /* ignore all the rest: */
  //return SUCCESSFUL;

  EntryType = msg->Info[0] >> 4;
  
  entry  =EntryType;
  code   =msg->Info[1];
  time   =(((UWORD*)(msg->Info))[1] << 16) + ((UWORD*)(msg->Info))[2];
  rep1   =0;
  rep2   =0;
  rep3   =0;
  
  switch( EntryType ) {
  case HA_EVENT_04:
    sprintf(str,"");
    break;
  case HA_EVENT_00:
  case HA_EVENT_01:
  case HA_EVENT_02:
  case HA_EVENT_03:
  case HA_EVENT_06:
    rep1 = ((UWORD*)(msg->Info))[3];
    sprintf(str,"R1: %10d 0x%x",rep1,rep1);
    break;
    
  case HA_EVENT_10:
    rep1 = ((UWORD*)(msg->Info))[3];
    rep2 = ((UWORD*)(msg->Info))[4];
    sprintf(str,"R1: %10d %x - R2: %10d 0x%x ",rep1,rep1,rep2,rep2);
    break;
    
  case HA_EVENT_05:
    line  = ((UWORD*)(msg->Info))[3];
    rep2  = ((UWORD*)(msg->Info))[4];
    rep1  = ((UWORD*)(msg->Info))[5];
    rep3  = rep1 | (rep2<<16);
    LU_GetSourceInfo(str2,code,line);
    sprintf(str,"INFN: Info1: %9u 0x%08x [%s]",rep3,rep3,str2);
    break;
  case HA_EVENT_07:
  case HA_EVENT_11:
    rep1 = ((UWORD*)(msg->Info))[3];
    rep2 = ((UWORD*)(msg->Info))[4];
    rep3 = ((UWORD*)(msg->Info))[5];
    sprintf(str,"R1: %10d 0x%x - R2: %10d 0x%x  - R3: %10d %x ",rep1,rep1,rep2,rep2,rep3,rep3);
    break;
  default:
    break;
  }
  
  sprintf(buf,"%2d OBT: %5ld Code: %3d - %s",entry,time,code,str);
  
  TS_piWriteOnUART(buf);
  return (SUCCESSFUL);
  
}




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

/*============= S W _ T E S T   I N T E R N A L   F U N C T I O N ===========*/

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

void TS_TmTc(void)
{
status_code  status;
unsigned int value;

    /* Analogic parameters        */
    status  =TM_piGetTMValue (TM_DEA_ANA_1       , TM_CURRENT_VAL, &TmValue[0x00]);
    status  =TM_piGetTMValue (TM_DEA_ANA_2       , TM_CURRENT_VAL, &TmValue[0x01]);
    status  =TM_piGetTMValue (TM_DEA_ANA_3       , TM_CURRENT_VAL, &TmValue[0x02]);
    status  =TM_piGetTMValue (TM_DEA_ANA_4       , TM_CURRENT_VAL, &TmValue[0x03]);
    status  =TM_piGetTMValue (TM_DEA_ANA_5       , TM_CURRENT_VAL, &TmValue[0x04]);
    status  =TM_piGetTMValue (TM_DEA_ANA_6       , TM_CURRENT_VAL, &TmValue[0x05]);
    status  =TM_piGetTMValue (TM_DEA_ANA_7       , TM_CURRENT_VAL, &TmValue[0x06]);
    status  =TM_piGetTMValue (TM_DEA_ANA_8       , TM_CURRENT_VAL, &TmValue[0x07]);
    status  =TM_piGetTMValue (TM_DEA_ANA_9       , TM_CURRENT_VAL, &TmValue[0x08]);
    status  =TM_piGetTMValue (TM_DEA_ANA_10      , TM_CURRENT_VAL, &TmValue[0x09]);
    status  =TM_piGetTMValue (TM_DEA_ANA_11      , TM_CURRENT_VAL, &TmValue[0x0A]);
    status  =TM_piGetTMValue (TM_DEA_ANA_12      , TM_CURRENT_VAL, &TmValue[0x0B]);
    status  =TM_piGetTMValue (TM_DEA_ANA_13      , TM_CURRENT_VAL, &TmValue[0x0C]);
    status  =TM_piGetTMValue (TM_DEA_ANA_14      , TM_CURRENT_VAL, &TmValue[0x0D]);
    status  =TM_piGetTMValue (TM_DEA_ANA_15      , TM_CURRENT_VAL, &TmValue[0x0E]);
    status  =TM_piGetTMValue (TM_DEA_ANA_16      , TM_CURRENT_VAL, &TmValue[0x0F]);
    
    status  =TM_piGetTMValue (TM_TH_ANA_1        , TM_CURRENT_VAL, &TmValue[0x10]);
    status  =TM_piGetTMValue (TM_TH_ANA_2        , TM_CURRENT_VAL, &TmValue[0x11]);
    status  =TM_piGetTMValue (TM_TH_ANA_3        , TM_CURRENT_VAL, &TmValue[0x12]);
    status  =TM_piGetTMValue (TM_TH_ANA_4        , TM_CURRENT_VAL, &TmValue[0x13]);
    status  =TM_piGetTMValue (TM_TH_ANA_5        , TM_CURRENT_VAL, &TmValue[0x14]);
    status  =TM_piGetTMValue (TM_TH_ANA_6        , TM_CURRENT_VAL, &TmValue[0x15]);
    status  =TM_piGetTMValue (TM_TH_ANA_7        , TM_CURRENT_VAL, &TmValue[0x16]);
    status  =TM_piGetTMValue (TM_TH_ANA_8        , TM_CURRENT_VAL, &TmValue[0x17]);
    status  =TM_piGetTMValue (TM_TH_ANA_9        , TM_CURRENT_VAL, &TmValue[0x18]);
    status  =TM_piGetTMValue (TM_TH_ANA_10       , TM_CURRENT_VAL, &TmValue[0x19]);
    status  =TM_piGetTMValue (TM_TH_ANA_11       , TM_CURRENT_VAL, &TmValue[0x1A]);
    status  =TM_piGetTMValue (TM_TH_ANA_12       , TM_CURRENT_VAL, &TmValue[0x1B]);
    status  =TM_piGetTMValue (TM_TH_ANA_13       , TM_CURRENT_VAL, &TmValue[0x1C]);
    status  =TM_piGetTMValue (TM_TH_ANA_14       , TM_CURRENT_VAL, &TmValue[0x1D]);
    status  =TM_piGetTMValue (TM_TH_ANA_15       , TM_CURRENT_VAL, &TmValue[0x1E]);
    status  =TM_piGetTMValue (TM_TH_ANA_16       , TM_CURRENT_VAL, &TmValue[0x1F]);
    
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_1  , TM_CURRENT_VAL, &TmValue[0x20]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_2  , TM_CURRENT_VAL, &TmValue[0x21]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_3  , TM_CURRENT_VAL, &TmValue[0x22]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_4  , TM_CURRENT_VAL, &TmValue[0x23]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_5  , TM_CURRENT_VAL, &TmValue[0x24]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_6  , TM_CURRENT_VAL, &TmValue[0x25]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_7  , TM_CURRENT_VAL, &TmValue[0x26]);
    status  =TM_piGetTMValue (TM_BIL_DIAG_ACQ_8  , TM_CURRENT_VAL, &TmValue[0x27]);
    
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_1   , TM_CURRENT_VAL, &TmValue[0x28]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_2   , TM_CURRENT_VAL, &TmValue[0x29]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_3   , TM_CURRENT_VAL, &TmValue[0x2A]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_4   , TM_CURRENT_VAL, &TmValue[0x2B]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_5   , TM_CURRENT_VAL, &TmValue[0x2C]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_6   , TM_CURRENT_VAL, &TmValue[0x2D]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_7   , TM_CURRENT_VAL, &TmValue[0x2E]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_8   , TM_CURRENT_VAL, &TmValue[0x2F]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_9   , TM_CURRENT_VAL, &TmValue[0x30]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_10  , TM_CURRENT_VAL, &TmValue[0x31]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_11  , TM_CURRENT_VAL, &TmValue[0x32]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_12  , TM_CURRENT_VAL, &TmValue[0x33]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_13  , TM_CURRENT_VAL, &TmValue[0x34]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_14  , TM_CURRENT_VAL, &TmValue[0x35]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_15  , TM_CURRENT_VAL, &TmValue[0x36]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_16  , TM_CURRENT_VAL, &TmValue[0x37]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_17  , TM_CURRENT_VAL, &TmValue[0x38]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_18  , TM_CURRENT_VAL, &TmValue[0x39]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_19  , TM_CURRENT_VAL, &TmValue[0x3a]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_20  , TM_CURRENT_VAL, &TmValue[0x3b]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_21  , TM_CURRENT_VAL, &TmValue[0x3c]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_22  , TM_CURRENT_VAL, &TmValue[0x3d]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_23  , TM_CURRENT_VAL, &TmValue[0x3e]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_24  , TM_CURRENT_VAL, &TmValue[0x3f]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_25  , TM_CURRENT_VAL, &TmValue[0x40]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_26  , TM_CURRENT_VAL, &TmValue[0x41]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_27  , TM_CURRENT_VAL, &TmValue[0x42]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_28  , TM_CURRENT_VAL, &TmValue[0x43]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_29  , TM_CURRENT_VAL, &TmValue[0x44]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_30  , TM_CURRENT_VAL, &TmValue[0x45]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_31  , TM_CURRENT_VAL, &TmValue[0x46]);
    status  =TM_piGetTMValue (TM_CC_DIAG_ACQ_32  , TM_CURRENT_VAL, &TmValue[0x47]);
    
    status  =TM_piGetTMValue (TM_DBL_ACQ_3       , TM_CURRENT_VAL, &TmValue[0x48]);
    status  =TM_piGetTMValue (TM_DBL_ACQ_4       , TM_CURRENT_VAL, &TmValue[0x49]);
    status  =TM_piGetTMValue (TM_DBL_ACQ_7       , TM_CURRENT_VAL, &TmValue[0x4a]);
    status  =TM_piGetTMValue (TM_DBL_ACQ_8       , TM_CURRENT_VAL, &TmValue[0x4b]);
    status  =TM_piGetTMValue (TM_DS_16_ACQ_1     , TM_CURRENT_VAL, &TmValue[0x4c]);
    status  =TM_piGetTMValue (TM_DS_16_ACQ_2     , TM_CURRENT_VAL, &TmValue[0x4d]);

    /* Start cyclic telemetry acquisition            */ 
    // TM_piStartAutomaticTMSeq();   
    
 
}


status_code Sys_test        (void)
{
BYTE *cmds;
BYTE sw;
BYTE pktbuffer[10];
MsgTsk sndMsg;
UINT16 mcmdw[12];
int mcmdttag_direct;

  cmds = (BYTE*)0x21f0048;
  sw = *cmds;

  switch(sw)
  {
    // Experiment acquisition (2 pkts)
    case 1:{
      teststate = 0x0001;
      *cmds = 0x00;
      pkts = 10;
      MC_piSetState(MC_ACTIVE_STATE);
      FT_piChangeAcqMode(SM_EXP64/*it was SM_EXPERIMENT*/);
      PM_pi_SetExpPktCpyFlag(TRUE);
    } break;
    // INFN Pachet storage
    case 2:{
      teststate = 0x0100;
      *cmds = 0x00;
      MC_piSetState(MC_ACTIVE_STATE);
    } break;
    // Bite
    case 3:{
      teststate = 0x1000;
      *cmds = 0x00;
      MC_piSetState(MC_MAINTENANCE_STATE);
    } break;
    // Raw Downlink MCMD (4 sectors)
    case 4:{
      teststate = 0x3000;
      *cmds = 0x00;
      MC_piSetState(MC_ACTIVE_STATE);
    } break;
    // Downlink RPB-EOT simulation (4 sectors)
    case 5:{
      teststate = 0x4000;
      *cmds = 0x00;
    } break;
    // File1 Downlink MCMD (4 sectors)
    case 6:{
      teststate = 0x5000;
      *cmds = 0x00;
      MC_piSetState(MC_ACTIVE_STATE);
    } break;
  }

  switch(teststate){
    case 1:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_5);
      teststate = 0x02;
    } break;
    case 2:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_1);
      teststate = 0x03;
    } break;
    case 3:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_5);
      teststate = 0x04;
    } break;
    case 4:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_1);
      teststate = 0x05;
    } break;
    case 5:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_4);
      teststate = 0x06;
    } break;
    case 6:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_4);
      teststate = 0x00;
    } break;
    case 0x0100:{
      PM_pi_StorePacket(10, &pktbuffer[0],FS_CMD);
      teststate = 0x0200;
    } break;
    case 0x0200:{
      PM_pi_StorePacket(10, &pktbuffer[0],FS_CMD);
      teststate = 0x0300;
    } break;
    case 0x0300:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_4);
      teststate = 0x0000;
    } break;
    case 0x1000:{
      mcmdw[0] = 0x1122;
      mcmdw[1] = 0x3344;
      mcmdw[2] = 0x5566;
      mcmdw[3] = 0x7788;
      mcmdw[4] = 0x99BB;
      mcmdw[5] = 0xAA55;
      McMd.TimeTag = 0;
      McMd.Type = BITE;
      McMd.Length = 2;
      mcmdttag_direct = IMMEDIATE_MCMD;
      sndMsg.Code = MD_MCMD_EXEC;
      MA_piGetBufferMCMD(&McMd.PtrMCMD, mcmdttag_direct);
      memcpy((unsigned short int*)McMd.PtrMCMD, &mcmdw[0], sizeof(McMd));
      *(MA_HEADER_MCMD*)sndMsg.Info = McMd;
      sndMsg.LlInfo = sizeof(MA_HEADER_MCMD);
      MD_piSndMsgMCMDDispatcher(&sndMsg);
      teststate = 0x2000;
    } break;
    case 0x2000:{
      ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_1);
      teststate = 0x0000;
    } break;
    case 0x3000:{
      mcmdw[0] = 0x1122;
      mcmdw[1] = 0x3344;
      mcmdw[2] = 0x5566;
      mcmdw[3] = 0x7788;
      mcmdw[4] = 0x99BB;
      mcmdw[5] = 0x004a;
      McMd.TimeTag = 0;
      McMd.Type = DOWN_LINK;
      McMd.Length = 2;
      mcmdttag_direct = IMMEDIATE_MCMD;
      sndMsg.Code = MD_MCMD_EXEC;
      MA_piGetBufferMCMD(&McMd.PtrMCMD, mcmdttag_direct);
      memcpy((unsigned short int*)McMd.PtrMCMD, &mcmdw[0], sizeof(McMd));
      *(MA_HEADER_MCMD*)sndMsg.Info = McMd;
      sndMsg.LlInfo = sizeof(MA_HEADER_MCMD);
      MD_piSndMsgMCMDDispatcher(&sndMsg);
      pkts = 4;
      teststate = 0x0000;
    } break;
    case 0x4000:{
      if(pkts)
      {
        ERC32_Force_interrupt(ERC32_INTERRUPT_EXTERNAL_3);
      }
      else
      {
        teststate = 0x0000;
      }
      pkts--;
    } break;
    case 0x5000:{
      mcmdw[0] = 0x1122;
      mcmdw[1] = 0x3344;
      mcmdw[2] = 0x5566;
      mcmdw[3] = 0x7788;
      mcmdw[4] = 0x99BB;
      mcmdw[5] = 0x0002;
      McMd.TimeTag = 0;
      McMd.Type = DOWN_LINK;
      McMd.Length = 2;
      mcmdttag_direct = IMMEDIATE_MCMD;
      sndMsg.Code = MD_MCMD_EXEC;
      MA_piGetBufferMCMD(&McMd.PtrMCMD, mcmdttag_direct);
      memcpy((unsigned short int*)McMd.PtrMCMD, &mcmdw[0], sizeof(McMd));
      *(MA_HEADER_MCMD*)sndMsg.Info = McMd;
      sndMsg.LlInfo = sizeof(MA_HEADER_MCMD);
      MD_piSndMsgMCMDDispatcher(&sndMsg);
      pkts = 5;
      teststate = 0x0000;
    } break;
  }
}


void TS_piTMTCTest(void)
{
UINT32 Value;
UINT32 loop;

  TM_piGetTMValue(TM_TH_ANA_1, TM_CURRENT_VAL, &Value);
  for(loop=0; loop<500; loop++);
  TM_piGetTMValue(TM_TH_ANA_2, TM_CURRENT_VAL, &Value);
  for(loop=0; loop<1000; loop++);
  TM_piSendTC(TM_HL_CMD_1, 0);
}

void TS_piTCTest(void)
{
UINT32 Value;
UINT32 loop;

  for (loop=0; loop< 10; loop++)
  {
    TM_piSendTC(TM_HL_CMD_1, 0);
    OS_piTaskSuspend(100);
  }
}

void TS_piMCMDForward(MA_HEADER_MCMD* pMcmdExec)
{
UINT16*            pMcmdInfo;
UINT32             timetagFlag;
UINT32             mode;
UINT32             type;

   pMcmdInfo =(UINT16* )pMcmdExec->PtrMCMD;
   type = (UINT32)(*(pMcmdInfo+1) & 0x00ff);
   mode = (UINT32)(*(pMcmdInfo+OFFSET_HEADER));
   /* Delete MCMD from partition                         */
   timetagFlag =(*(pMcmdInfo+1)&TIMETAG_MASK) ? TIMETAG_MCMD : IMMEDIATE_MCMD;
   MA_piDelBufferMCMD(pMcmdExec->PtrMCMD,timetagFlag);
   /*============= Entry 2 Log in History Area ==========*/
   // HA_piLogHistoryEntry2 (pMcmdExec->Type,0x00);
   LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,pMcmdExec->Type);
   if(type = ID_SELECT_MODE)
   {
     if(mode)
       TS_piSndMsgTest (&ActiveMsg);
     else
       TS_piSndMsgTest (&MaintMsg);
   }
}

void TS_try(TS_MSG_TSK msg)
{
MsgTsk             SndMsg;

    SndMsg.Code    = msg;
    SndMsg.LlInfo  = 0; 
    TS_piSndMsgTest (&SndMsg); 

}


void TS_piWriteOnUART(unsigned char *string)
{
unsigned char *s;

    for ( s = string ; *s ; s++ ) 
      console_outbyte_polled( 0, *s );

    console_outbyte_polled( 0, '\r' );
    console_outbyte_polled( 0, '\n' );
}
