/** * 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(); }
/** 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 (); } }; }
/** 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); }