ln-dotnet
/
SOES
mirror of https://github.com/OpenEtherCATsociety/SOES.git
3
0
Fork 0
Code Issues Projects Releases Wiki Activity
SOES/soes/esc.c

960 lines
25 KiB
C
Raw Blame History

/*
* Licensed under the GNU General Public License version 2 with exceptions. See
* LICENSE file in the project root for full license information
*/
#include <string.h>
#include <cc.h>
#include "esc.h"
#include "esc_coe.h"
#include "esc_foe.h"
/** \file
* \brief
* Base EtherCAT functions for handling the Data Link Layer and Malilboxes
*
*
* State machine and mailbox support.
*/
static esc_cfg_t * esc_cfg = NULL;
/** Write AL Status Code to the ESC.
*
* @param[in] errornumber = Write an by EtherCAT specified Error number register 0x134 AL Status Code
*/
void ESC_ALerror (uint16_t errornumber)
{
uint16_t dummy;
ESCvar.ALerror = errornumber;
dummy = htoes (errornumber);
ESC_write (ESCREG_ALERROR, &dummy, sizeof (dummy));
}
/** Write AL Status to the ESC.
*
* @param[in] status = Write current slave status to register 0x130 AL Status
* reflecting actual state and error indication if present
*/
void ESC_ALstatus (uint8_t status)
{
uint16_t dummy;
ESCvar.ALstatus = status;
dummy = htoes ((uint16_t) status);
ESC_write (ESCREG_ALSTATUS, &dummy, sizeof (dummy));
}
/** 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);
}
/** 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);
#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
}
/** Write ESCvar.SM[n] data to ESC PDI control register 0x807(+ offset to SyncManager n).
*
* @param[in] n = Write to Sync Manager no. n
*/
void ESC_SMwritepdi (uint8_t n)
{
_ESCsm2 *sm;
sm = (_ESCsm2 *) & ESCvar.SM[n];
ESC_write (ESCREG_SM0PDI + (n << 3), &(sm->ActPDI), 1);
}
/** Write 0 to Bit0 in SM PDI control register 0x807(+ offset to SyncManager n) to Activate the Sync Manager n.
*
* @param[in] n = Write to Sync Manager no. n
*/
void ESC_SMenable (uint8_t n)
{
_ESCsm2 *sm;
sm = (_ESCsm2 *) & ESCvar.SM[n];
sm->ActPDI &= ~ESCREG_SMENABLE_BIT;
ESC_SMwritepdi (n);
}
/** Write 1 to Bit0 in SM PDI control register 0x807(+ offset to SyncManager n) to De-activte the Sync Manager n.
*
* @param[in] n = Write to Sync Manager no. n
*/
void ESC_SMdisable (uint8_t n)
{
_ESCsm2 *sm;
sm = (_ESCsm2 *) & ESCvar.SM[n];
sm->ActPDI |= ESCREG_SMENABLE_BIT;
ESC_SMwritepdi (n);
}
/** Read Configured Station Address register 0x010 assigned by the Master.
*
*/
void ESC_address (void)
{
ESC_read (ESCREG_ADDRESS, (void *) &ESCvar.address, sizeof (ESCvar.address));
ESCvar.address = etohs (ESCvar.address);
}
/** 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);
wdstatus = etohs (wdstatus);
return (uint8_t) wdstatus;
}
/** Check 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_checkmbx (uint8_t state)
{
_ESCsm2 *SM;
ESC_read (ESCREG_SM0, (void *) &ESCvar.SM[0], sizeof (ESCvar.SM[0]));
ESC_read (ESCREG_SM1, (void *) &ESCvar.SM[1], sizeof (ESCvar.SM[1]));
SM = (_ESCsm2 *) & ESCvar.SM[0];
if ((etohs (SM->PSA) != MBX0_sma) || (etohs (SM->Length) != MBX0_sml)
|| (SM->Command != MBX0_smc) || (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) || (etohs (SM->Length) != MBX1_sml)
|| (SM->Command != MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM1;
ESC_SMdisable (0);
ESC_SMdisable (1);
return ESCinit | ESCerror; //fail state change
}
return state;
}
/** Try to start mailboxes for current ALControl state request by enabling SyncManager 0 and 1.
* If all mailbox settings is correct we return incoming state request, otherwise
* we return state Init with Error flag set and update local ALerror with code 0x16 Invalid
* mailbox configuration.
*
* @param[in] state = Current state request read from ALControl 0x0120
* @return if all Mailbox values is correct we return incoming state, otherwise
* we return state Init with Error flag set.
*/
uint8_t ESC_startmbx (uint8_t state)
{
ESC_SMenable (0);
ESC_SMenable (1);
ESC_SMstatus (0);
ESC_SMstatus (1);
if ((state = ESC_checkmbx (state)) & ESCerror)
{
ESC_ALerror (ALERR_INVALIDMBXCONFIG);
MBXrun = 0;
}
else
{
ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
MBXrun = 1;
}
return state;
}
/** 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]));
ESC_read (ESCREG_SM1, (void *) &ESCvar.SM[1], sizeof (ESCvar.SM[1]));
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;
}
/** Try to start bootstrap mailboxes for current ALControl state request by enabling SyncManager 0 and 1.
* If all mailbox settings is correct we return incoming state request, otherwise
* we return state Init with Error flag set and update local ALerror with code 0x16 Invalid
* mailbox configuration.
*
* @param[in] state = Current state request read from ALControl 0x0120
* @return if all Mailbox values is correct we return incoming state, otherwise
* we return state Init with Error flag set.
*/
uint8_t ESC_startmbxboot (uint8_t state)
{
ESC_SMenable (0);
ESC_SMenable (1);
ESC_SMstatus (0);
ESC_SMstatus (1);
if ((state = ESC_checkmbxboot (state)) & ESCerror)
{
ESC_ALerror (ALERR_INVALIDBOOTMBXCONFIG);
MBXrun = 0;
}
else
{
ESCvar.toggle = ESCvar.SM[1].ECrep; //sync repeat request toggle state
MBXrun = 1;
}
return state;
}
/** Stop mailboxes by disabling SyncManager 0 and 1. Clear local mailbox variables
* stored in ESCvar.
*/
void ESC_stopmbx (void)
{
uint8_t n;
MBXrun = 0;
ESC_SMdisable (0);
ESC_SMdisable (1);
for (n = 0; n < MBXBUFFERS; n++)
{
MBXcontrol[n].state = MBXstate_idle;
}
ESCvar.mbxoutpost = 0;
ESCvar.mbxbackup = 0;
ESCvar.xoe = 0;
ESCvar.mbxfree = 1;
ESCvar.toggle = 0;
ESCvar.mbxincnt = 0;
ESCvar.segmented = 0;
ESCvar.frags = 0;
ESCvar.fragsleft = 0;
ESCvar.txcue = 0;
}
/** 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);
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);
if (length + MBXHSIZE < MBX0_sml_b)
{
ESC_read (MBX0_sme_b, &length, 1);
}
}
else
{
ESC_read (MBX0_sma, MB, MBXHSIZE);
length = etohs (MB->header.length);
if (length > (MBX0_sml - MBXHSIZE))
{
length = MBX0_sml - MBXHSIZE;
}
ESC_read (MBX0_sma + MBXHSIZE, &(MB->b[0]), length);
if (length + MBXHSIZE < MBX0_sml)
{
ESC_read (MBX0_sme, &length, 1);
}
}
MBXcontrol[0].state = MBXstate_inclaim;
}
/** Write local mailbox buffer ESCvar.MBX[n] to Send mailbox.
* Combined function for bootstrap and other states. State check decides
* which one to write.
*
* @param[in] n = Which local mailbox buffer n to send.
*/
void ESC_writembx (uint8_t n)
{
_MBX *MB = &MBX[n];
uint8_t dummy = 0;
uint16_t length;
length = etohs (MB->header.length);
if (ESCvar.ALstatus == ESCboot)
{
if (length > (MBX1_sml_b - MBXHSIZE))
{
length = MBX1_sml_b - MBXHSIZE;
}
ESC_write (MBX1_sma_b, MB, MBXHSIZE + length);
if (length + MBXHSIZE < MBX1_sml_b)
{
ESC_write (MBX1_sme_b, &dummy, 1);
}
}
else
{
if (length > (MBX1_sml - MBXHSIZE))
{
length = MBX1_sml - MBXHSIZE;
}
ESC_write (MBX1_sma, MB, MBXHSIZE + length);
if (length + MBXHSIZE < MBX1_sml)
{
ESC_write (MBX1_sme, &dummy, 1);
}
}
ESCvar.mbxfree = 0;
}
/** TBD
*/
void ESC_ackmbxread (void)
{
uint8_t dummy = 0;
if (ESCvar.ALstatus == ESCboot)
{
ESC_write (MBX1_sma_b, &dummy, 1);
}
else
{
ESC_write (MBX1_sma, &dummy, 1);
}
ESCvar.mbxfree = 1;
}
/** Allocate and prepare a mailbox buffer. Take the first Idle buffer from the End.
* Set Mailbox control state to be used for outbox and fill the mailbox buffer with
* address master and mailbox next CNT value between 1-7.
*
* @return The index of Mailbox buffer prepared for outbox. IF no buffer is available return 0.
*/
uint8_t ESC_claimbuffer (void)
{
_MBX *MB;
uint8_t n = MBXBUFFERS - 1;
while ((n > 0) && (MBXcontrol[n].state))
{
n--;
}
if (n)
{
MBXcontrol[n].state = MBXstate_outclaim;
MB = &MBX[n];
ESCvar.mbxcnt = (++ESCvar.mbxcnt) & 0x07;
if (ESCvar.mbxcnt == 0)
{
ESCvar.mbxcnt = 1;
}
MB->header.address = htoes (0x0000); // destination is master
MB->header.channel = 0;
MB->header.priority = 0;
MB->header.mbxcnt = ESCvar.mbxcnt;
ESCvar.txcue++;
}
return n;
}
/** Look for any present requests for posting to the outbox.
*
* @return the index of Mailbox buffer ready to be posted.
*/
uint8_t ESC_outreqbuffer (void)
{
uint8_t n = MBXBUFFERS - 1;
while ((n > 0) && (MBXcontrol[n].state != MBXstate_outreq))
{
n--;
}
return n;
}
/** Allocate and prepare a mailbox buffer for sending an error message. Take the first Idle
* buffer from the end. Set Mailbox control state to be used for outbox and fill the mailbox
* buffer with error information.
*
* @param[in] n = Error number to be sent in mailbox error message.
*/
void MBX_error (uint16_t error)
{
uint8_t MBXout;
_MBXerr *mbxerr;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
mbxerr = (_MBXerr *) & MBX[MBXout];
mbxerr->mbxheader.length = htoes ((uint16_t) 0x04);
mbxerr->mbxheader.mbxtype = MBXERR;
mbxerr->type = htoes ((uint16_t) 0x01);
mbxerr->detail = htoes (error);
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
/** Mailbox routine for implementing the low-level part of the mailbox protocol
* used by Application Layers running on-top of mailboxes. It takes care of sending
* a mailbox, re-sending a mailbox, reading a mailbox and handles a mailbox full event.
*
* @return =0 if nothing to do. =1 if something to be handled by mailbox protocols.
*/
uint8_t ESC_mbxprocess (void)
{
uint8_t mbxhandle = 0;
_MBX *MB = &MBX[0];
if (!MBXrun)
{
/* nothing to do */
return 0;
}
/* SM0/1 access or SMn change event */
if (ESCvar.ALevent & ESCREG_ALEVENT_SM_MASK)
{
ESC_SMstatus (0);
ESC_SMstatus (1);
}
/* outmbx read by master */
if (ESCvar.mbxoutpost && ESCvar.SM[1].IntR)
{
ESC_ackmbxread ();
/* dispose old backup */
if (ESCvar.mbxbackup)
{
MBXcontrol[ESCvar.mbxbackup].state = MBXstate_idle;
}
/* if still to do */
if (MBXcontrol[ESCvar.mbxoutpost].state == MBXstate_again)
{
ESC_writembx (ESCvar.mbxoutpost);
}
/* create new backup */
MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_backup;
ESCvar.mbxbackup = ESCvar.mbxoutpost;
ESCvar.mbxoutpost = 0;
/* Do we have any ongoing protocol transfers, return 1 */
if(ESCvar.xoe > 0)
{
return 1;
}
return 0;
}
/* repeat request */
if (ESCvar.SM[1].ECrep != ESCvar.toggle)
{
if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
{
/* if outmbx empty */
if (!ESCvar.mbxoutpost)
{
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
}
else
{
/* reset mailbox */
ESC_SMdisable (1);
/* have to resend later */
MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_again;
/* activate mailbox */
ESC_SMenable (1);
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
}
ESCvar.toggle = ESCvar.SM[1].ECrep;
ESCvar.SM[1].PDIrep = ESCvar.toggle;
ESC_SMwritepdi (1);
}
return 0;
}
/* if the outmailbox is free check if we have something to send */
if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
{
/* check out request mbx */
mbxhandle = ESC_outreqbuffer ();
/* outmbx empty and outreq mbx available */
if (mbxhandle)
{
ESC_writembx (mbxhandle);
/* change state */
MBXcontrol[mbxhandle].state = MBXstate_outpost;
ESCvar.mbxoutpost = mbxhandle;
if (ESCvar.txcue)
{
ESCvar.txcue--;
}
}
}
/* read mailbox if full and no xoe in progress */
if (ESCvar.SM[0].MBXstat && !MBXcontrol[0].state && !ESCvar.mbxoutpost
&& !ESCvar.xoe)
{
ESC_readmbx ();
ESCvar.SM[0].MBXstat = 0;
if (etohs (MB->header.length) == 0)
{
MBX_error (MBXERR_INVALIDHEADER);
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
if ((MB->header.mbxcnt != 0) && (MB->header.mbxcnt == ESCvar.mbxincnt))
{
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
ESCvar.mbxincnt = MB->header.mbxcnt;
return 1;
}
/* ack changes in non used SM */
if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
ESC_SMack (4);
ESC_SMack (5);
ESC_SMack (6);
ESC_SMack (7);
}
return 0;
}
/** Handler for incorrect or unsupported mailbox data. Write error response
* in Mailbox.
*/
void ESC_xoeprocess (void)
{
_MBXh *mbh;
if (!MBXrun)
{
return;
}
if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
{
mbh = (_MBXh *) & MBX[0];
if ((mbh->mbxtype == 0) || (etohs (mbh->length) == 0))
{
MBX_error (MBXERR_INVALIDHEADER);
}
else
{
MBX_error (MBXERR_UNSUPPORTEDPROTOCOL);
}
/* mailbox type not supported, drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
}
/** 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)
{
_ESCsm2 *SM;
ESC_read (ESCREG_SM2, (void *) &ESCvar.SM[2], sizeof (ESCvar.SM[2]));
ESC_read (ESCREG_SM3, (void *) &ESCvar.SM[3], sizeof (ESCvar.SM[3]));
SM = (_ESCsm2 *) & ESCvar.SM[2];
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 */
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))
{
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
return (ESCpreop | ESCerror);
}
return state;
}
/** Function trying to enable start updating the process data inputs. It calls the check SM 2 & 3
* routine, based on the result from there if enables or disables the Input SyncManager, in addition
* it updates the ALStatusCode case something didn't pass the check.
*
* @param[in] state = Requested state.
* @return = state, incoming state request if every thing checks out OK. =state (PREOP | ERROR) if something isn't correct.
*/
uint8_t ESC_startinput (uint8_t state)
{
state = ESC_checkSM23 (state);
if (state != (ESCpreop | ESCerror))
{
ESC_SMenable (3);
App.state = APPSTATE_INPUT;
}
else
{
ESC_SMdisable (2);
ESC_SMdisable (3);
if (ESCvar.SMtestresult & SMRESULT_ERRSM3)
{
ESC_ALerror (ALERR_INVALIDINPUTSM);
}
else
{
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
}
return state;
}
/** Unconditional stop of updating inputs by disabling Sync Manager 2 & 3.
* Set the App.state to APPSTATE_IDLE.
*
*/
void ESC_stopinput (void)
{
App.state = APPSTATE_IDLE;
ESC_SMdisable (3);
ESC_SMdisable (2);
}
/** Unconditional start of updating outputs by enabling Sync Manager 2.
* Set the App.state to APPSTATE_OUTPUT.
*
* @param[in] state = Not used.
* @return = state unchanged.
*
*/
uint8_t ESC_startoutput (uint8_t state)
{
ESC_SMenable (2);
App.state |= APPSTATE_OUTPUT;
return state;
}
/** Unconditional stop of updating outputs by disabling Sync Manager 2.
* Set the App.state to APPSTATE_ONPUT. Call application hook APP_safeoutput
* letting the user to set safe state values on outputs.
*
*/
void ESC_stopoutput (void)
{
App.state &= APPSTATE_INPUT;
ESC_SMdisable (2);
APP_safeoutput ();
}
/** 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));
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);
}
/** Function copying the application configuration variable
* to the stack local pointer variable.
*
* @param[in] cfg = Pointer to by the Application static declared
* configuration variable holding application specific details. Ex. post- and
* pre state change hooks
*/
void ESC_config (esc_cfg_t * cfg)
{
esc_cfg = cfg;
}
Reference in New Issue View Git Blame Copy Permalink