srvfsm.c

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

/******************************************************************************/
/* Function:  focus_recover_stf                                               */
/*                                                                            */
/*! \brief    State function for FOCUS_RECOVER
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void focus_recover_stf(void)
{
  if (OK == servo_fsm_transition_handling(focus_recover()))
  {
    DEBUG_SERVO(("focus_recover state OK", 4, 0));
    servo_fsm_state = FOCUS_CLOSING;
  }
}


/******************************************************************************/
/* Function:  hf_recover_stf                                                  */
/*                                                                            */
/*! \brief    State function for HF_RECOVER
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void hf_recover_stf(void)
{
  if (OK == servo_fsm_transition_handling(hf_recover()))
  {
    DEBUG_SERVO(("hf_recover state OK", 4, 0));
    servo_fsm_state = TE_ADJUST;
  }
}


/******************************************************************************/
/* Function:  erased_cd_rw_recover_stf                                        */
/*                                                                            */
/*! \brief    State function for ERASED_CD_RW_RECOVER
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void erased_cd_rw_recover_stf(void)
{
  if (OK == servo_fsm_transition_handling(erased_cd_rw_recover()))
  {
    DEBUG_SERVO(("erased_cd_rw_recover state OK", 4, 0));
    servo_fsm_state = TRACK_CLOSING;
  }
}


/******************************************************************************/
/* Function:  pll_recover_stf                                                 */
/*                                                                            */
/*! \brief
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void pll_recover_stf(void)
{
  if (OK == servo_fsm_transition_handling(pll_recover()))
  {
    DEBUG_SERVO(("pll_recover state OK", 4, 0));
    servo_fsm_state = TRACK_CLOSING;
  }
}


/******************************************************************************/
/* Function:  tracking_recover_stf                                            */
/*                                                                            */
/*! \brief    State function for TRACKING_RECOVER
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void tracking_recover_stf(void)
{
  tracking_recover();
  DEBUG_SERVO(("tracking_recover state OK", 4, 0));
  servo_fsm_state = TRACK_CLOSING;
}


/******************************************************************************/
/* Function:  ttm_recover_stf                                            */
/*                                                                            */
/*! \brief    State function for TTM_RECOVER
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void ttm_recover_stf(void)
{
#ifdef APM_PICKUP
  if (OK == servo_fsm_transition_handling(ttm_recover()))
  {
    DEBUG_SERVO(("ttm_recover state OK", 4, 0));
    servo_fsm_state = START_TTM;
  }
#else   /* (not) APM_PICKUP */
  ttm_recover();
  DEBUG_SERVO(("ttm_recover state OK", 4, 0));
  servo_fsm_state = START_TTM;
#endif   /* APM_PICKUP */
}




////////////////////////////////////////////////////////////////////////////////////////////
// Servo Controller Top State Machine
////////////////////////////////////////////////////////////////////////////////////////////


/******************************************************************************/
/* Function:  init_servo_fsm                                                  */
/*                                                                            */
/*! \brief    Initializer for the servo state machine. Interface
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void init_servo_fsm(void)
{
  max_servo_retries = MAX_SERVO_RETRIES;
  servo_fsm_result = BUSY;
  servo_command_result = SERVO_COMMAND_NO_RESULT;
  sm_state_0 = IDLE;
  sm_state_1 = STATE_1;
  sm_state_2 = STATE_1;
  prev_sm_state_0 = SERVO_UNKNOWN;

  event_in[DSP_FOCUS_EVENT].event = FOCUS_EVENT_OPEN;
#ifdef APM_PICKUP
  event_in[DSP_TTM_FG_EVENT].event = TTM_FG_EVENT_OK;
#endif
#ifdef ECC_WORKAROUND
  event_in[DSP_CLV_BUFFER_EVENT].event = CLV_BUFFER_EVENT_NOT_OK;
#endif
  event_in[DSP_RAMP_EVENT].event = RAMP_EVENT_STOPPED;
  event_in[DSP_SHORT_JUMP_EVENT].event = SHORT_JUMP_EVENT_STOPPED;

  init_sledge_fsm();            //BBTODO is this best place?
  init_ttm_fsm();               //BBTODO is this best place?
  init_check_tracking_fsm();    //BBTODO is this best place?
  init_check_hf_fsm();

  servo_misc_flags.field.init_done = 0;
  servo_misc_flags.field.init_for_new_disc_done = 0;
  servo_misc_flags.field.init_request = 0;
  servo_misc_flags.field.init_for_new_disc_request = 0;
  servo_misc_flags.field.start_request = 0;
  reset_servo_errors();
}


/******************************************************************************/
/* Function:  servo_fsm                                                       */
/*                                                                            */
/*! \brief    Servo state machine function. Interface
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void servo_fsm(void)
{
  RETVAL old_fsm_result;

  if (NO_EVENT != event_in[SERVO_CMD_EVENT].pevent)
  {
    servo_fsm_result = BUSY;
    servo_exec_cmd((t_servo_cmd_event *)(event_in[SERVO_CMD_EVENT].pevent));
  }

  if (NO_EVENT != event_in[ACQ_IRQ_SUBCODE_EVENT].event)
  {
    t_acq_subcode_event *psubcode_event = (t_acq_subcode_event *)
                                          event_in[ACQ_IRQ_SUBCODE_EVENT].pevent;

//BB060929a    if (servo_active_flags.field.servo_mode == TOC_JUMP_MODE)
    {
      if (psubcode_event->subcode_event.event_type & ACQ_EVENT_SUBCODE_CRCOK)
      {
        tQsubcode_event *pout_subcode_event = (tQsubcode_event *)
                                              pevent_get_out(SERVO_SUBCODE_EVENT);
        pout_subcode_event->event_type = psubcode_event->subcode_event.event_type;
        pout_subcode_event->q_subcode = psubcode_event->subcode_event.q_subcode;
        pout_subcode_event->T_actual = psubcode_event->T_actual;  /* BB070110a */
        event_set_out(SERVO_SUBCODE_EVENT);
      }
    }

    if (psubcode_event->subcode_event.event_type & ACQ_EVENT_SUBCODE_NEW_T)   // BBTODO
    {
      T_actual = psubcode_event->T_actual;
      freq_1T = psubcode_event->freq_1T;
      acq_subcode_event_type = psubcode_event->subcode_event.event_type;
      servo_acq_flags.field.new_T = 1;       /* BB041110a */
    }
#if PRINTF_BLER
    service_print_error_statistics(); // This will be only used when errors stats is enabled by service commands
#endif
  }

  old_fsm_result = servo_fsm_result;

  if (sm_state_0 != prev_sm_state_0)
  {
    prev_sm_state_0 = sm_state_0;
    sm_state_1 = STATE_1;
    sm_state_2 = STATE_1;
    prev_sm_state_1 = STATE_1;
    prev_sm_state_2 = STATE_1;
    DEBUG_SERVO_STATE(("SERVO_STATE:", 1, 4, sm_state_0, sm_state_1, sm_state_2, error_reason));
  }
  else if (sm_state_1 != prev_sm_state_1)
  {
    prev_sm_state_1 = sm_state_1;
    sm_state_2 = STATE_1;
    prev_sm_state_2 = STATE_1;
    DEBUG_SERVO_STATE(("SERVO_STATE:", 1, 4, sm_state_0, sm_state_1, sm_state_2, error_reason));
  }
  else if (sm_state_2 != prev_sm_state_2)
  {
    prev_sm_state_2 = sm_state_2;
    DEBUG_SERVO_STATE(("SERVO_STATE:", 1, 4, sm_state_0, sm_state_1, sm_state_2, error_reason));
  }

  switch (servo_fsm_state)
  {
  case SERVO_INIT:
    init_servo_stf();
    break;

  case SERVO_INIT_FOR_NEW_DISC:
    init_servo_for_new_disc_stf();
    break;

  case IDLE:
    servo_fsm_result = idle_stf();
    break;

  case SERVO_START:
    init_start_stf();
    break;

  case FE_ADJUST:
    fe_adjust_stf();
    break;

  case START_TTM:
    start_ttm_stf();
    break;

  case FOCUS_CLOSING:
    focus_closing_stf();
    break;

  case TE_ADJUST:
    te_adjust_stf();
    break;

  case TRACK_CLOSING:
    track_closing_stf();
    break;

  case AGC_FOCUS:
    agc_focus_stf();
    break;

  case AGC_TRACKING:
    agc_tracking_stf();
    break;

  case JUMP_TTM_CHECK:
    jump_ttm_check_stf();
    break;

  case ACCESS_CHECK:
    access_check_stf();
    break;

  case ACTIVE_CHECK:
    servo_fsm_result = active_check_stf();
    break;

  case SERVO_STOP:
    servo_stop_stf();
    break;

  case START_JUMP:
    start_jump_stf();
    break;

  case JUMP_CHECK:
    jump_check_stf();
    break;

  case ERROR_STATE:
    error_stf();
    break;

  case FATAL_ERROR:
    servo_fsm_result = fatal_error_stf();
    break;

  case FOCUS_RECOVER:
    focus_recover_stf();
    break;

  case HF_RECOVER:
    hf_recover_stf();
    break;

  case TRACKING_RECOVER:
    tracking_recover_stf();
    break;

  case ERASED_CD_RW_RECOVER:
    erased_cd_rw_recover_stf();
    break;

  case PLL_RECOVER:
    pll_recover_stf();
    break;

  case TTM_RECOVER:
    ttm_recover_stf();
    break;

  default:
    break;
  }

  if (old_fsm_result != servo_fsm_result)  /* FD060929b */
  {
    if (BUSY != servo_fsm_result)
    {
      if (READY == servo_fsm_result)
      {
        if (SERVO_CMD_NOEVENT != tmp_cmd)
        {
          event_set_servo(READY);
          /* TODO to be replaced with: event_set_servo((tmp_cmd << 8) | READY); */
          /* when extended READY will be supported                         */
          DEBUG_SERVO_STATUS_EVENT(("SERVO_STATUS_EVENT: READY", 1, 1, tmp_cmd));
          tmp_cmd = SERVO_CMD_NOEVENT;
        }
        else
        {
          /* should never occur: for protection do not send READY event            */
          /* for the moment it occurs accessing IDLE state, after init_servo_fsm() */
          /* and in case of SERVO_EVENT_ENDPLAY_XXXX                               */
          DEBUG_SERVO_STATUS_EVENT(("SERVO_STATUS_EVENT: UNEXPECTED READY", 4, 0));
        }
      }
      else   /* XXXX_ERROR == servo_fsm_result */
      {
        event_set_servo(servo_fsm_result);
        DEBUG_SERVO_STATUS_EVENT(("SERVO_EVENT: ERROR", 1, 1, tmp_cmd));
      }
    }
  }

  if ((sm_state_0 != prev_sm_state_0)
   || (sm_state_1 != prev_sm_state_1)
   || (sm_state_2 != prev_sm_state_2))
  {
    OSAL_wake_thread(OSAL_THREAD_ServoTask);
  }
}

void ServoTask(void *unused)
{
  while (1)
  {
    OSAL_wait_thread(OSAL_THREAD_ServoTask);

    event_disable_scheduling();
    event_in_shedule(SERVO_CMD_EVENT);
    event_in_shedule(DSP_AGC_EVENT);
    event_in_shedule(DSP_FOCUS_EVENT);
#ifdef APM_PICKUP
    event_in_shedule(DSP_TTM_FG_EVENT);
#endif
#ifdef ECC_WORKAROUND
    event_in_shedule(DSP_CLV_BUFFER_EVENT);
#endif
    event_in_shedule(DSP_RAMP_EVENT);
    event_in_shedule(DSP_SHORT_JUMP_EVENT);
    event_in_shedule(TRACKING_EVENT);
    event_in_shedule(HF_EVENT);
    event_in_shedule(SLEDGE_SW_EVENT);
    event_in_shedule(SLEDGE_STATUS_EVENT);
    event_in_shedule(TTM_EVENT);
#ifdef ECC_WORKAROUND
    event_in_shedule(CLV_IRQ_EVENT);
#endif
    event_in_shedule(ACQ_IRQ_SUBCODE_EVENT);
    event_enable_scheduling();

    servo_fsm();

    event_disable_scheduling();
    event_in_clear(SERVO_CMD_EVENT);
    event_in_clear(ACQ_IRQ_SUBCODE_EVENT);
    event_out_shedule(SERVO_STATUS_EVENT);
    event_out_shedule(SERVO_SUBCODE_EVENT);
    event_out_shedule(SERVO_ENDPLAY_EVENT);
    event_enable_scheduling();
  }
}

/******************************************************************************/
/* Function:  get_servo_action                                                */
/*                                                                            */
/*! \brief    Interface. Returns current action of servo fsm depending on state.
 *  \param    void
 *  \return   void
 *  \remark
 */
/******************************************************************************/

uint8 get_servo_action(void)
{
  uint8 servo_action_result;

  servo_action_result = servo_fsm_state & SERVO_ACTION_MASK;

  if ((servo_action_result == MOVING_ACTION_1) || (servo_action_result == MOVING_ACTION_2))
  {
    servo_action_result = MOVING_ACTION;
  }

  return servo_action_result;
}
#endif // #if (HAVE_CD_MECHA == 1)