src/INFN/CAL_Calorimeter_INFN.c

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


#include <src/INFN/LU_SourceFileID_INFN.h>
#define __FILEID__ _CAL_Calorimeter_INFN__c

#include <src/INFN/PRH_ParamHandler_INFN.h>
#include <src/INFN/LU_LogUtility_INFN.h>
#include <src/INFN/PRH_ParamHandler_INFN_auto.h>
LU_DECL_MASK();

#include <src/INFN/DAQ_IDAQ_INFN.h>
#include <src/INFN/CM_Common_INFN.h>
#include <src/INFN/CH_CommandHandler_INFN.h>
#include <src/INFN/CAL_Calorimeter_INFN.h>
#include <src/INFN/KHB_Driver_INFN.h>
#include <src/INFN/PM_PamManager_INFN.h>
#include <src/INFN/TRG_Trigger_INFN.h>
#include <src/INFN/HK_Manager_INFN.h>
#include <src/INFN/ALM_Alarm_INFN.h>

/*============================ Global define ================================*/


/*============================== global types  ==============================*/


/*=========================== Structure define ==============================*/


/*============================ Enumerate define =============================*/


/*============================== global variables  ==============================*/

DAQ_DECLBUF(CAL_TempPif0,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif1,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif2,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif3,CAL_MAXTEMPBUFFER);

extern BOOL  RM_Init_Before_Calib;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I211;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I221;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I231;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I41;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I42;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I43;

static DAQ_CMD_BUF CAL_READ_DSP_MEM_C31;

static DAQ_CMD_BUF CAL_READ_DSP_MEM_C32;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_OFF;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I241[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I251[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I261[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I260[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I311[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I310[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I321;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I331[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I341[4+1];

DAQ_DECLBUF(daq_reply,128*1024);
DAQ_DECLBUF(daq_temp,128*1024);
DAQ_DECLBUF(daq_command,1024);
DAQ_DECLBUF(daq_calo,1024);

#define CAL_CAL_TRIGGER_DELAY 10

PRH_VAR_TYPE CAL_DSP_MASK;
PRH_VAR_TYPE CAL_FE_MASK;

#define CAL_STRIP_SKIP_LEN 66*16
#define CAL_SKIP_HEADER SM_PKTHEADER_LEN

static UINT32 STRIP_SKIP[CAL_STRIP_SKIP_LEN];
static UINT16 CAL_CRC[4][4]={
  {0x635a,0x6700,0x6bee,0x6fb4},
  {0xba7c,0xbe26,0xb2c8,0xb692},
  {0x723d,0x7667,0x7a89,0x7ed3},
  {0xab1b,0xaf41,0xa3af,0xa7f5}
};

status_code CAL_Init_ETOATO()
{
  int i;
  
  CH_SetAtoEto(CH_CAL_RESET_FE,1,2);
  CH_SetAtoEto(CH_CAL_READ_ALARM,1,2);
  
  for(i=0; i < 7; i++)
    CH_SetAtoEto(CAL_CAL_READ_FPGA_REG_BASE_ID+i,1,1);


  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_OFF,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I211,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I221,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I231,1,2);
  
  for(i=1; i < 5; i++)
    {
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I241[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I251[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I261[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I260[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I311[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I310[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I331[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I341[i],1,2);
    }

  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I41,1,7);  
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I42,1,7);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I43,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_READ_DSP_MEM_C31,1,8);
  CH_PARAMBUF_SET_ATO_ETO(CAL_READ_DSP_MEM_C32,1,8);
  
  CH_SetAtoEto(CH_CAL_READ_HK,1,7);
  CH_SetAtoEto(CH_CAL_READ_EVENT_DATA,1,7);
  CH_SetAtoEto(CH_CAL_FULL,1,2);
  CH_SetAtoEto(CH_CAL_COMPRESS,1,2);

  return CM_RC_SUCCESSFUL;
}

void CAL_Init_ParamBuffers() {
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I211);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I221);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I231);

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I41);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I42);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I43);

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_READ_DSP_MEM_C31);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_READ_DSP_MEM_C32);

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,4);  
  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I321);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_OFF);

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,4);  

}

status_code CAL_Init() 
{
  DAQ_BUFFER_INIT(daq_reply);
  DAQ_BUFFER_INIT(daq_temp);
  DAQ_BUFFER_INIT(daq_command);
  DAQ_BUFFER_INIT(daq_calo);
  DAQ_BUFFER_INIT(CAL_TempPif0);
  DAQ_BUFFER_INIT(CAL_TempPif1);
  DAQ_BUFFER_INIT(CAL_TempPif2);
  DAQ_BUFFER_INIT(CAL_TempPif3);

  CAL_DSP_MASK=PRH_VAR_CAL_UPLOAD_CAL_DSP_MASK;
  CAL_FE_MASK =PRH_VAR_CAL_UPLOAD_CAL_FE_MASK;

  CAL_Init_ParamBuffers();
  CAL_Init_ETOATO();
  return CM_RC_SUCCESSFUL;
}

status_code CAL_SwitchDCDC_On()
{
  return CM_RC_SUCCESSFUL;
}


status_code CAL_SendTrigger(SM_PAGEID page,UINT16 n) {
  int i;
  status_code status;
  for (i=0; i < n; i++)
    {
      status = DAQ_SendCmdQueue2CMDIF(page);
      LU_EXIT_IFERROR(status);
      status = RM_TaskSuspendUnlessSINT(CAL_CAL_TRIGGER_DELAY);
      LU_EXIT_IFERROR(status);
    }
}


status_code CAL_Initialization()
{
  DAQ_FE idaqfe;
  BYTE cal_loop1,cal_loop2,*ptr,dummy;
  UINT16 temp,temp1,i,fe,alarm_reg,j;
  RM_ACQ_SETTING sector;
  status_code status=CM_RC_SUCCESSFUL;
  DAQ_CMD_BUF* auto_buf;
  static int debug_cal=0;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_MAX_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_CAL_INIT);

  CAL_FE_MASK = PRH_VAR_CAL_UPLOAD_CAL_FE_MASK; 
  CAL_DSP_MASK = PRH_VAR_CAL_UPLOAD_CAL_DSP_MASK;
  
  for(idaqfe=DAQ_FE_CAL_1,fe = 0x0001,i=0; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1,i++) 
    {
      if(fe & CAL_FE_MASK) 
        {
          cal_loop1=cal_loop2=0; 
          
        START_ACQ:
          
          DAQ_Format_CMD_Empty(&daq_temp);
          DAQ_Format_CMD_Empty(&CAL_TempPif0);
          DAQ_Format_CMD_Empty(&CAL_TempPif1);
          DAQ_Format_CMD_Empty(&CAL_TempPif2);
          DAQ_Format_CMD_Empty(&CAL_TempPif3);

          CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
/* ZULLO 06 Jul 2004 Fe2Daq --> Fe2Ram */
/*        DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe); 
          DAQ_Format_Fe2Ram(&CAL_TempPif0,auto_buf,idaqfe); 
*/

          if(debug_cal)
            DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
          else
            DAQ_Format_Cmd2Fe(&CAL_TempPif0,auto_buf,idaqfe); 
          
          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
          LU_EXIT_IFERROR(status);

          if(debug_cal)
            status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
          else
            status = DAQ_SendCmdQueue2CMDIF(SM_CALEXP0);
          

          LU_EXIT_IFERROR(status);

          status = RM_TaskSuspendUnlessSINT(1);
          LU_EXIT_IFERROR(status);
          
          CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
         
          if (RM_Init_Before_Calib)
            DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I211,idaqfe);
          else
            {
              CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
              DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
            }
          
          DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I221,idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I231,idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I241[i+1],idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I251[i+1],idaqfe);
          
          if (RM_Init_Before_Calib)
            DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I261[i+1],idaqfe);
          else
            DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I260[i+1],idaqfe);
          
          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,TRUE,&daq_reply,TRUE);
          LU_EXIT_IFERROR(status);

          status = RM_TaskSuspendUnlessSINT(100);
          LU_EXIT_IFERROR(status);

          DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);

          
          if (RM_Init_Before_Calib)
            {
              DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I311[i+1],idaqfe);
              DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I321,idaqfe);
            }
          else
            {
              DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I310[i+1],idaqfe);
              CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
              DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
            }
          
          DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I331[i+1],idaqfe);

          DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I341[i+1],idaqfe);

          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,TRUE,&daq_reply,TRUE);
          LU_EXIT_IFERROR(status);

          DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);

          DAQ_Format_CMD_Empty(&CAL_TempPif2);

          if (RM_Init_Before_Calib)
            {
              if(fe & CAL_DSP_MASK) 
                {
                  DAQ_Format_CMD_Empty(&CAL_TempPif2);
                  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I41,idaqfe);
                  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I42,idaqfe);
                  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I43,idaqfe);
                  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif2);
                  LU_EXIT_IFERROR(status);
                  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,TRUE,&daq_reply,TRUE);
                  LU_EXIT_IFERROR(status);
                  DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);
                }
            }
          else
            CAL_DSP_MASK &=~fe;
                  
          if (daq_temp.len < 2); // go to reset 
          
          for (j=1; j <=10; j++)
            {
              ptr = (BYTE*)&daq_temp.buf[j*2];
              CM_READ_BE_UINT16(ptr,temp,dummy);
              if ((temp & 0x4007) !=  0x4000)
                goto INC_LOOP1;
            }
          
          if (fe & CAL_DSP_MASK)
            {
              for (j=11; j <= 13 ; j++)
                {
                  ptr = (BYTE*)&daq_temp.buf[j*2];
                  CM_READ_BE_UINT16(ptr,temp,dummy);
                  if ((temp & 0x8007) !=  0x8000)
                    goto INC_LOOP2;
                }
            }
          
          continue;
          
        INC_LOOP2:
          if (++cal_loop2 < PRH_ARR_CAL_COUNT[1])
            goto START_ACQ;
          
          DAQ_Format_CMD_Empty(&CAL_TempPif3);
          CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
          CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);

          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif3);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,TRUE,&daq_reply,FALSE);
          LU_EXIT_IFERROR(status);
          
          CAL_DSP_MASK&=~fe;
          continue;
          
        INC_LOOP1:      
          if (++cal_loop1 < PRH_ARR_CAL_COUNT[0])
            goto START_ACQ;
          
          CAL_FE_MASK&=~fe;       
        }

      CAL_DSP_MASK&=~fe;
    }

  return status;
} 

BOOL CAL_CheckCRC(int i,int val)
{
  int j;
  
  for (j=0; j < 4; j++)
    if (val == CAL_CRC[i][j])
      return(TRUE);
  
  return(FALSE);
}


int CAL_GetWriteFPGARegChannel(int x)
{
  int i=1;
  
  if (x == 0)
    return(CH_CAL_WRITE_FPGA_REG_0000);
  
  if (x < 0x8005)
    while (x = (x >> 1)) i++;
  else
    {
      switch(x)
        {
        case 0x8005: i = 17;
          break;
        case 0x8015: i = 18;
          break;
        case 0xc2:   i = 19;
        }
    }
  return(i+CH_CAL_WRITE_FPGA_REG_0000);
}

status_code CAL_Pulse(int cal_dsp_mask,int channel,int cal_vcal)
{
  UINT16 fe,i;
  DAQ_FE idaqfe;
  status_code status;
  DAQ_CMD_BUF* auto_buf;

  for(idaqfe=DAQ_FE_CAL_1,fe=0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
    {
      if (fe & cal_dsp_mask)
        {
          DAQ_Format_CMD_Empty(&CAL_TempPif1);
          
          CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
          CH_GetBuf(CAL_GetWriteFPGARegChannel(channel),&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
          CH_GetBuf(CAL_GetWriteFPGARegChannel(cal_vcal),&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
          
          DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,TRUE,&daq_reply,FALSE);
          LU_EXIT_IFERROR(status);                        
        }
    }

  //  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
  //  LU_EXIT_IFERROR(status);
  status = CAL_SendTrigger(SM_CALEXP0,130);
  LU_EXIT_IFERROR(status);
  
  return(CM_RC_SUCCESSFUL);  
}


status_code CAL_ReadPulseData(UINT16 fe_mask)
{
  DAQ_FE idaqfe;
  DAQ_CMD_BUF *auto_buf;
  status_code status = CM_RC_SUCCESSFUL;
  UINT16 fe;

  for(idaqfe=DAQ_FE_CAL_1,fe = 0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
    {
      if (fe & fe_mask)
        {
          DAQ_Format_CMD_Empty(&CAL_TempPif2);
          CH_GetBuf(CH_CAL_READ_EVENT_DATA,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_READ_DSP_MEM_C32,idaqfe);
          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,TRUE,&daq_reply,FALSE);
          LU_EXIT_IFERROR(status);                        
        }
    }

  return status;
}

BOOL CAL_Pack(DAQ_CMD_BUF *buffer) 
{
  int i;
  UINT16 temp,dummy;
  long int chped,chped2,chthr2,chthr;
  BYTE *ptr;
  UINT32 w1,w2;
  
  chped2 = 0; chthr2 =0;
  temp = 0;
    
  ptr = &buffer->buf[CAL_SKIP_HEADER+6*2];

  for(i=0; i < 1056; i++)
    {
      CM_READ_BE_UINT16(ptr,temp,dummy);
      ptr += 2;
      chped2+=(short int)temp;
    }

  CM_READ_BE_UINT16(ptr,w1,dummy); ptr += 2;
  CM_READ_BE_UINT16(ptr,w2,dummy); ptr += 2;

  chped = (w1 & 0x0000ffff) | (w2 << 16);
  
   for (i=0; i < 66; i++)
    {
      CM_READ_BE_UINT16(ptr,temp,dummy);
      ptr += 2;
      chthr2+=(short int)temp;
    }

  CM_READ_BE_UINT16(ptr,w1,dummy); ptr += 2;
  CM_READ_BE_UINT16(ptr,w2,dummy); 
  
  chthr = (w1 & 0xffff) | (w2 << 16);
  
  return((chped == chped2) && (chthr2 == chthr));
}


status_code CAL_Calibration() 
{
  DAQ_FE idaqfe;
  UINT16 fe_mask,cal_raw_mask,cal_loop3,cal_loop4,repeat_fe,fe,data[6],dummy,alarm_reg;
  BYTE *ptr;
  int i,j;
  status_code status;
  DAQ_CMD_BUF* auto_buf;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_MAX_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_RSV_CAL_CALIB);

  fe_mask = CAL_DSP_MASK;
  cal_raw_mask = cal_loop3 = repeat_fe = 0; 

 REDO_CALIB:
  cal_loop4 = 0;  
  DAQ_Format_CMD_Empty(&daq_temp);
  DAQ_Format_CMD_Empty(&CAL_TempPif3);
  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL);

  for(fe = 1,idaqfe=DAQ_FE_CAL_1; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1 ) 
    {
      if (fe & fe_mask)
        {
          DAQ_Format_CMD_Empty(&CAL_TempPif1);   
          CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_DSP_MEM_I41,idaqfe);
          CH_GetBuf(CH_CAL_WRITE_FPGA_REG_C2,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);

          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,DAQ_STORE_MM,&daq_reply,DAQ_NO_STORE_RAM);
          LU_EXIT_IFERROR(status);
        }
    }

  DAQ_Format_CMD_Empty(&CAL_TempPif0);
  CH_GetBuf(CH_TB_GIVE_ME_TRIGGER,&auto_buf );
  DAQ_Format_Cmd2Fe(&CAL_TempPif0,auto_buf,DAQ_SELECT_TRIG(PRH_VAR_TB_LINK)); 
  
  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
  LU_EXIT_IFERROR(status);


  status = CAL_SendTrigger(SM_CALEXP0,1850);
  LU_EXIT_IFERROR(status);

  fe = 0x0001;
  
  for(idaqfe=DAQ_FE_CAL_1, j=0; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1, j++ ) 
    {
      if (fe & fe_mask)
        {
          CH_GetBuf(CH_CAL_READ_EVENT_DATA,&auto_buf);
          DAQ_Format_CMD_Empty(&CAL_TempPif2);
          DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
          DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_READ_DSP_MEM_C31,idaqfe);
  
          PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PED);
          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
          LU_EXIT_IFERROR(status);
          status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,DAQ_STORE_MM,&daq_reply,DAQ_STORE_RAM);
          LU_EXIT_IFERROR(status);
          
          for (i = 0, ptr = &daq_reply.buf[CAL_SKIP_HEADER]; i < 6; i++)
            CM_READ_BE_UINT16(ptr,data[i],dummy);
          
          if (!( 
              ((data[0] & 0xe047) == 0xe000) && 
              (data[1]==2) && 
              (data[2]==0) && 
              (CAL_CheckCRC(j,data[3])) && 
              ((data[4] & 0xe047)==0xa000) &&
              (data[5]==0x1215) && 
              CAL_Pack(&daq_reply)
              ))
            {
              repeat_fe+=fe;
              cal_loop4 = 1;
            }
        }
    }

  if (cal_loop4)
    {
      if (cal_loop3 == PRH_ARR_CAL_COUNT[2])
        {
          for(idaqfe=DAQ_FE_CAL_1, fe = 1; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
            {
              if (fe & repeat_fe)
                {
                  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
                  DAQ_Format_CMD_Empty(&CAL_TempPif3);
                  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
                  CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
                  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
                  CAL_DSP_MASK &=~fe;
                  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif3);         
                  LU_EXIT_IFERROR(status);
                  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,DAQ_STORE_MM,&daq_reply,DAQ_NO_STORE_RAM);
                  LU_EXIT_IFERROR(status);                                
                }
            }
        }
      else
        {
          cal_loop3++;
          fe_mask = repeat_fe;
          repeat_fe=0;
          goto REDO_CALIB;
        }
    }

  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PULSE1);
  
  status = CAL_Pulse(CAL_DSP_MASK,0,PRH_VAR_CAL_VCAL);
  LU_EXIT_IFERROR(status);        
  
  status = CAL_ReadPulseData(CAL_DSP_MASK);
  LU_EXIT_IFERROR(status);        

  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PULSE2);

  status = CAL_Pulse(CAL_DSP_MASK,PRH_VAR_CAL_CH,PRH_VAR_CAL_VCAL);
  LU_EXIT_IFERROR(status);        
  status = CAL_ReadPulseData(CAL_DSP_MASK);
  LU_EXIT_IFERROR(status);        
  
  if (PRH_VAR_CAL_CH == 0x8000)
    {
      PRH_VAR_CAL_CH = 0x0001;
      
      if (PRH_VAR_CAL_VCAL == 0x8005)
        PRH_VAR_CAL_VCAL = 0x8015;
      else
        PRH_VAR_CAL_VCAL = 0x8005;
    }
  else
    PRH_VAR_CAL_CH*=2;

  return status;
}

status_code CAL_Error_Handler()
{
  DAQ_FE idaqfe;
  int i,bit_count;
  BYTE *ptr,dummy;
  UINT16 idaq_status,fe,data[4];
  UINT32 alarm_reg;
  UINT32 cal_count=0;
  status_code status = CM_RC_SUCCESSFUL;
  DAQ_CMD_BUF* auto_buf;
  BOOL check_ok;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_DEFAULT_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_CAL_ALARM);

  status = HK_ReadKHBAlarmRegister(&alarm_reg);
  LU_EXIT_IFERROR(status);
  
  alarm_reg&=(KHB_ALARM_CALO_1 | KHB_ALARM_CALO_2 | KHB_ALARM_CALO_3 | KHB_ALARM_CALO_4);

  alarm_reg>>=4;
  

  for(idaqfe=DAQ_FE_CAL_1,fe=0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++, fe <<=1) 
    {
      if (fe & alarm_reg)
        {
          DAQ_Format_CMD_Empty(&CAL_TempPif0);
          CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
          DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
  
          for(i=0; i < 7; i++)
            {
              CH_GetBuf(CAL_CAL_READ_FPGA_REG_BASE_ID+i,&auto_buf);
              DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
            }

          status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);         
          if (status) 
            {
              status = CM_RC_DO_RESET_IF_ALM;
              LU_EXIT_IFERROR(status);
            }

          cal_count = 0; check_ok = FALSE;

          while ((cal_count < PRH_ARR_CAL_COUNT[3]) && (status == CM_RC_SUCCESSFUL) && !check_ok)
            {
              status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,DAQ_STORE_RAM);
  
              if (status == CM_RC_SUCCESSFUL) 
                {
                  if (daq_reply.len < 2 + CAL_SKIP_HEADER)
                    status = CM_RC_DO_RESET_IF_ALM;
                  else
                    {
                      ptr = &daq_reply.buf[CAL_SKIP_HEADER];
                      CM_READ_BE_UINT16(ptr,data[0],dummy);
                      CM_READ_BE_UINT16(ptr,data[1],dummy); 
                      CM_READ_BE_UINT16(ptr,data[2],dummy);
                      CM_READ_BE_UINT16(ptr,data[3],dummy); 
                      
                      if (((data[0] & 0xE007) == 0x2000) && (data[1] == 3))
                        check_ok = TRUE;
                    }
          
                  cal_count++;
                }
              else
                status = CM_RC_DO_RESET_IF_ALM;
            }
  
          LU_EXIT_IFERROR(status);
  
          if (check_ok)
            {
              if ((data[0] & 0x0020) == 0x0020)
                {
                  bit_count = 0;
          
                  for(i=0; i < 11; i++)
                    {
                      if (data[3] & 1)
                        bit_count++;
              
                      data[3]>>=1;
                    }
              
                  if (bit_count > PRH_VAR_CAL_TEMP)
                    {
                      DAQ_Format_CMD_Empty(&CAL_TempPif0);                    
                      
                      CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
                      DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
                      
                      DAQ_Format_Fe2Daq(&CAL_TempPif0,&CAL_WRITE_FPGA_REG_OFF,idaqfe);
                                      
                      status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
                      if (status) 
                        {
                          status = CM_RC_DO_RESET_IF_ALM;
                          LU_EXIT_IFERROR(status);
                        }
                      
                      status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
                      if (status) 
                        {
                          status = CM_RC_DO_RESET_IF_ALM;
                          LU_EXIT_IFERROR(status);
                        }
                      
                      status = RM_TaskSuspendUnlessSINT(100);
                      if (status) 
                        {
                          status = CM_RC_DO_RESET_IF_ALM;
                          LU_EXIT_IFERROR(status);
                        }

                      DAQ_Format_CMD_Empty(&CAL_TempPif0);                    
                      CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
                      DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
                      
                      status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
                      if (status) 
                        {
                          status = CM_RC_DO_RESET_IF_ALM;
                          LU_EXIT_IFERROR(status);
                        }
                      
                      status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
                      if (status)
                        {
                          status = CM_RC_DO_RESET_IF_ALM;
                          LU_EXIT_IFERROR(status);
                        }                             
                      CAL_FE_MASK &=~fe;
                      CAL_DSP_MASK &=~fe;
                    }
                }
            }
          else 
            // CAL_FE_MASK&=~fe;
            ;
        }
    }
  
  status = DAQ_ReadStatusWord(&idaq_status);
  if (status) 
    status = CM_RC_DO_RESET_IF_ALM;
     
  return status;
}