387 lines
9.6 KiB
C
Executable File
387 lines
9.6 KiB
C
Executable File
/*
|
|
* Licensed under the GNU General Public License version 2 with exceptions. See
|
|
* LICENSE file in the project root for full license information
|
|
*/
|
|
#include <stddef.h>
|
|
#include "esc.h"
|
|
#include "esc_coe.h"
|
|
#include "esc_foe.h"
|
|
#include "esc_eoe.h"
|
|
#include "ecat_slv.h"
|
|
|
|
#define IS_RXPDO(index) ((index) >= 0x1600 && (index) < 0x1800)
|
|
#define IS_TXPDO(index) ((index) >= 0x1A00 && (index) < 0x1C00)
|
|
|
|
/* Global variables used by the stack */
|
|
uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE, MBXSIZEBOOT)];
|
|
_MBXcontrol MBXcontrol[MBXBUFFERS];
|
|
_SMmap SMmap2[MAX_MAPPINGS_SM2];
|
|
_SMmap SMmap3[MAX_MAPPINGS_SM3];
|
|
_ESCvar ESCvar;
|
|
|
|
/* Private variables */
|
|
static volatile int watchdog;
|
|
|
|
#if MAX_MAPPINGS_SM2 > 0
|
|
static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned(8)));
|
|
#else
|
|
extern uint8_t *rxpdo;
|
|
#endif
|
|
|
|
#if MAX_MAPPINGS_SM3 > 0
|
|
static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned(8)));
|
|
#else
|
|
extern uint8_t *txpdo;
|
|
#endif
|
|
|
|
/** Function to pre-qualify the incoming SDO download.
|
|
*
|
|
* @param[in] index = index of SDO download request to check
|
|
* @param[in] sub-index = sub-index of SDO download request to check
|
|
* @return SDO abort code, or 0 on success
|
|
*/
|
|
uint32_t ESC_download_pre_objecthandler(uint16_t index,
|
|
uint8_t subindex,
|
|
void *data,
|
|
size_t size,
|
|
uint16_t flags)
|
|
{
|
|
if (IS_RXPDO(index) ||
|
|
IS_TXPDO(index) ||
|
|
index == RX_PDO_OBJIDX ||
|
|
index == TX_PDO_OBJIDX)
|
|
{
|
|
uint8_t minSub = ((flags & COMPLETE_ACCESS_FLAG) == 0) ? 0 : 1;
|
|
if (subindex > minSub && COE_maxSub(index) != 0)
|
|
{
|
|
return ABORT_SUBINDEX0_NOT_ZERO;
|
|
}
|
|
}
|
|
|
|
if (ESCvar.pre_object_download_hook)
|
|
{
|
|
return (ESCvar.pre_object_download_hook)(index,
|
|
subindex,
|
|
data,
|
|
size,
|
|
flags);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/** Hook called from the slave stack SDO Download handler to act on
|
|
* user specified Index and Sub-index.
|
|
*
|
|
* @param[in] index = index of SDO download request to handle
|
|
* @param[in] sub-index = sub-index of SDO download request to handle
|
|
* @return SDO abort code, or 0 on success
|
|
*/
|
|
uint32_t ESC_download_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
|
|
{
|
|
if (ESCvar.post_object_download_hook != NULL)
|
|
{
|
|
return (ESCvar.post_object_download_hook)(index, subindex, flags);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/** Function to pre-qualify the incoming SDO upload.
|
|
*
|
|
* @param[in] index = index of SDO upload request to handle
|
|
* @param[in] sub-index = sub-index of SDO upload request to handle
|
|
* @return SDO abort code, or 0 on success
|
|
*/
|
|
uint32_t ESC_upload_pre_objecthandler(uint16_t index,
|
|
uint8_t subindex,
|
|
void *data,
|
|
size_t size,
|
|
uint16_t flags)
|
|
{
|
|
if (ESCvar.pre_object_upload_hook != NULL)
|
|
{
|
|
return (ESCvar.pre_object_upload_hook)(index,
|
|
subindex,
|
|
data,
|
|
size,
|
|
flags);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/** Hook called from the slave stack SDO Upload handler to act on
|
|
* user specified Index and Sub-index.
|
|
*
|
|
* @param[in] index = index of SDO upload request to handle
|
|
* @param[in] sub-index = sub-index of SDO upload request to handle
|
|
* @return SDO abort code, or 0 on success
|
|
*/
|
|
uint32_t ESC_upload_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
|
|
{
|
|
if (ESCvar.post_object_upload_hook != NULL)
|
|
{
|
|
return (ESCvar.post_object_upload_hook)(index, subindex, flags);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/** Hook called from the slave stack ESC_stopoutputs to act on state changes
|
|
* forcing us to stop outputs. Here we can set them to a safe state.
|
|
*/
|
|
void APP_safeoutput(void)
|
|
{
|
|
DPRINT("APP_safeoutput\n");
|
|
|
|
if (ESCvar.safeoutput_override != NULL)
|
|
{
|
|
(ESCvar.safeoutput_override)();
|
|
}
|
|
}
|
|
|
|
/** Write local process data to Sync Manager 3, Master Inputs.
|
|
*/
|
|
void TXPDO_update(void)
|
|
{
|
|
if (ESCvar.txpdo_override != NULL)
|
|
{
|
|
(ESCvar.txpdo_override)();
|
|
}
|
|
else
|
|
{
|
|
if (MAX_MAPPINGS_SM3 > 0)
|
|
{
|
|
COE_pdoPack(txpdo, ESCvar.sm3mappings, SMmap3);
|
|
}
|
|
ESC_write(ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
|
|
}
|
|
}
|
|
|
|
/** Read Sync Manager 2 to local process data, Master Outputs.
|
|
*/
|
|
void RXPDO_update(void)
|
|
{
|
|
if (ESCvar.rxpdo_override != NULL)
|
|
{
|
|
(ESCvar.rxpdo_override)();
|
|
}
|
|
else
|
|
{
|
|
ESC_read(ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
|
|
if (MAX_MAPPINGS_SM2 > 0)
|
|
{
|
|
COE_pdoUnpack(rxpdo, ESCvar.sm2mappings, SMmap2);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Set the watchdog count value, don't have any affect when using
|
|
* HW watchdog 0x4xx
|
|
*
|
|
* @param[in] watchdogcnt = new watchdog count value
|
|
*/
|
|
void APP_setwatchdog(int watchdogcnt)
|
|
{
|
|
CC_ATOMIC_SET(ESCvar.watchdogcnt, watchdogcnt);
|
|
}
|
|
|
|
/* Function to update local I/O, call read ethercat outputs, call
|
|
* write ethercat inputs. Implement watch-dog counter to count-out if we have
|
|
* made state change affecting the App.state.
|
|
*/
|
|
|
|
void DIG_process(uint16_t ALEvent, uint8_t flags)
|
|
{
|
|
/* Handle watchdog */
|
|
if ((flags & DIG_PROCESS_WD_FLAG) > 0)
|
|
{
|
|
if (CC_ATOMIC_GET(watchdog) > 0)
|
|
{
|
|
CC_ATOMIC_SUB(watchdog, 1);
|
|
}
|
|
|
|
if ((CC_ATOMIC_GET(watchdog) <= 0) &&
|
|
((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0))
|
|
{
|
|
DPRINT("DIG_process watchdog expired\n");
|
|
ESC_ALstatusgotoerror((ESCsafeop | ESCerror), ALERR_WATCHDOG);
|
|
}
|
|
else if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
|
|
{
|
|
CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
|
|
}
|
|
}
|
|
|
|
/* Handle Outputs */
|
|
if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
|
|
{
|
|
if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
|
|
(ALEvent & ESCREG_ALEVENT_SM2))
|
|
{
|
|
RXPDO_update();
|
|
CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
|
|
/* Set outputs */
|
|
cb_set_outputs();
|
|
}
|
|
else if (ALEvent & ESCREG_ALEVENT_SM2)
|
|
{
|
|
RXPDO_update();
|
|
}
|
|
}
|
|
|
|
/* Call application */
|
|
if ((flags & DIG_PROCESS_APP_HOOK_FLAG) > 0)
|
|
{
|
|
/* Call application callback if set */
|
|
if (ESCvar.application_hook != NULL)
|
|
{
|
|
(ESCvar.application_hook)();
|
|
}
|
|
}
|
|
|
|
/* Handle Inputs */
|
|
if ((flags & DIG_PROCESS_INPUTS_FLAG) > 0)
|
|
{
|
|
if (CC_ATOMIC_GET(ESCvar.App.state) > 0)
|
|
{
|
|
/* Update inputs */
|
|
cb_get_inputs();
|
|
TXPDO_update();
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
|
|
* control what interrupts that should be served and re-activated with
|
|
* event mask argument
|
|
*/
|
|
void ecat_slv_worker(uint32_t event_mask)
|
|
{
|
|
do
|
|
{
|
|
/* Check the state machine */
|
|
ESC_state();
|
|
/* Check the SM activation event */
|
|
ESC_sm_act_event();
|
|
|
|
/* Check mailboxes */
|
|
while ((ESC_mbxprocess() > 0) || (ESCvar.txcue > 0))
|
|
{
|
|
ESC_coeprocess();
|
|
#if USE_FOE
|
|
ESC_foeprocess();
|
|
#endif
|
|
#if USE_EOE
|
|
ESC_eoeprocess();
|
|
#endif
|
|
ESC_xoeprocess();
|
|
}
|
|
#if USE_EOE
|
|
ESC_eoeprocess_tx();
|
|
#endif
|
|
/* Call emulated eeprom handler if set */
|
|
if (ESCvar.esc_hw_eep_handler != NULL)
|
|
{
|
|
(ESCvar.esc_hw_eep_handler)();
|
|
}
|
|
|
|
CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
|
|
|
|
} while (ESCvar.ALevent & event_mask);
|
|
|
|
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | event_mask);
|
|
}
|
|
|
|
/*
|
|
* Polling function. It should be called periodically for an application
|
|
* when only SM2/DC interrupt is active.
|
|
* Read and handle events for the EtherCAT state, status, mailbox and eeprom.
|
|
*/
|
|
void ecat_slv_poll(void)
|
|
{
|
|
/* Read local time from ESC*/
|
|
ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
|
|
ESCvar.Time = etohl(ESCvar.Time);
|
|
|
|
/* Check the state machine */
|
|
ESC_state();
|
|
/* Check the SM activation event */
|
|
ESC_sm_act_event();
|
|
|
|
/* Check mailboxes */
|
|
if (ESC_mbxprocess())
|
|
{
|
|
ESC_coeprocess();
|
|
#if USE_FOE
|
|
ESC_foeprocess();
|
|
#endif
|
|
#if USE_EOE
|
|
ESC_eoeprocess();
|
|
#endif
|
|
ESC_xoeprocess();
|
|
}
|
|
#if USE_EOE
|
|
ESC_eoeprocess_tx();
|
|
#endif
|
|
|
|
/* Call emulated eeprom handler if set */
|
|
if (ESCvar.esc_hw_eep_handler != NULL)
|
|
{
|
|
(ESCvar.esc_hw_eep_handler)();
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Poll all events in a free-run application
|
|
*/
|
|
void ecat_slv(void)
|
|
{
|
|
ecat_slv_poll();
|
|
DIG_process(ESC_ALeventread(), DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
|
|
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
|
|
}
|
|
|
|
/*
|
|
* Initialize the slave stack.
|
|
*/
|
|
void ecat_slv_init(esc_cfg_t *config)
|
|
{
|
|
/* Init watchdog */
|
|
watchdog = config->watchdog_cnt;
|
|
|
|
/* Call stack configuration */
|
|
ESC_config(config);
|
|
/* Call HW init */
|
|
ESC_init(config);
|
|
|
|
/* wait until ESC is started up */
|
|
while ((ESCvar.DLstatus & 0x0001) == 0)
|
|
{
|
|
ESC_read(ESCREG_DLSTATUS, (void *)&ESCvar.DLstatus,
|
|
sizeof(ESCvar.DLstatus));
|
|
ESCvar.DLstatus = etohs(ESCvar.DLstatus);
|
|
}
|
|
|
|
#if USE_FOE
|
|
/* Init FoE */
|
|
FOE_init();
|
|
#endif
|
|
|
|
#if USE_EOE
|
|
/* Init EoE */
|
|
EOE_init();
|
|
#endif
|
|
|
|
/* reset ESC to init state */
|
|
ESC_ALstatus(ESCinit);
|
|
ESC_ALerror(ALERR_NONE);
|
|
ESC_stopmbx();
|
|
ESC_stopinput();
|
|
ESC_stopoutput();
|
|
/* Init Object Dictionary default values */
|
|
COE_initDefaultValues();
|
|
}
|