srvdsp.c

本程序为ST公司开发的源代码

  *size = sizeof(DSP_PRAM);
}


/******************************************************************************/
/* Function:  load_dsp                                                        */
/*                                                                            */
/*! \brief
 *  \param
 *  \return
 *  \remark
 */
/******************************************************************************/

RETVAL load_dsp(void)
{
  uint8 *dsp_param;
  uint16 i, size;

  loadDSP(&dsp_param, &size);


  //DEBUG_printf(("DSP version: " EMERALD_DSP_FIRMWARE_VERSION_LOADFILE, 4, 0));
  DISABLE_INTERRUPTS();  /* BB050221d */
  ERESET = DSP_UNDER_RESET;   /* Put the DSP under Reset to allow writing into PRAM
                                   initialize SERVO in write mode in PRAM
                                   and set starting address of code at 0H */
  EPRADL = 0;
  EPRADH.all = _RAM_WRITE | _PRAM;   /*  R/W bit -> 0 (W)*/
  ENABLE_INTERRUPTS();  /* BB050221d */


  for (i = 0; i < size; i++)
  {
    EPRD = dsp_param[i];
  }

  DISABLE_INTERRUPTS();  /* BB050221d */
  EPRADL = 0;
  EPRADH.all = _RAM_READ | _PRAM;   /* R/W bit -> 1 (R) */
                                    /* P/Y bit -> 0 (P) */
  ENABLE_INTERRUPTS();  /* BB050221d */
  for (i = 0; i < size; i++)
  {
    if (dsp_param[i] != EPRD)
    {
      return LOAD_DSP_ERROR;
    }
  }

  OSAL_isr_install(OSAL_ISR_MAILBOX, 0x0f, mailbox_isr_irq);
  ERESET = DSP_RUN;
  return OK;
}


/******************************************************************************/
/* Function:  mailbox_isr_irq                                                 */
/*                                                                            */
/*! \brief    Interrupt routine of the dsp mailbox
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void mailbox_isr_irq(void)
{
  uint16 dsp_state;
  uint16 dsp_previous_state;
  uint8 dsp_event_code;
  volatile uint8 dummy_read;

  DISABLE_INTERRUPTS();
  /* Send code 0x0200 to inform DSP about mailbox_isr_irq execution. */
  DSP_EVENT_ACK_L = (uint8)0x00;
  DSP_EVENT_ACK_H = (uint8)0x02;
  /* Retrieve information put into the mailbox by the DSP. */
  /* Read previous state from the DSP. */
  dsp_previous_state = DSP_PREV_STATE_L;
  dsp_previous_state |= (DSP_PREV_STATE_H << 8);
  /* Read current state from the DSP. */
  dsp_state = DSP_STATE_L;
  dsp_state |= (DSP_STATE_H << 8);
  /* Read current event code from the DSP. */
  dsp_event_code = DSP_EVENT_CODE_L;
  dummy_read = DSP_EVENT_CODE_H;
  ENABLE_INTERRUPTS();

  DEBUG_SERVO_DSP_STATE(("DSP_STATE:", 2, 2, dsp_state, dsp_previous_state));

  /* Check AGC update */
  if (0 != get_agc(dsp_state))
  {
#ifdef APM_PICKUP
    int8 new_shift;
    int16 delta_gain;
#endif

    /* Interrupt received for DSP AGC procedure after it changed the loop gain. */
    /* Check if the newly set value lies in allowed range to make sure that the */
    /* OPU actuators will not overheat.                                         */
    set_dsp_agc_event(AGC_NO_EVENT);
    if (AGC_INJECTION_ON_FOCUS == get_agc(dsp_state))          /* Loop gain updated for the focus control loop. */
    {
      set_dsp_agc_event(AGC_EVENT_FOCUS_UPDATE);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Focus AGC", 4, 0));
#ifdef APM_PICKUP     /* Checking of AGC values only for APM */
      /* Get newly set gain from DSP. */
      /* Fractional part of the gain set by the AGC. */
      delta_gain = (int16)dsp_read_ymem(FOCUS_GAIN_ADD);
      /* Shift part if the gain set by the AGC. */
      new_shift = (int8)dsp_read_ymem(FOCUS_SHIFT_GAIN_ADD);
      /* Shift part of the default loop gain. is in FOCUS_PRO_CONTROLLER_COEFF[FOCUS_SHIFT_GAIN_INDEX]. */
      new_shift = (int8)(2 * AGC_PRO_MINMAX_NR_CYCLES
                       + FOCUS_PRO_CONTROLLER_COEFF[FOCUS_SHIFT_GAIN_INDEX]
                       - new_shift);
      /* Determine how must the newly set loop gain deviates from default value. */
      if (new_shift >= 0)
      {
        delta_gain >>= new_shift;

        /* Open the focus loop when the newly set loop gain deviates too much from */
        /* the default loop gain. */
        if ((delta_gain > AGC_PRO_FOCUS_DELTA_GAIN_MAX)
         || (delta_gain < AGC_PRO_FOCUS_DELTA_GAIN_MIN))
        {
          to_dsp_flags_pc(FCS_OFF); /* Open loop */
          dsp_error_flags.field.agc_focus_error = 1;
        }
      }
      else
      {
        //      delta_gain <<= -new_shift;

        /* Open the focus loop when the newly set loop gain deviates too much from */
        /* the default loop gain. */
        if ((delta_gain > (AGC_PRO_FOCUS_DELTA_GAIN_MAX >> -new_shift))
         || (delta_gain < (AGC_PRO_FOCUS_DELTA_GAIN_MIN >> -new_shift)))
        {
          to_dsp_flags_pc(FCS_OFF); /* Open loop */
          dsp_error_flags.field.agc_focus_error = 1;
        }
      }
#endif /* APM_PICKUP */
    }
    else if (AGC_INJECTION_ON_TRACKING == get_agc(dsp_state))  /* Loop gain updated for the tracking control loop. */
    {
      set_dsp_agc_event(AGC_EVENT_TRACKING_UPDATE);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Tracking AGC", 4, 0));
#ifdef APM_PICKUP     /* Checking of AGC values only for APM */
      /* Get newly set gain from DSP. */
      /* Fractional part of the gain set by the AGC. */
      delta_gain = (int16)dsp_read_ymem(TRACKING_GAIN_ADD);
      /* Shift part if the gain set by the AGC. */
      new_shift = (int8)dsp_read_ymem(TRACKING_SHIFT_GAIN_ADD);
      /* Shift part of the default loop gain. is in TRACKING_PRO_CONTROLLER_COEFF[TRACKING_SHIFT_GAIN_INDEX]. */
      new_shift = (int8)(2 * AGC_PRO_MINMAX_NR_CYCLES
                       + TRACKING_PRO_CONTROLLER_COEFF[TRACKING_SHIFT_GAIN_INDEX]
                       - new_shift);

      /* Determine how must the newly set loop gain deviates from default value. */
      if (new_shift >= 0)
      {
        delta_gain >>= new_shift;

        /* Open the tracking loop when the newly set loop gain deviates too much   */
        /* from the default loop gain. */
        if ((delta_gain > AGC_PRO_TRACKING_DELTA_GAIN_MAX)
         || (delta_gain < AGC_PRO_TRACKING_DELTA_GAIN_MIN))
        {
          to_dsp_flags_pc(TRK_OFF); /* Open loop */
          dsp_error_flags.field.agc_tracking_error = 1;
        }
      }
      else
      {
        //        delta_gain <<= -new_shift;

        /* Open the tracking loop when the newly set loop gain deviates too much   */
        /* from the default loop gain. */
        if ((delta_gain > (AGC_PRO_TRACKING_DELTA_GAIN_MAX >> -new_shift))
         || (delta_gain < (AGC_PRO_TRACKING_DELTA_GAIN_MIN >> -new_shift)))
        {
          to_dsp_flags_pc(TRK_OFF); /* Open loop */
          dsp_error_flags.field.agc_tracking_error = 1;
        }
      }
#endif /* APM_PICKUP */
    }
    else
    {
      /* If everything works correctly, this branch is never entered. */
      ASSERT(0, "AGC check: This branch should never be taken");
    }
  }


  /* Check for change in the focus_ok state. */
  set_dsp_focus_event(FOCUS_NO_EVENT);
  if (get_focus_closed(dsp_state) != get_focus_closed(dsp_previous_state))
  {
    if (0 != get_focus_closed(dsp_state))
    {
      /* Changed to focus closed. */
      set_dsp_focus_event(FOCUS_EVENT_CLOSED);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Focus closed", 4, 0));
    }
    else
    {
      /* Changed to focus open. */
      set_dsp_focus_event(FOCUS_EVENT_OPEN);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Focus open", 4, 0));
    }
  }


  /* Check for change in the tracking_ok state. */
  /* If changed the new state will be tracking lost (DSP resets, ARM sets). */
  if (get_tracking_glitch(dsp_state) != get_tracking_glitch(dsp_previous_state))
  {
    /* Tracking lost, update the tracking state machine, which also */
    /* sends an event to the Servo Task. */
    DEBUG_SERVO_FREQUENT_DSP_EVENT(("DSP_EVENT: Tracking glitch", 4, 0));
    check_tracking_fsm_call(CHECK_TRACKING_CALLER_DSP_GLITCH);
  }


  /* Check for change in the acquisition defect state. */
  if (get_acq_defect(dsp_state) != get_acq_defect(dsp_previous_state))
  {
    if (0 != get_acq_defect(dsp_state))
    {
      /* Changed to "acquisition defect present". */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Acquisition defect present", 4, 0));
      check_hf_fsm_call(CHECK_HF_CALLER_DSP_ACQ_DEFECT_PRESENT);
    }
    else
    {
      /* Changed to "acquisition defect absent". */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Acquisition defect absent", 4, 0));
      check_hf_fsm_call(CHECK_HF_CALLER_DSP_ACQ_DEFECT_ABSENT);
    }
  }


#ifdef APM_PICKUP
  /* Check for change in the ttm_fg_ok state. */
  /* If changed the new state will be not okay (DSP resets, ARM sets). */
  set_dsp_ttm_fg_event(TTM_FG_NO_EVENT);
  if (get_ttm_fg_ok(dsp_state) != get_ttm_fg_ok(dsp_previous_state))
  {
    if (0 != get_ttm_fg_ok(dsp_state))
    {
      /* Changed to "ttm fg okay". */
      set_dsp_ttm_fg_event(TTM_FG_EVENT_OK);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: TTM FG okay", 4, 0));
    }
    else
    {
      /* Changed to "ttm fg not okay". */
      set_dsp_ttm_fg_event(TTM_FG_EVENT_NOT_OK);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: TTM FG not okay", 4, 0));
    }
  }
#endif /* APM_PICKUP */


  /* Check for change in the ttm_speed_ok state. */
  if (get_ttm_speed_ok(dsp_state) != get_ttm_speed_ok(dsp_previous_state))
  {
    if (0 != get_ttm_speed_ok(dsp_state))
    {
      /* Changed to "ttm speed okay". */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: TTM Speed okay", 4, 0));
      ttm_fsm_call(TTM_CALLER_DSP_SPEED_OK);
    }
    else
    {
      /* Changed to "ttm speed not okay". */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: TTM Speed not okay", 4, 0));
      ttm_fsm_call(TTM_CALLER_DSP_SPEED_NOT_OK);
    }
  }


#ifdef ECC_WORKAROUND
  /* Check for change in the clv_buffer_ok state. */
  /* If changed the new state will be okay (DSP sets, ARM resets). */
  if (get_clv_buffer_ok(dsp_state) != get_clv_buffer_ok(dsp_previous_state))
  {
    /* Event received from DSP indicating the CLV buffer has reached the desired  */
    /* level, after being reset.                                                  */
    /* In order to  fill the CLV buffer quickly, the blocks after the CLV buffer  */
    /* are forced to stop pulling data out of the CLV buffer by reseting the      */
    /* value AP_CONTROL_PART_2.field.enable_sample_req to 0. When the correct     */
    /* level is reached then the DSP sends ARM an event/interrupt and this ISR is */
    /* carried out. Here the data retrieval from the CLV allowed again by setting */
    /* AP_CONTROL_PART_2.field.enable_sample_req to 1.                            */
    /* Checking for the desired fullness of the CLV buffer is initiated in the    */
    /* DSP after the ARM clears the clv_buffer_ok state bit of the new_dsp_state.     */
    if (0 != get_clv_buffer_ok(dsp_state))
    {
      /* Changed to "ttm speed okay". */
      set_clv_buffer_event(CLV_BUFFER_EVENT_OK);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: *CLV Buffer okay", 4, 0));
    }
    else
    {
      /* Changed to "ttm speed not okay". */
      set_clv_buffer_event(CLV_BUFFER_EVENT_NOT_OK);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: *CLV Buffer not okay", 4, 0));
    }
  }
#endif /* ECC_WORKAROUND */


  /* Check for change in the ramp_stopped state. */
  set_dsp_ramp_event(RAMP_NO_EVENT);
  if (get_ramp_stopped(dsp_state) != get_ramp_stopped(dsp_previous_state))
  {
    if (0 != get_ramp_stopped(dsp_state))
    {
      /* Changed to "ramp stopped". */
      set_dsp_ramp_event(RAMP_EVENT_STOPPED);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Ramp stopped", 4, 0));
    }
    else
    {
      /* Changed to "ramp moving". */
      /* set_dsp_ramp_event(RAMP_EVENT_MOVING) not needed */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Ramp moving", 4, 0));
    }
  }


  /* Check for change in the short_jump_stopped state. */
  set_dsp_short_jump_event(SHORT_JUMP_NO_EVENT);
  if (get_short_jump_stopped(dsp_state) != get_short_jump_stopped(dsp_previous_state))
  {
    if (0 != get_short_jump_stopped(dsp_state))
    {
      /* Changed to "short jump stopped". */
      set_dsp_short_jump_event(SHORT_JUMP_EVENT_STOPPED);
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Short Jump stopped", 4, 0));
    }
    else
    {
      /* Changed to "short jump moving". */
      /*  set_dsp_short_jump_event(SHORT_JUMP_EVENT_MOVING) not needed */
      DEBUG_SERVO_DSP_EVENT(("DSP_EVENT: Short Jump moving", 4, 0));
    }
  }


  /* Check for change in the sledge_moved state. */
  if (SLEDGE_MOVED_FORWARD == get_sledge_moved(dsp_state))
  {
    DEBUG_SERVO_FREQUENT_DSP_EVENT(("DSP_EVENT: Sledge moved forward", 4, 0));
    sledge_fsm_call(SLEDGE_CALLER_DSP_FORWARD);
  }
  else if (SLEDGE_MOVED_BACKWARD == get_sledge_moved(dsp_state))
  {
    DEBUG_SERVO_FREQUENT_DSP_EVENT(("DSP_EVENT: Sledge moved backward", 4, 0));
    sledge_fsm_call(SLEDGE_CALLER_DSP_BACKWARD);
  }


  /* Send events to the Servo Task */
  event_disable_scheduling();
  event_out_shedule(DSP_AGC_EVENT);
  event_out_shedule(DSP_FOCUS_EVENT);
#ifdef APM_PICKUP
  event_out_shedule(DSP_TTM_FG_EVENT);
#endif
#ifdef ECC_WORKAROUND
  event_out_shedule(DSP_CLV_BUFFER_EVENT);
#endif
  event_out_shedule(DSP_RAMP_EVENT);
  event_out_shedule(DSP_SHORT_JUMP_EVENT);
  event_enable_scheduling();

  /* Send back current event code to inform DSP about mailbox_isr_irq completion. */
  DISABLE_INTERRUPTS();
  DSP_EVENT_ACK_L = dsp_event_code;
  DSP_EVENT_ACK_H = (uint8)0x00;
  ENABLE_INTERRUPTS();
}

#endif // HAVE_CD_MECHA

/*** (c) 2003  STMicroelectronics **************************** END OF FILE ***/