sdo.c

AVR平台下的CanOpen协议桟源码包括应用

/* ------------------------------------------------------------------------
File   : sdo.c

Descr  : The CANopen SDO server, which serves read/write requests to the
	 Object Dictionary.

History: 25JAN.00; Henk B&B; Start of development of a version for the ELMB.
--------------------------------------------------------------------------- */

#include "general.h"
#include "adc.h"
#include "can.h"
#include "crc.h"
#include "dac.h"
#include "digio.h"
#include "guarding.h"
#include "objects.h"
#include "pdo.h"
#include "serialno.h"
#include "spi_gp.h"
#include "store.h"
#include "timer103.h"

#ifndef __ELMB103__
#include "watchdog.h"
#endif

#ifdef __2313_SLAVE_PRESENT__
#include "download.h"
#endif /* __2313_SLAVE_PRESENT__ */

#ifdef __INCLUDE_TESTS__
#include "iotest.h"
#endif

/* ------------------------------------------------------------------------ */
/* Local prototypes */

static BYTE sdo_expedited_read( BYTE *msg_data );
static BYTE sdo_expedited_write( BYTE *msg_data );
static void sdo_abort( BYTE error_class,
		       BYTE error_code,
		       BYTE *msg_data );

static void jump_to_bootloader( void );

/* ------------------------------------------------------------------------ */

void sdo_server( BYTE *msg_data )
{
  BYTE sdo_mode;
  BYTE cs;
  BYTE sdo_error;

  /* SDO modifier bits are in the first data byte */
  sdo_mode = msg_data[0];

  /* Determine the command specifier (cs) from
     the SDO modifier bits in the first byte */
  cs = sdo_mode & SDO_COMMAND_SPECIFIER_MASK;

  switch( cs )
    {
    case SDO_INITIATE_UPLOAD_REQ:
      /* ==> Read from the Object Dictionary <== */

      /* Expedited transfer only (data: 4 bytes or less) */

      /* Process the SDO received */
      sdo_error = sdo_expedited_read( msg_data );

      /* Send the SDO reply... */
      if( !sdo_error )
	{
	  /* All went okay */
	  can_write( C91_SDOTX, C91_SDOTX_LEN, msg_data );
	}
      else
	{
	  /* Aborted... */
	  sdo_abort( SDO_ECLASS_ACCESS, sdo_error, msg_data );
	}
      break;

    case SDO_INITIATE_DOWNLOAD_REQ:
      /* ==> Write to the Object Dictionary <== */

      if( sdo_mode & SDO_EXPEDITED )
	{
	  /* Expedited transfer (data: 4 bytes or less) */

	  /* Process the SDO received */
	  sdo_error = sdo_expedited_write( msg_data );

	  /* Send the SDO reply... */
	  if( !sdo_error )
	    {
	      /* All went okay */
	      can_write( C91_SDOTX, C91_SDOTX_LEN, msg_data );
	    }
	  else
	    {
	      /* Aborted... */
	      sdo_abort( SDO_ECLASS_ACCESS, sdo_error, msg_data );
	    }
	}
      else
	{
	  /* Start of segmented transfer (OD write): no service... */
	  sdo_abort( SDO_ECLASS_SERVICE, SDO_ECODE_PAR_ILLEGAL, msg_data );
	}
      break;

    case SDO_UPLOAD_SEGMENT_REQ:
      /* ==> Read from the Object Dictionary (segmented): no service... <== */
      sdo_abort( SDO_ECLASS_SERVICE, SDO_ECODE_PAR_ILLEGAL, msg_data );
      break;

    default:
      /* Unknown command specifier !? */
      sdo_abort( SDO_ECLASS_SERVICE, SDO_ECODE_PAR_ILLEGAL, msg_data );
      break;
    }
}

/* ------------------------------------------------------------------------ */

static BYTE sdo_expedited_read( BYTE *msg_data )
{
  BYTE sdo_error;
  BYTE nbytes;
  BYTE od_index_hi, od_index_lo, od_subind;

  /* No error */
  sdo_error   = 0;

  /* Extract Object Dictionary indices */
  od_index_lo = msg_data[1];
  od_index_hi = msg_data[2];
  od_subind   = msg_data[3];

  /* Initialise data bytes to zero */
  msg_data[4] = 0;
  msg_data[5] = 0;
  msg_data[6] = 0;
  msg_data[7] = 0;

  /* Default number of significant bytes:
     set to a different value if it saves more statements,
     now default assuming 32-bit data item... */
  nbytes = 4;

  /* Get the requested object */
  switch( od_index_hi )
    {
    case OD_DEVICE_INFO_HI:
      switch( od_index_lo )
	{
	case OD_DEVICE_TYPE_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = DEVICE_TYPE_CHAR0;
	      msg_data[5] = DEVICE_TYPE_CHAR1;
	      msg_data[6] = DEVICE_TYPE_CHAR2;
	      msg_data[7] = DEVICE_TYPE_CHAR3;
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_ERROR_REG_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = CANopenErrorReg;
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_STATUS_REG_LO:
	  if( od_subind == 0 )
	    {
	      adc_status( &msg_data[4] );
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_DEVICE_NAME_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = MNFCT_DEV_NAME_CHAR0;
	      msg_data[5] = MNFCT_DEV_NAME_CHAR1;
	      msg_data[6] = MNFCT_DEV_NAME_CHAR2;
	      msg_data[7] = MNFCT_DEV_NAME_CHAR3;
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_HW_VERSION_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = MNFCT_HARDW_VERSION_CHAR0;
	      msg_data[5] = MNFCT_HARDW_VERSION_CHAR1;
	      msg_data[6] = MNFCT_HARDW_VERSION_CHAR2;
	      msg_data[7] = MNFCT_HARDW_VERSION_CHAR3;
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_SW_VERSION_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = MNFCT_SOFTW_VERSION_CHAR0;
	      msg_data[5] = MNFCT_SOFTW_VERSION_CHAR1;
	      msg_data[6] = MNFCT_SOFTW_VERSION_CHAR2;
	      msg_data[7] = MNFCT_SOFTW_VERSION_CHAR3;
	    }
	  else
	    {
	      if( od_subind == 1 )
		{
		  msg_data[4] = SOFTW_MINOR_VERSION_CHAR0;
		  msg_data[5] = SOFTW_MINOR_VERSION_CHAR1;
		  msg_data[6] = SOFTW_MINOR_VERSION_CHAR2;
		  msg_data[7] = SOFTW_MINOR_VERSION_CHAR3;
		}
	      else
		{
		  /* The sub-index does not exist */
		  sdo_error = SDO_ECODE_ATTRIBUTE;
		}
	    }
	  break;

	case OD_GUARDTIME_LO:
	  if( od_subind == 0 )
	    {
	      nbytes = guarding_get_guardtime( &msg_data[4] );
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_LIFETIME_FACTOR_LO:
	  if( od_subind == 0 )
	    {
	      nbytes = guarding_get_lifetime( &msg_data[4] );
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_STORE_PARAMETERS_LO:
	case OD_DFLT_PARAMETERS_LO:
	  if( od_subind == OD_NO_OF_ENTRIES )
	    {
	      msg_data[4] = OD_STORE_MAX_SUBID;
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      if( od_subind <= OD_STORE_MAX_SUBID )
		{
		  /* Device saves parameters on command (OD_STORE_PARAMETERS),
		     restores parameters (OD_DFLT_PARAMETERS) */
		  msg_data[4] = 0x01;

		  /* ###??? Device saves parameters autonomously
		     (OD_STORE_PARAMETERS_LO) */
		  /*if( od_ind_lo == OD_STORE_PARAMETERS_LO )
		    msg_data[4] = 0x03; */
		}
	      else
		{
		  /* The sub-index does not exist */
		  sdo_error = SDO_ECODE_ATTRIBUTE;
		}
	    }
	  break;

	case OD_HEARTBEAT_TIME_LO:
	  if( od_subind == 0 )
	    {
	      nbytes = guarding_get_heartbeattime( &msg_data[4] );
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_IDENTITY_LO:
	  switch( od_subind )
	    {
	    case OD_NO_OF_ENTRIES:
	      msg_data[4] = 1;
	      nbytes = 1;  /* Significant bytes < 4 */
	      break;
	    case 1:
	      msg_data[4] = 0x78;
	      msg_data[5] = 0x56;
	      msg_data[6] = 0x34;
	      msg_data[7] = 0x12;
	      break;
	    default:
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	      break;
	    }
	  break;

	default:
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	  break;
	}
      break;

    case OD_RPDO_PAR_HI:
      if( od_index_lo < RPDO_CNT )
	{
	  if( rpdo_get_comm_par( od_index_lo, od_subind,
				 &nbytes, &msg_data[4] ) == FALSE )
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	}
      else
	{
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	}
      break;

    case OD_RPDO_MAP_HI:
      if( od_index_lo < RPDO_CNT )
	{
	  if( rpdo_get_mapping( od_index_lo, od_subind,
				&nbytes, &msg_data[4] ) == FALSE )
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	}
      else
	{
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	}
      break;

    case OD_TPDO_PAR_HI:
      if( od_index_lo < TPDO_CNT )
	{
	  if( tpdo_get_comm_par( od_index_lo, od_subind,
				 &nbytes, &msg_data[4] ) == FALSE )
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	}
      else
	{
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	}
      break;

    case OD_TPDO_MAP_HI:
      if( od_index_lo < TPDO_CNT )
	{
	  if( tpdo_get_mapping( od_index_lo, od_subind,
				&nbytes, &msg_data[4] ) == FALSE )
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	}
      else
	{
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	}
      break;

    case OD_CAN_CONFIG_HI:
      if( od_index_lo == OD_CAN_CONFIG_LO )
	{
	  switch( od_subind )
	    {
	    case OD_NO_OF_ENTRIES:
	      msg_data[4] = 3;
	      nbytes = 1;  /* Significant bytes < 4 */
	      break;
	    case 1:
	      msg_data[4] = can_get_rtr_disabled();
	      nbytes = 1;  /* Significant bytes < 4 */
	      break;
	    case 2:
	      msg_data[4] = can_get_opstate_init();
	      nbytes = 1;  /* Significant bytes < 4 */
	      break;
	    case 3:
	      msg_data[4] = can_get_busoff_maxcnt();
	      nbytes = 1;  /* Significant bytes < 4 */
	      break;
	    default:
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	      break;
	    }
	}
      else
	{
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	}
      break;

    case OD_DIGITAL_IN_HI:
      switch( od_index_lo )
	{
	case OD_DIGITAL_IN_8_LO:
	  if( od_subind == OD_NO_OF_ENTRIES )
	    {
	      msg_data[4] = digin_port_cnt();
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      if( od_subind <= digin_port_cnt() )
		{
		  msg_data[4] = digin_get_port( od_subind );
		  nbytes = 1;  /* Significant bytes < 4 */
		}
	      else
		{
		  /* The sub-index does not exist */
		  sdo_error = SDO_ECODE_ATTRIBUTE;
		}
	    }
	  break;

	case OD_DIGIN_INTRPT_ENA_LO:
	  if( od_subind == 0 )
	    {
	      msg_data[4] = digin_get_intrpt_ena();
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      /* The sub-index does not exist */
	      sdo_error = SDO_ECODE_ATTRIBUTE;
	    }
	  break;

	case OD_DIGIN_INTRPT_MSK_LO:
	  if( od_subind == OD_NO_OF_ENTRIES )
	    {
	      msg_data[4] = digin_port_cnt();
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      if( od_subind <= digin_port_cnt() )
		{
		  msg_data[4] = digin_get_intrpt_mask( od_subind );
		  nbytes = 1;  /* Significant bytes < 4 */
		}
	      else
		{
		  /* The sub-index does not exist */
		  sdo_error = SDO_ECODE_ATTRIBUTE;
		}
	    }
	  break;

	default:
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	  break;
	}
      break;

    case OD_DIGITAL_OUT_HI:
      switch( od_index_lo )
	{
	case OD_DIGITAL_OUT_8_LO:
	case OD_DIGITAL_OUT_8_MSK_LO:
	  if( od_subind == OD_NO_OF_ENTRIES )
	    {
	      msg_data[4] = digout_port_cnt();
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      if( od_subind <= digout_port_cnt() )
		{
		  if( od_index_lo == OD_DIGITAL_OUT_8_LO )
		    msg_data[4] = digout_get_port( od_subind );
		  else
		    msg_data[4] = digout_get_mask( od_subind );
		  nbytes = 1;  /* Significant bytes < 4 */
		}
	      else
		{
		  /* The sub-index does not exist */
		  sdo_error = SDO_ECODE_ATTRIBUTE;
		}
	    }
	  break;

	default:
	  /* The index can not be accessed, does not exist */
	  sdo_error = SDO_ECODE_NONEXISTENT;
	  break;
	}
      break;

    case OD_ANALOG_IN_VOLTS_HI:
      if( od_index_lo == OD_ANALOG_IN_VOLTS_LO )
	{
	  if( od_subind == OD_NO_OF_ENTRIES )
	    {
	      msg_data[4] = ADC_MAX_INPUTS;
	      nbytes = 1;  /* Significant bytes < 4 */
	    }
	  else
	    {
	      if( od_subind <= ADC_MAX_INPUTS )
		{
		  if( adc_read_volts( od_subind-1, &msg_data[4] ) == FALSE )
		    {
		      /* Something went wrong... */
		      sdo_error = SDO_ECODE_HARDWARE;
		    }
		}
	      else
		{