srvdsp.c

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

/**************************************************
 *
 * srvdsp.c
 *
 * CVS ID:   $Id: srvdsp.c,v 1.23 2007/09/26 15:13:56 dellorto Exp $
 * Author:   Fabio Dell'Orto [FD] - STM
 * Date:     $Date: 2007/09/26 15:13:56 $
 * Revision: $Revision: 1.23 $
 *
 * Description:
 *
 *
 ***************************************************
 *
 * COPYRIGHT (C) ST Microelectronics  2005
 *            All Rights Reserved
 *
 *******************************************************************************
 *  \brief        DSP Driver function
 *
 *  \par          Change History:
 *
 * - BB040924a    Optimized and fixed AGC calculation checking
 * - BB040928a    Code rom and ram size optimisation:
 *                - Optimised DSP access code
 * - BB040929a    Protected writing and reading to DSP from interrupt,
 *                mailbox interrupt also read from DSP (fixed actuator "swinging")
 * - BB050221d    Protected read and write to 16bit DSP subsystem registers
 * - MR060426     DSP generates events (via the mailbox interrupt) when the internal state changes.
 *                The DSP state changes result in the follwing events to be send to the ServoTask:
 *                - DSP_AGC_EVENT:       NO_EVENT, AGC_EVENT_FOCUS_UPDATE, AGC_EVENT_TRACKING_UPDATE
 *                - DSP_FOCUS_EVENT:     NO_EVENT, FOCUS_EVENT_OPEN, FOCUS_EVENT_CLOSED
 *                - DSP_TRACKING_EVENT:  NO_EVENT, TRACKING_EVENT_LOST, TRACKING_EVENT_OK
 *                - DSP_HF_EVENT:        NO_EVENT, HF_EVENT_LOST, HF_EVENT_OK
 *                - DSP_TTM_FG_EVENT:    NO_EVENT, TTM_FG_EVENT_NOT_OK, TTM_FG_EVENT_OK
 *                - DSP_TTM_SPEED_EVENT: NO_EVENT, TTM_SPEED_EVENT_NOT_OK, TTM_SPEED_EVENT_OK
 *                - DSP_RAMP_EVENT:      NO_EVENT, RAMP_EVENT_STOPPED
 *
 ***************************************************
 *
 * STM CVS Log:
 *
 * $Log: srvdsp.c,v $
 * Revision 1.23  2007/09/26 15:13:56  dellorto
 * acquisition defect event instead of dsp hf event
 *
 * Revision 1.22  2007/08/14 15:30:15  dellorto
 * multiple mechanisms
 *
 * Revision 1.21  2007/04/11 14:21:04  belardi
 * Integration of HAVE_CD_MECHA modification by [GP]
 *
 * Revision 1.20  2006/11/10 16:33:48  dellorto
 * no message
 *
 * Revision 1.19  2006/11/07 13:46:55  belardi
 * Added 'volatile' to remove compiler warning
 *
 * Revision 1.18  2006/10/25 09:55:03  papp
 * removed blank lines
 *
 * Revision 1.17  2006/10/09 15:11:35  dellorto
 * no message
 *
 * Revision 1.16  2006/09/19 10:29:30  belardi
 * Removed unused variables
 *
 * Revision 1.15  2006/09/18 14:54:44  belardi
 * White-spaces
 *
 * Revision 1.14  2006/09/18 09:55:25  belardi
 * Corrected CVS keyword usage
 *
 * Revision 1.13  2006/09/18 09:25:37  belardi
 * AddeLogg$ CVS keyword into file header
 *
 *
 ******************************************************************************/

#include "gendef.h"

#if (HAVE_CD_MECHA == 1)

#include "hwreg.h"
#include "osal.h"
#include "srvinc.h"
#include "emeacc.h"
#include "pdebug.h"

#include "srvcfg.h"   /* For DEBUG_SERVOEVENT() */

void mailbox_isr_irq(void);

#define set_dsp_agc_event(e)          do{event_out[DSP_AGC_EVENT].event = e;}while (0)
#define set_dsp_focus_event(e)        do{event_out[DSP_FOCUS_EVENT].event = e;}while (0)
#ifdef APM_PICKUP
#define set_dsp_ttm_fg_event(e)       do{event_out[DSP_TTM_FG_EVENT].event = e;}while (0)
#endif
#define set_clv_buffer_event(e)       do{event_out[DSP_CLV_BUFFER_EVENT].event = e;}while (0)
#define set_dsp_ramp_event(e)         do{event_out[DSP_RAMP_EVENT].event = e;}while (0)
#define set_dsp_short_jump_event(e)   do{event_out[DSP_SHORT_JUMP_EVENT].event = e;}while (0)

static uint8 dsp_code;
DSP_ERROR_UNION dsp_error_flags;


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

void to_dsp_flags_pc(uint8 data)
{
  DISABLE_INTERRUPTS();
  FLAGS_PC_L = data;
  FLAGS_PC_H = (uint8)0x00;
  ENABLE_INTERRUPTS();
}


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

#define to_dsp_write_address(dsp_address) do   \
{                                              \
  DSPWRADD_L = (uint8)(dsp_address);           \
  DSPWRADD_H = (uint8)0x00;                    \
}                                       while(0)


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

#define to_dsp_write_data(dsp_data)       do   \
{                                              \
  DSPWRDAT_L = (uint8)((dsp_data) & 0x00FF);   \
  DSPWRDAT_H = (uint8)((dsp_data) >> 8);       \
}                                       while(0)


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

#define to_dsp_read_address(dsp_address)  do   \
{                                              \
  DSPRDADD_L = (uint8)(dsp_address);           \
  DSPRDADD_H = (uint8)0x00;                    \
}                                       while(0)


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

#define to_dsp_flags(dsp_flags)           do   \
{                                              \
  FLAGS_L = (uint8)((dsp_flags) & 0x00FF);     \
  FLAGS_H = (uint8)((dsp_flags) >> 8);         \
}                                       while(0)


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

#define from_dsp_read_data(p_dsp_data)    do   \
{                                              \
  *p_dsp_data = DSPRDDAT_L;                    \
  *p_dsp_data += (DSPRDDAT_H << 8);            \
}                                       while(0)


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

#define to_dsp_write_code()             do     \
{                                              \
  dsp_code++;                                  \
  ARM7_COM_CODE_L = (uint8)(dsp_code);         \
  ARM7_COM_CODE_H = (uint8)0x00;               \
}                                       while(0)


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

#ifdef DEBUG_CHECK_DSP_ST7_COM
void wait_for_dsp_action(void)
{
  uint8 cnt = 30; /* STTODO S7 time with 30 is 150us timeout till error */

  while ((ARM7_ACK_L != dsp_code) && (!dsp_error_flags.field.dsp_arm7_com_error))
  {
    if (0 == cnt--)
    {
      dsp_error_flags.field.dsp_arm7_com_error = 1;
    }
  }
}
#else
#define wait_for_dsp_action()                  do     \
{                                                     \
  while(ARM7_ACK_L != dsp_code);                      \
}                                              while(0)
#endif


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

void dsp_write_xmem(uint8 address, uint16 data)
{
  DISABLE_INTERRUPTS();
  to_dsp_write_address(address);
  to_dsp_write_data(data);
  to_dsp_write_code();
  to_dsp_flags(WRITE_EN | XIO16_RW_ON);
  wait_for_dsp_action();
  to_dsp_flags(WRITE_EN);
  ENABLE_INTERRUPTS();
}


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

uint16 dsp_read_xmem(uint8 address)
{
  uint16 dsp_data;

  DISABLE_INTERRUPTS();
  to_dsp_read_address(address);
  to_dsp_write_code();
  to_dsp_flags(READ_EN | XIO16_RW_ON);
  wait_for_dsp_action();
  from_dsp_read_data(&dsp_data);
  to_dsp_flags(READ_EN);
  ENABLE_INTERRUPTS();

  return dsp_data;
}


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

void dsp_write_ymem(uint8 address, uint16 data)
{
  DISABLE_INTERRUPTS();
  to_dsp_write_address(address);
  to_dsp_write_data(data);
  to_dsp_write_code();
  to_dsp_flags(WRITE_EN | YIO16_RW_ON);
  wait_for_dsp_action();
  to_dsp_flags(WRITE_EN);
  ENABLE_INTERRUPTS();
}


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

uint16 dsp_read_ymem(uint8 address)
{
  uint16 dsp_data;

  DISABLE_INTERRUPTS();
  to_dsp_read_address(address);
  to_dsp_write_code();
  to_dsp_flags(READ_EN | YIO16_RW_ON);
  wait_for_dsp_action();
  from_dsp_read_data(&dsp_data);
  to_dsp_flags(READ_EN);
  ENABLE_INTERRUPTS();

  return dsp_data;
}


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

void dsp_write_coef(uint8 ymem_offset, uint16 const *filter)
{
  uint8 i;

  for (i = 0; i < 16; i++)
  {
    dsp_write_ymem((uint8)(ymem_offset + i), *(filter + i));
  }
}


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

void loadDSP(uint8 **dsp_param, uint16 *size)
{
  *dsp_param = (uint8 *)DSP_PRAM;