/** Check boostrap mailbox status by reading all SyncManager 0 and 1 data. The read values
* are compared with local definitions for SM Physical Address, SM Length and SM Control.
* If we check fails we disable Mailboxes by disabling SyncManager 0 and 1 and return
* state Init with Error flag set.
*
* @param[in] state = Current state request read from ALControl 0x0120
* @return if all Mailbox values is correct we return incoming state request, otherwise
* we return state Init with Error flag set.
*/
uint8_t ESC_checkmbxboot (uint8_t state)
{
_ESCsm2 *SM;
ESC_read (ESCREG_SM0, (void *) &ESCvar.SM[0], sizeof (ESCvar.SM[0]),
(void *) &ESCvar.ALevent);
ESC_read (ESCREG_SM1, (void *) &ESCvar.SM[1], sizeof (ESCvar.SM[1]),
(void *) &ESCvar.ALevent);
SM = (_ESCsm2 *) & ESCvar.SM[0];
if ((etohs (SM->PSA) != MBX0_sma_b) || (etohs (SM->Length) != MBX0_sml_b)
|| (SM->Command != MBX0_smc_b) || (ESCvar.SM[0].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM0;
ESC_SMdisable (0);
ESC_SMdisable (1);
return (uint8_t) (ESCinit | ESCerror); //fail state change
}
SM = (_ESCsm2 *) & ESCvar.SM[1];
if ((etohs (SM->PSA) != MBX1_sma_b) || (etohs (SM->Length) != MBX1_sml_b)
|| (SM->Command != MBX1_smc_b) || (ESCvar.SM[1].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM1;
ESC_SMdisable (0);
ESC_SMdisable (1);
return (uint8_t) (ESCinit | ESCerror); //fail state change
}
return state;
}
/** Validate the values of Sync Manager 2 & 3 that the current ESC values is
* equal to configured and calculated local values.
*
* @param[in] state = Requested state.
* @return = incoming state request if every thing checks out OK. = state (PREOP | ERROR) if something isn't correct.
*/
uint8_t ESC_checkSM23 (uint8_t state)
{
uint64_t sm2;
// printf("checking SM2 & SM3\n");
_ESCsm2 *SM;
// printf(".");
ESC_read (ESCREG_SM2, (void *) &ESCvar.SM[2], sizeof (ESCvar.SM[2]),
(void *) &ESCvar.ALevent);
ESC_read(ESCREG_SM2, (void*)&sm2, 8, (void*)&ESCvar.ALevent);
// printf(".");
ESC_read (ESCREG_SM3, (void *) &ESCvar.SM[3], sizeof (ESCvar.SM[3]),
(void *) &ESCvar.ALevent);
// printf(".\n");
SM = (_ESCsm2 *) & ESCvar.SM[2];
// printf("read SM2, comparing\n");
if ((etohs (SM->PSA) != SM2_sma) || (etohs (SM->Length) != SM2_sml)
|| (SM->Command != SM2_smc) || !(SM->ActESC & SM2_act))
{
ESCvar.SMtestresult = SMRESULT_ERRSM2;
/* fail state change */
// printf("SM2:\tread\tvs\tneeded\n_sma\t%x\tvs\t%x\n_sml\t%d\tvs\t%d\n_smc\t%x\tvs\t%x\n_act\t%x\tvs\t%x\n",SM->PSA,SM2_sma,SM->Length,SM2_sml,SM->Command,SM2_smc,SM->ActESC,SM2_act);
return (ESCpreop | ESCerror);
}
SM = (_ESCsm2 *) & ESCvar.SM[3];
if ((etohs (SM->PSA) != SM3_sma) || (etohs (SM->Length) != SM3_sml)
|| (SM->Command != SM3_smc) || !(SM->ActESC & SM3_act))
{
// printf("SM3:\tread\tvs\needed\n_sma\t%x\tvs\t%x\n_sml\t%d\tvs\t%d\n_smc\t%x\tvs\t%x\n_act\t%x\tvs\t%x\n",etohs (SM->PSA),SM3_sma,etohs (SM->Length),SM3_sml,SM->Command,SM3_smc,SM->ActESC,SM3_act);
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
return (ESCpreop | ESCerror);
}
return state;
}
/** EPP periodic task of ESC side EEPROM emulation.
*
*/
void EEP_process (void)
{
eep_stat_t stat;
/* check for eeprom event */
if ((ESCvar.ALevent & ESCREG_ALEVENT_EEP) == 0) {
return;
}
while (1) {
/* read eeprom status */
ESC_read (ESCREG_EECONTSTAT, &stat, sizeof (eep_stat_t));
stat.contstat.reg = etohs(stat.contstat.reg);
stat.addr = etohl(stat.addr);
/* check busy flag, exit if job finished */
if (!stat.contstat.bits.busy) {
return;
}
/* clear error bits */
stat.contstat.bits.csumErr = 0;
stat.contstat.bits.eeLoading = 0;
stat.contstat.bits.ackErr = 0;
stat.contstat.bits.wrErr = 0;
/* process commands */
switch (stat.contstat.bits.cmdReg) {
case EEP_CMD_IDLE:
break;
case EEP_CMD_READ:
case EEP_CMD_RELOAD:
/* handle read request */
if (EEP_read (stat.addr * sizeof(uint16_t), eep_buf, EEP_READ_SIZE) != 0) {
stat.contstat.bits.ackErr = 1;
} else {
ESC_write (ESCREG_EEDATA, eep_buf, EEP_READ_SIZE);
}
break;
case EEP_CMD_WRITE:
/* handle write request */
ESC_read (ESCREG_EEDATA, eep_buf, EEP_WRITE_SIZE);
if (EEP_write (stat.addr * sizeof(uint16_t), eep_buf, EEP_WRITE_SIZE) != 0) {
stat.contstat.bits.ackErr = 1;
}
break;
default:
stat.contstat.bits.ackErr = 1;
}
/* acknowledge command */
stat.contstat.reg = htoes(stat.contstat.reg);
ESC_write (ESCREG_EECONTSTAT, &stat.contstat.reg, sizeof(uint16_t));
}
}
/**
* 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();
ESC_foeprocess();
ESC_xoeprocess();
}
/* Call emulated eeprom handler if set */
if (ESCvar.esc_hw_eep_handler != NULL)
{
(ESCvar.esc_hw_eep_handler)();
}
}
/** PDI ISR handler
*
* @param[in] arg = NOT USED
*/
static void ecat_isr (void * arg)
{
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
CC_ATOMIC_SET(ESCvar.ALevent, etohl(ecat0->AL_EVENT_REQ));
if(ESCvar.ALevent & ESCREG_ALEVENT_SM2)
{
if(ESCvar.dcsync == 0)
{
DIG_process(DIG_PROCESS_OUTPUTS_FLAG | DIG_PROCESS_APP_HOOK_FLAG |
DIG_PROCESS_INPUTS_FLAG);
}
else
{
DIG_process(DIG_PROCESS_OUTPUTS_FLAG);
}
}
#if 1
if(ESCvar.ALevent & (ESCREG_ALEVENT_CONTROL | ESCREG_ALEVENT_SMCHANGE
| ESCREG_ALEVENT_SM0 | ESCREG_ALEVENT_SM1 | ESCREG_ALEVENT_EEP))
{
/* Mask interrupts while servicing them */
ecat0->AL_EVENT_MASK &= ~(ESCREG_ALEVENT_CONTROL | ESCREG_ALEVENT_SMCHANGE
| ESCREG_ALEVENT_SM0 | ESCREG_ALEVENT_SM1 | ESCREG_ALEVENT_EEP);
extern flags_t * ecat_events;
flags_set(ecat_events, EVENT_ISR);
//sem_signal(ecat_isr_sem);
}
#endif
}
/**
* Initialize the slave stack.
*/
void ecat_slv_init (esc_cfg_t * config)
{
DPRINT ("Slave stack init started\n");
ESCvar.TXPDOsize = ESCvar.ESC_SM3_sml = sizeOfPDO(TX_PDO_OBJIDX);
ESCvar.RXPDOsize = ESCvar.ESC_SM2_sml = sizeOfPDO(RX_PDO_OBJIDX);
/* 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);
}
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx();
ESC_stopinput();
ESC_stopoutput();
}
/** Read Watchdog Status register 0x440. Result Bit0 0= Expired, 1= Active or disabled.
*
* @return value of register Watchdog Status.
*/
uint8_t ESC_WDstatus (void)
{
uint16_t wdstatus;
ESC_read (ESCREG_WDSTATUS, &wdstatus, 2, (void *) &ESCvar.ALevent);
wdstatus = etohs (wdstatus);
return (uint8_t) wdstatus;
}
void read_MOSI(void){
if((Ec_state&APPSTATE_OUTPUT)&&(ESCvar.ALevent&ESCREG_ALEVENT_SM2)){
ESC_read(SM2_sma,(uint8_t*)&mosi_packet,RXPDOsize,(uint8_t*)&ESCvar.ALevent);
if(status.no_op_state==0)update_actuators();
status.timeout=0;
MOSI_time=ESCvar.Time;
}
}
/** Mandatory: 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 (SM2_sma, &Wb, ESCvar.RXPDOsize);
}
}
/** Read SM Status register 0x805(+ offset to SyncManager n) and save the
* result in global variable ESCvar.SM[n].
*
* @param[in] n = Read Sync Manager no. n
*/
void ESC_SMstatus (uint8_t n)
{
_ESCsm2 *sm;
uint16_t temp;
sm = (_ESCsm2 *) & ESCvar.SM[n];
ESC_read (ESCREG_SM0STATUS + (n << 3), &temp, 2, (void *) &ESCvar.ALevent);
#if defined(EC_LITTLE_ENDIAN)
sm->ActESC = temp >> 8;
sm->Status = temp;
#endif
#if defined(EC_BIG_ENDIAN)
sm->ActESC = temp;
sm->Status = temp >> 8;
#endif
}
/** Read Receive mailbox and store data in local ESCvar.MBX variable.
* Combined function for bootstrap and other states. State check decides
* which one to read.
*/
void ESC_readmbx (void)
{
_MBX *MB = &MBX[0];
uint16_t length;
if (ESCvar.ALstatus == ESCboot)
{
ESC_read (MBX0_sma_b, MB, MBXHSIZE, (void *) &ESCvar.ALevent);
length = etohs (MB->header.length);
if (length > (MBX0_sml_b - MBXHSIZE))
{
length = MBX0_sml_b - MBXHSIZE;
}
ESC_read (MBX0_sma_b + MBXHSIZE, &(MB->b[0]), length,
(void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX0_sml_b)
{
ESC_read (MBX0_sme_b, &length, 1, (void *) &ESCvar.ALevent);
}
}
else
{
ESC_read (MBX0_sma, MB, MBXHSIZE, (void *) &ESCvar.ALevent);
length = etohs (MB->header.length);
if (length > (MBX0_sml - MBXHSIZE))
{
length = MBX0_sml - MBXHSIZE;
}
ESC_read (MBX0_sma + MBXHSIZE, &(MB->b[0]), length,
(void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX0_sml)
{
ESC_read (MBX0_sme, &length, 1, (void *) &ESCvar.ALevent);
}
}
MBXcontrol[0].state = MBXstate_inclaim;
}
/** SOES main loop. Start by initializing the stack software followed by
* the application loop for cyclic read the EtherCAT state and staus, update
* of I/O.
*/
void soes (void *arg)
{
DPRINT ("SOES (Simple Open EtherCAT Slave)\n");
TXPDOsize = SM3_sml = sizeTXPDO ();
RXPDOsize = SM2_sml = sizeRXPDO ();
/* Setup post config hooks */
static esc_cfg_t config =
{
.pre_state_change_hook = NULL,
.post_state_change_hook = post_state_change_hook
};
ESC_config ((esc_cfg_t *)&config);
ESC_reset();
ESC_init (spi_name);
task_delay (tick_from_ms (200));
/* 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);
}
/* Pre FoE to set up Application information */
bootstrap_foe_init ();
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
DPRINT ("Application_loop GO\n");
/* application run loop */
while (1)
{
/* On init restore PDO mappings to default size */
if((ESCvar.ALstatus & 0x0f) == ESCinit)
{
txpdomap = DEFAULTTXPDOMAP;
rxpdomap = DEFAULTRXPDOMAP;
txpdoitems = DEFAULTTXPDOITEMS;
rxpdoitems = DEFAULTTXPDOITEMS;
}
/* 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 ();
/* If else to two separate execution paths
* If we're running BOOSTRAP
* - MailBox
* - FoE
* Else we're running normal execution
* - MailBox
* - CoE
*/
if(local_boot_state)
{
if (ESC_mbxprocess ())
{
ESC_foeprocess ();
ESC_xoeprocess ();
}
bootstrap_state ();
}
else
{
if (ESC_mbxprocess ())
{
ESC_coeprocess ();
ESC_xoeprocess ();
}
DIG_process ();
}
};
}
/** Mandatory: Read Sync Manager 2 to local process data, Master Outputs.
*/
void RXPDO_update (void)
{
ESC_read (SM2_sma, &Wb.LED, RXPDOsize);
}
/** The state handler acting on ALControl Bit(0) and SyncManager Activation BIT(4)
* events in the Al Event Request register 0x220.
*
*/
void ESC_state (void)
{
uint8_t ac, an, as, ax, ax23;
uint8_t handle_smchanged = 0;
/* Do we have a state change request pending */
if (ESCvar.ALevent & ESCREG_ALEVENT_CONTROL)
{
ESC_read (ESCREG_ALCONTROL, (void *) &ESCvar.ALcontrol,
sizeof (ESCvar.ALcontrol), (void *) &ESCvar.ALevent);
ESCvar.ALcontrol = etohs (ESCvar.ALcontrol);
}
/* Have at least on Sync Manager changed */
else if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
handle_smchanged = 1;
}
else
{
/* nothing to do */
return;
}
/* Mask state request bits + Error ACK */
ac = ESCvar.ALcontrol & ESCREG_AL_STATEMASK;
as = ESCvar.ALstatus & ESCREG_AL_STATEMASK;
an = as;
if (((ac & ESCerror) || (ac == ESCinit)))
{
/* if error bit confirmed reset */
ac &= ESCREG_AL_ERRACKMASK;
an &= ESCREG_AL_ERRACKMASK;
}
/* Enter SM changed handling for all steps but Init and Boot when Mailboxes
* is up and running
*/
if (handle_smchanged && (as & ESCREG_AL_ALLBUTINITMASK) &&
!(as == ESCboot) && MBXrun)
{
/* Validate Sync Managers, reading the Activation register will
* acknowledge the SyncManager Activation event making us enter
* this execution path.
*/
ax = ESC_checkmbx (as);
ax23 = ESC_checkSM23 (as);
if ((an & ESCerror) && !(ac & ESCerror))
{
/* if in error then stay there */
return;
}
/* Have we been forced to step down to INIT we will stop mailboxes,
* update AL Status Code and exit ESC_state
*/
if (ax == (ESCinit | ESCerror))
{
/* If we have activated Inputs and Outputs we need to disable them */
if (App.state)
{
ESC_stopoutput ();
ESC_stopinput ();
}
/* Stop mailboxes and update ALStatus code */
ESC_stopmbx ();
ESC_ALerror (ALERR_INVALIDMBXCONFIG);
MBXrun = 0;
ESC_ALstatus (ax);
return;
}
/* Have we been forced to step down to PREOP we will stop inputs
* and outputs, update AL Status Code and exit ESC_state
*/
if ((App.state) && (ax23 == (ESCpreop | ESCerror)))
{
ESC_stopoutput ();
ESC_stopinput ();
if (ESCvar.SMtestresult & SMRESULT_ERRSM3)
{
ESC_ALerror (ALERR_INVALIDINPUTSM);
}
else
{
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
ESC_ALstatus (ax23);
return;
}
}
/* Error state not acked, leave original */
if ((an & ESCerror) && !(ac & ESCerror))
{
return;
}
/* Mask high bits ALcommand, low bits ALstatus */
as = (ac << 4) | (as & 0x0f);
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->pre_state_change_hook)
{
esc_cfg->pre_state_change_hook (&as, &an);
}
/* Switch through the state change requested via AlControl from
* current state read in AL status
*/
switch (as)
{
case INIT_TO_INIT:
case PREOP_TO_PREOP:
case OP_TO_OP:
{
break;
}
case INIT_TO_PREOP:
{
/* get station address */
ESC_address ();
an = ESC_startmbx (ac);
break;
}
case INIT_TO_BOOT:
case BOOT_TO_BOOT:
{
/* get station address */
ESC_address ();
an = ESC_startmbxboot (ac);
break;
}
case INIT_TO_SAFEOP:
case INIT_TO_OP:
{
an = ESCinit | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_INIT:
{
ESC_stopoutput ();
}
case SAFEOP_TO_INIT:
{
ESC_stopinput ();
}
case PREOP_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case BOOT_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case PREOP_TO_BOOT:
case BOOT_TO_PREOP:
case BOOT_TO_SAFEOP:
case BOOT_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case PREOP_TO_SAFEOP:
case SAFEOP_TO_SAFEOP:
{
SM2_sml = sizeRXPDO ();
SM3_sml = sizeTXPDO ();
an = ESC_startinput (ac);
if (an == ac)
{
ESC_SMenable (2);
}
break;
}
case PREOP_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_PREOP:
{
ESC_stopoutput ();
}
case SAFEOP_TO_PREOP:
{
ESC_stopinput ();
an = ESCpreop;
break;
}
case SAFEOP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case SAFEOP_TO_OP:
{
an = ESC_startoutput (ac);
break;
}
case OP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
ESC_stopoutput ();
break;
}
case OP_TO_SAFEOP:
{
an = ESCsafeop;
ESC_stopoutput ();
break;
}
default:
{
if (an == ESCop)
{
ESC_stopoutput ();
an = ESCsafeop;
}
if (as == ESCsafeop)
{
ESC_stopinput ();
}
an |= ESCerror;
ESC_ALerror (ALERR_UNKNOWNSTATE);
break;
}
}
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->post_state_change_hook)
{
esc_cfg->post_state_change_hook (&as, &an);
}
if (!(an & ESCerror) && (ESCvar.ALerror))
{
/* clear error */
ESC_ALerror (ALERR_NONE);
}
ESC_ALstatus (an);
}
/** Read SM Status register 0x805(+ offset to SyncManager n) to acknowledge a
* Sync Manager event Bit 3 in ALevent. The result is not used.
*
* @param[in] n = Read Sync Manager no. n
*/
void ESC_SMack (uint8_t n)
{
uint16_t dummy;
ESC_read (ESCREG_SM0STATUS + (n << 3), &dummy, 2, (void *) &ESCvar.ALevent);
}
/** Mandatory: Read Sync Manager 2 to local process data, Master Outputs.
*/
void RXPDO_update (void)
{
ESC_read (SM2_sma, &Wb.LED, RXPDOsize, (void *) &ESCvar.ALevent);
}
/** Read Configured Station Address register 0x010 assigned by the Master.
*
*/
void ESC_address (void)
{
ESC_read (ESCREG_ADDRESS, (void *) &ESCvar.address, sizeof (ESCvar.address),
(void *) &ESCvar.ALevent);
ESCvar.address = etohs (ESCvar.address);
}
