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

extend esc_config for generic stack configuration

pull/32/head
rtlaka 2017-10-25 14:31:30 +02:00
parent 7e380b671c
commit 7715a2fffd
6 changed files with 258 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
soes/config.h
View File

@ -1,34 +0,0 @@
#ifndef __config_h__
#define __config_h__
#define MBXSIZE 0x80
#define MBXSIZEBOOT 0x80
#define MBXBUFFERS 3
#define MBX0_sma 0x1000
#define MBX0_sml MBXSIZE
#define MBX0_sme MBX0_sma+MBX0_sml-1
#define MBX0_smc 0x26
#define MBX1_sma 0x1080
#define MBX1_sml MBXSIZE
#define MBX1_sme MBX1_sma+MBX1_sml-1
#define MBX1_smc 0x22
#define MBX0_sma_b 0x1000
#define MBX0_sml_b MBXSIZEBOOT
#define MBX0_sme_b MBX0_sma_b+MBX0_sml_b-1
#define MBX0_smc_b 0x26
#define MBX1_sma_b 0x1080
#define MBX1_sml_b MBXSIZEBOOT
#define MBX1_sme_b MBX1_sma_b+MBX1_sml_b-1
#define MBX1_smc_b 0x22
#define SM2_sma 0x1100
#define SM2_smc 0x24
#define SM2_act 0x01
#define SM3_sma 0x1180
#define SM3_smc 0x20
#define SM3_act 0x01
#endif

209
soes/esc.c
View File

@ -15,7 +15,17 @@
*
* State machine and mailbox support.
*/
static esc_cfg_t * esc_cfg = NULL;
/* Global variables used by the stack */
esc_cfg_t * esc_cfg = NULL;
size_t ESC_MBXSIZE;
uint16_t ESC_MBX0_sma;
uint16_t ESC_MBX0_sml;
uint16_t ESC_MBX0_sme;
uint8_t ESC_MBX0_smc;
uint16_t ESC_MBX1_sma;
uint16_t ESC_MBX1_sml;
uint16_t ESC_MBX1_sme;
uint8_t ESC_MBX1_smc;
/** Write AL Status Code to the ESC.
*
@ -28,6 +38,7 @@ void ESC_ALerror (uint16_t 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
@ -143,8 +154,8 @@ uint8_t ESC_checkmbx (uint8_t state)
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))
if ((etohs (SM->PSA) != ESC_MBX0_sma) || (etohs (SM->Length) != ESC_MBX0_sml)
|| (SM->Command != ESC_MBX0_smc) || (ESCvar.SM[0].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM0;
ESC_SMdisable (0);
@ -152,8 +163,8 @@ uint8_t ESC_checkmbx (uint8_t state)
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))
if ((etohs (SM->PSA) != ESC_MBX1_sma) || (etohs (SM->Length) != ESC_MBX1_sml)
|| (SM->Command != ESC_MBX1_smc) || (ESCvar.SM[1].ECsm == 0))
{
ESCvar.SMtestresult = SMRESULT_ERRSM1;
ESC_SMdisable (0);
@ -173,6 +184,17 @@ uint8_t ESC_checkmbx (uint8_t state)
*/
uint8_t ESC_startmbx (uint8_t state)
{
/* Assign SM settings */
ESC_MBXSIZE = esc_cfg->mbxsize;
ESC_MBX0_sma = esc_cfg->mb[0].cfg_sma;
ESC_MBX0_sml = esc_cfg->mb[0].cfg_sml;
ESC_MBX0_sme = esc_cfg->mb[0].cfg_sme;
ESC_MBX0_smc = esc_cfg->mb[0].cfg_smc;
ESC_MBX1_sma = esc_cfg->mb[1].cfg_sma;
ESC_MBX1_sml = esc_cfg->mb[1].cfg_sml;
ESC_MBX1_sme = esc_cfg->mb[1].cfg_sme;
ESC_MBX1_smc = esc_cfg->mb[1].cfg_smc;
ESC_SMenable (0);
ESC_SMenable (1);
ESC_SMstatus (0);
@ -189,40 +211,6 @@ uint8_t ESC_startmbx (uint8_t state)
}
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
@ -235,11 +223,22 @@ uint8_t ESC_checkmbxboot (uint8_t state)
*/
uint8_t ESC_startmbxboot (uint8_t state)
{
/* Assign SM settings */
ESC_MBXSIZE = esc_cfg->mbxsizeboot;
ESC_MBX0_sma = esc_cfg->mb_boot[0].cfg_sma;
ESC_MBX0_sml = esc_cfg->mb_boot[0].cfg_sml;
ESC_MBX0_sme = esc_cfg->mb_boot[0].cfg_sme;
ESC_MBX0_smc = esc_cfg->mb_boot[0].cfg_smc;
ESC_MBX1_sma = esc_cfg->mb_boot[1].cfg_sma;
ESC_MBX1_sml = esc_cfg->mb_boot[1].cfg_sml;
ESC_MBX1_sme = esc_cfg->mb_boot[1].cfg_sme;
ESC_MBX1_smc = esc_cfg->mb_boot[1].cfg_smc;
ESC_SMenable (0);
ESC_SMenable (1);
ESC_SMstatus (0);
ESC_SMstatus (1);
if ((state = ESC_checkmbxboot (state)) & ESCerror)
if ((state = ESC_checkmbx (state)) & ESCerror)
{
ESC_ALerror (ALERR_INVALIDBOOTMBXCONFIG);
MBXrun = 0;
@ -261,7 +260,7 @@ void ESC_stopmbx (void)
MBXrun = 0;
ESC_SMdisable (0);
ESC_SMdisable (1);
for (n = 0; n < MBXBUFFERS; n++)
for (n = 0; n < ESC_MBXBUFFERS; n++)
{
MBXcontrol[n].state = MBXstate_idle;
}
@ -283,39 +282,22 @@ void ESC_stopmbx (void)
*/
void ESC_readmbx (void)
{
_MBX *MB = &MBX[0];
_MBX *MB = (_MBX *)&MBX[0];
uint16_t length;
if (ESCvar.ALstatus == ESCboot)
{
ESC_read (MBX0_sma_b, MB, MBXHSIZE);
length = etohs (MB->header.length);
ESC_read (ESC_MBX0_sma, MB, ESC_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
if (length > (ESC_MBX0_sml - ESC_MBXHSIZE))
{
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);
}
length = ESC_MBX0_sml - ESC_MBXHSIZE;
}
ESC_read (ESC_MBX0_sma + ESC_MBXHSIZE, MB->b, length);
if (length + ESC_MBXHSIZE < ESC_MBX0_sml)
{
ESC_read (ESC_MBX0_sme, &length, 1);
}
MBXcontrol[0].state = MBXstate_inclaim;
}
/** Write local mailbox buffer ESCvar.MBX[n] to Send mailbox.
@ -326,34 +308,21 @@ void ESC_readmbx (void)
*/
void ESC_writembx (uint8_t n)
{
_MBX *MB = &MBX[n];
_MBXh *MBh = (_MBXh *)&MBX[n * ESC_MBXSIZE];
uint8_t dummy = 0;
uint16_t length;
length = etohs (MB->header.length);
if (ESCvar.ALstatus == ESCboot)
length = etohs (MBh->length);
if (length > (ESC_MBX1_sml - ESC_MBXHSIZE))
{
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);
}
length = ESC_MBX1_sml - ESC_MBXHSIZE;
}
else
ESC_write (ESC_MBX1_sma, MBh, ESC_MBXHSIZE + length);
if (length + ESC_MBXHSIZE < ESC_MBX1_sml)
{
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);
}
ESC_write (ESC_MBX1_sme, &dummy, 1);
}
ESCvar.mbxfree = 0;
}
@ -362,14 +331,8 @@ void ESC_writembx (uint8_t n)
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);
}
ESC_write (ESC_MBX1_sma, &dummy, 1);
ESCvar.mbxfree = 1;
}
@ -381,8 +344,8 @@ void ESC_ackmbxread (void)
*/
uint8_t ESC_claimbuffer (void)
{
_MBX *MB;
uint8_t n = MBXBUFFERS - 1;
_MBXh *MBh;
uint8_t n = ESC_MBXBUFFERS - 1;
while ((n > 0) && (MBXcontrol[n].state))
{
n--;
@ -390,16 +353,16 @@ uint8_t ESC_claimbuffer (void)
if (n)
{
MBXcontrol[n].state = MBXstate_outclaim;
MB = &MBX[n];
MBh = (_MBXh *)&MBX[n * ESC_MBXSIZE];
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;
MBh->address = htoes (0x0000); // destination is master
MBh->channel = 0;
MBh->priority = 0;
MBh->mbxcnt = ESCvar.mbxcnt;
ESCvar.txcue++;
}
return n;
@ -411,7 +374,7 @@ uint8_t ESC_claimbuffer (void)
*/
uint8_t ESC_outreqbuffer (void)
{
uint8_t n = MBXBUFFERS - 1;
uint8_t n = ESC_MBXBUFFERS - 1;
while ((n > 0) && (MBXcontrol[n].state != MBXstate_outreq))
{
n--;
@ -431,7 +394,7 @@ void MBX_error (uint16_t error)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
mbxerr = (_MBXerr *) & MBX[MBXout];
mbxerr = (_MBXerr *) &MBX[MBXout * ESC_MBXSIZE];
mbxerr->mbxheader.length = htoes ((uint16_t) 0x04);
mbxerr->mbxheader.mbxtype = MBXERR;
mbxerr->type = htoes ((uint16_t) 0x01);
@ -449,7 +412,7 @@ void MBX_error (uint16_t error)
uint8_t ESC_mbxprocess (void)
{
uint8_t mbxhandle = 0;
_MBX *MB = &MBX[0];
_MBXh *MBh = (_MBXh *)&MBX[0];
if (!MBXrun)
{
@ -544,18 +507,18 @@ uint8_t ESC_mbxprocess (void)
{
ESC_readmbx ();
ESCvar.SM[0].MBXstat = 0;
if (etohs (MB->header.length) == 0)
if (etohs (MBh->length) == 0)
{
MBX_error (MBXERR_INVALIDHEADER);
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
if ((MB->header.mbxcnt != 0) && (MB->header.mbxcnt == ESCvar.mbxincnt))
if ((MBh->mbxcnt != 0) && (MBh->mbxcnt == ESCvar.mbxincnt))
{
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
ESCvar.mbxincnt = MB->header.mbxcnt;
ESCvar.mbxincnt = MBh->mbxcnt;
return 1;
}
@ -582,7 +545,7 @@ void ESC_xoeprocess (void)
}
if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
{
mbh = (_MBXh *) & MBX[0];
mbh = (_MBXh *) &MBX[0];
if ((mbh->mbxtype == 0) || (etohs (mbh->length) == 0))
{
MBX_error (MBXERR_INVALIDHEADER);
@ -606,18 +569,18 @@ 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))
if ((etohs (SM->PSA) != ESC_SM2_sma) || (etohs (SM->Length) != ESC_SM2_sml)
|| (SM->Command != ESC_SM2_smc) || !(SM->ActESC & ESC_SM2_act))
{
ESCvar.SMtestresult = SMRESULT_ERRSM2;
/* fail state change */
return (ESCpreop | ESCerror);
}
ESC_read (ESCREG_SM3, (void *) &ESCvar.SM[3], sizeof (ESCvar.SM[3]));
SM = (_ESCsm2 *) & ESCvar.SM[3];
if ((etohs (SM->PSA) != SM3_sma) || (etohs (SM->Length) != SM3_sml)
|| (SM->Command != SM3_smc) || !(SM->ActESC & SM3_act))
if ((etohs (SM->PSA) != ESC_SM3_sma) || (etohs (SM->Length) != ESC_SM3_sml)
|| (SM->Command != ESC_SM3_smc) || !(SM->ActESC & ESC_SM3_act))
{
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
@ -786,7 +749,7 @@ void ESC_state (void)
}
/* Error state not acked, leave original */
if ((an & ESCerror) && !(ac & ESCerror))
if ((an & ESCerror) && ((ac & ESCerror) == 0))
{
return;
}
@ -795,7 +758,7 @@ void ESC_state (void)
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)
if (esc_cfg->pre_state_change_hook != NULL)
{
esc_cfg->pre_state_change_hook (&as, &an);
}
@ -865,8 +828,8 @@ void ESC_state (void)
case PREOP_TO_SAFEOP:
case SAFEOP_TO_SAFEOP:
{
SM2_sml = sizeRXPDO ();
SM3_sml = sizeTXPDO ();
ESC_SM2_sml = sizeOfPDO (RX_PDO_OBJIDX);
ESC_SM3_sml = sizeOfPDO (TX_PDO_OBJIDX);
an = ESC_startinput (ac);
if (an == ac)
{
@ -932,7 +895,7 @@ void ESC_state (void)
}
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->post_state_change_hook)
if (esc_cfg->post_state_change_hook != NULL)
{
esc_cfg->post_state_change_hook (&as, &an);
}

76
soes/esc.h
View File

@ -12,7 +12,6 @@
#define __esc__
#include <cc.h>
#include "config.h"
#define ESCREG_ADDRESS 0x0010
#define ESCREG_DLSTATUS 0x0110
@ -140,9 +139,6 @@
#define COE_VALUEINFO_MAXIMUM 0x40
#define COE_MINIMUM_LENGTH 8
#define MBXHSIZE sizeof(_MBXh)
#define MBXDSIZE MBXSIZE-MBXHSIZE
#define MBXERR 0x00
#define MBXAOE 0x01
#define MBXEOE 0x02
@ -327,7 +323,7 @@ typedef struct CC_PACKED
typedef struct CC_PACKED
{
_MBXh header;
uint8_t b[MBXDSIZE];
uint8_t b[0];
} _MBX;
typedef struct CC_PACKED
@ -406,18 +402,15 @@ typedef struct CC_PACKED
};
} _FOEh;
#define FOEHSIZE (sizeof(_FOEh))
#define FOE_DATA_SIZE (MBXSIZEBOOT - (MBXHSIZE+FOEHSIZE))
typedef struct CC_PACKED
{
_MBXh mbxheader;
_FOEh foeheader;
union
{
char filename[FOE_DATA_SIZE];
uint8_t data[FOE_DATA_SIZE];
char errortext[FOE_DATA_SIZE];
char filename[0];
uint8_t data[0];
char errortext[0];
};
} _FOE;
@ -435,12 +428,64 @@ typedef struct
uint8_t state;
} _MBXcontrol;
typedef struct sm_cfg
{
uint16_t cfg_sma;
uint16_t cfg_sml;
uint16_t cfg_sme;
uint8_t cfg_smc;
uint8_t cfg_smact;
}sm_cfg_t;
typedef struct esc_cfg
{
void * user_arg;
int use_interrupt;
int watchdog_cnt;
size_t mbxsize;
size_t mbxsizeboot;
int mbxbuffers;
sm_cfg_t mb[2];
sm_cfg_t mb_boot[2];
sm_cfg_t pdosm[2];
void (*pre_state_change_hook) (uint8_t * as, uint8_t * an);
void (*post_state_change_hook) (uint8_t * as, uint8_t * an);
void (*application_hook) (void);
void (*safeoutput_override) (void);
int (*pre_object_download_hook) (uint16_t index, uint8_t subindex);
void (*post_object_download_hook) (uint16_t index, uint8_t subindex);
void (*rxpdo_override) (void);
void (*txpdo_override) (void);
void (*esc_hw_interrupt_enable) (uint32_t mask);
void (*esc_hw_interrupt_disable) (uint32_t mask);
void (*esc_hw_eep_handler) (void);
} esc_cfg_t;
/* Stack reference to application configuration of the ESC */
extern esc_cfg_t * esc_cfg;
extern size_t ESC_MBXSIZE;
extern uint16_t ESC_MBX0_sma;
extern uint16_t ESC_MBX0_sml;
extern uint16_t ESC_MBX0_sme;
extern uint8_t ESC_MBX0_smc;
extern uint16_t ESC_MBX1_sma;
extern uint16_t ESC_MBX1_sml;
extern uint16_t ESC_MBX1_sme;
extern uint8_t ESC_MBX1_smc;
#define ESC_MBXBUFFERS (esc_cfg->mbxbuffers)
#define ESC_SM2_sma (esc_cfg->pdosm[0].cfg_sma)
#define ESC_SM2_smc (esc_cfg->pdosm[0].cfg_smc)
#define ESC_SM2_act (esc_cfg->pdosm[0].cfg_smact)
#define ESC_SM3_sma (esc_cfg->pdosm[1].cfg_sma)
#define ESC_SM3_smc (esc_cfg->pdosm[1].cfg_smc)
#define ESC_SM3_act (esc_cfg->pdosm[1].cfg_smact)
#define ESC_MBXHSIZE sizeof(_MBXh)
#define ESC_MBXDSIZE (ESC_MBXSIZE - ESC_MBXHSIZE)
#define ESC_FOEHSIZE sizeof(_FOEh)
#define ESC_FOE_DATA_SIZE (ESC_MBXSIZE - (ESC_MBXHSIZE +ESC_FOEHSIZE))
void ESC_config (esc_cfg_t * cfg);
void ESC_ALerror (uint16_t errornumber);
void ESC_ALstatus (uint8_t status);
@ -461,17 +506,16 @@ void ESC_state (void);
/* From hardware file */
void ESC_read (uint16_t address, void *buf, uint16_t len);
void ESC_write (uint16_t address, void *buf, uint16_t len);
void ESC_init (const void * arg);
void ESC_init (const esc_cfg_t * cfg);
void ESC_reset (void);
/* From application */
extern void APP_safeoutput ();
extern volatile _ESCvar ESCvar;
extern _MBX MBX[MBXBUFFERS];
extern _MBXcontrol MBXcontrol[MBXBUFFERS];
extern uint8_t MBX[];
extern _MBXcontrol MBXcontrol[];
extern uint8_t MBXrun;
extern uint16_t SM2_sml, SM3_sml;
extern uint16_t ESC_SM2_sml, ESC_SM3_sml;
typedef struct
{

248
soes/esc_coe.c
View File

@ -18,121 +18,6 @@
#define BITS2BYTES(b) ((b + 7) >> 3)
extern uint8_t txpdoitems;
extern uint8_t rxpdoitems;
/** Search for an object index matching the wanted value in the Object List.
*
* @param[in] index = value on index of object we want to locate
* @return local array index if we succeed, -1 if we didn't find the index.
*/
int32_t SDO_findobject (uint16_t index)
{
int32_t n = 0;
while (SDOobjects[n].index < index)
{
n++;
}
if (SDOobjects[n].index != index)
{
return -1;
}
return n;
}
/** Calculate the size in Bytes of TxPDOs by adding the objects in SyncManager
* SDO 1C13.
*
* @return size of TxPDOs in Bytes.
*/
uint16_t sizeTXPDO (void)
{
uint16_t size = 0, hobj, l, si, c, sic;
int16_t nidx;
const _objd *objd;
if (SDO1C13[0].data)
{
si = *((uint8_t *) SDO1C13[0].data);
}
else
{
si = (uint8_t) SDO1C13[0].value;
}
if (si)
{
for (sic = 1; sic <= si; sic++)
{
if (SDO1C13[sic].data)
{
hobj = *((uint16_t *) SDO1C13[sic].data);
hobj = htoes(hobj);
}
else
{
hobj = (uint16_t) SDO1C13[sic].value;
}
nidx = SDO_findobject (hobj);
if (nidx > 0)
{
objd = SDOobjects[nidx].objdesc;
l = (uint8_t) objd->value;
for (c = 1; c <= l; c++)
{
size += ((objd + c)->value & 0xff);
}
}
}
}
return BITS2BYTES (size);
}
/** Calculate the size in Bytes of RxPDOs by adding the objects in SyncManager
* SDO 1C12.
*
* @return size of RxPDOs in Bytes.
*/
uint16_t sizeRXPDO (void)
{
uint16_t size = 0, hobj, c, l, si, sic;
int16_t nidx;
const _objd *objd;
if (SDO1C12[0].data)
{
si = *((uint8_t *) SDO1C12[0].data);
}
else
{
si = (uint8_t) SDO1C12[0].value;
}if (si)
{
for (sic = 1; sic <= si; sic++)
{
if (SDO1C12[sic].data)
{
hobj = *((uint16_t *) SDO1C12[sic].data);
hobj = htoes(hobj);
}
else
{
hobj = (uint16_t) SDO1C12[sic].value;
}
nidx = SDO_findobject (hobj);
if (nidx > 0)
{
objd = SDOobjects[nidx].objdesc;
l = (uint8_t) objd->value;
for (c = 1; c <= l; c++)
{
size += ((objd + c)->value & 0xff);
}
}
}
}
return BITS2BYTES (size);
}
/** Search for an object sub-index.
*
* @param[in] nidx = local array index of object we want to find sub-index to
@ -157,6 +42,87 @@ int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex)
return n;
}
/** Search for an object index matching the wanted value in the Object List.
*
* @param[in] index = value on index of object we want to locate
* @return local array index if we succeed, -1 if we didn't find the index.
*/
int32_t SDO_findobject (uint16_t index)
{
int32_t n = 0;
while (SDOobjects[n].index < index)
{
n++;
}
if (SDOobjects[n].index != index)
{
return -1;
}
return n;
}
/** Calculate the size in Bytes of RxPDO or TxPDOs by adding the objects
* in SyncManager
* SDO 1C1x.
*
* @return size of RxPDO or TxPDOs in Bytes.
*/
uint16_t sizeOfPDO (uint16_t index)
{
uint16_t size = 0, hobj, l, si, c, sic;
int16_t nidx;
const _objd *objd;
const _objd *objd1c1x;
nidx = SDO_findobject (index);
if(nidx < 0)
{
return size;
}
else if((index != RX_PDO_OBJIDX) && (index != TX_PDO_OBJIDX))
{
return size;
}
objd1c1x = objd = SDOobjects[nidx].objdesc;
if (objd1c1x[0].data)
{
si = *((uint8_t *) objd1c1x[0].data);
}
else
{
si = (uint8_t) objd1c1x[0].value;
}
if (si)
{
for (sic = 1; sic <= si; sic++)
{
if (objd1c1x[sic].data)
{
hobj = *((uint16_t *) objd1c1x[sic].data);
hobj = htoes(hobj);
}
else
{
hobj = (uint16_t) objd1c1x[sic].value;
}
nidx = SDO_findobject (hobj);
if (nidx > 0)
{
objd = SDOobjects[nidx].objdesc;
l = (uint8_t) objd->value;
for (c = 1; c <= l; c++)
{
size += ((objd + c)->value & 0xff);
}
}
}
}
return BITS2BYTES (size);
}
/** Copy to mailbox.
*
* @param[in] source = pointer to source
@ -181,7 +147,7 @@ void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
@ -221,7 +187,7 @@ void SDO_upload (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
@ -269,13 +235,13 @@ void SDO_upload (void)
/* convert bits to bytes */
size = (size + 7) >> 3;
coeres->size = htoel (size);
if ((size + COE_HEADERSIZE) > MBXDSIZE)
if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE)
{
/* segmented transfer needed */
/* set total size in bytes */
ESCvar.frags = size;
/* limit to mailbox size */
size = MBXDSIZE - COE_HEADERSIZE;
size = ESC_MBXDSIZE - COE_HEADERSIZE;
/* number of bytes done */
ESCvar.fragsleft = size;
/* signal segmented transfer */
@ -319,18 +285,18 @@ void SDO_uploadsegment (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
offset = ESCvar.fragsleft;
size = ESCvar.frags - ESCvar.fragsleft;
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
coeres->command = COE_COMMAND_UPLOADSEGMENT + (coesdo->command & COE_TOGGLEBIT); // copy toggle bit
if ((size + COE_SEGMENTHEADERSIZE) > MBXDSIZE)
if ((size + COE_SEGMENTHEADERSIZE) > ESC_MBXDSIZE)
{
/* more segmented transfer needed */
/* limit to mailbox size */
size = MBXDSIZE - COE_SEGMENTHEADERSIZE;
size = ESC_MBXDSIZE - COE_SEGMENTHEADERSIZE;
/* number of bytes done */
ESCvar.fragsleft += size;
coeres->mbxheader.length = htoes (COE_SEGMENTHEADERSIZE + size);
@ -360,26 +326,6 @@ void SDO_uploadsegment (void)
ESCvar.xoe = 0;
}
/** 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 1 if the SDO Download is correct. 0 If not correct.
*/
int ESC_pre_objecthandler (uint16_t index, uint8_t subindex)
{
if ((index == 0x1c12) && (subindex > 0) && (rxpdoitems != 0))
{
SDO_abort (index, subindex, ABORT_READONLY);
return 0;
}
if ((index == 0x1c13) && (subindex > 0) && (txpdoitems != 0))
{
SDO_abort (index, subindex, ABORT_READONLY);
return 0;
}
return 1;
}
/** Function for handling incoming requested SDO Download, validating the
* request and sending an response. On error an SDO Abort will be sent.
@ -429,7 +375,7 @@ void SDO_download (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
@ -485,7 +431,7 @@ void SDO_infoerror (uint32_t abortcode)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEobjdesc *) &MBX[MBXout];
coeres = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
coeres->mbxheader.length = htoes ((uint16_t) 0x0a);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
@ -500,7 +446,7 @@ void SDO_infoerror (uint32_t abortcode)
}
}
#define ODLISTSIZE ((MBX1_sml - MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)
#define ODLISTSIZE ((ESC_MBX1_sml - ESC_MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)
/** Function for handling incoming requested SDO Get OD List, validating the
* request and sending an response. On error an SDO Info Error will be sent.
@ -533,7 +479,7 @@ void SDO_getodlist (void)
}
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
@ -609,7 +555,7 @@ void SDO_getodlistcont (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
@ -663,7 +609,7 @@ void SDO_getod (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
@ -686,7 +632,7 @@ void SDO_getod (void)
coel->objectcode = SDOobjects[nidx].objtype;
s = (uint8_t *) SDOobjects[nidx].name;
d = (uint8_t *) &(coel->name);
while (*s && (n < (MBXDSIZE - 0x0c)))
while (*s && (n < (ESC_MBXDSIZE - 0x0c)))
{
*d = *s;
n++;
@ -734,7 +680,7 @@ void SDO_geted (void)
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEentdesc *) &MBX[MBXout];
coel = (_COEentdesc *) &MBX[MBXout * ESC_MBXSIZE];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
@ -751,7 +697,7 @@ void SDO_geted (void)
coel->access = htoes ((objd + nsub)->access);
s = (uint8_t *) (objd + nsub)->name;
d = (uint8_t *) &(coel->name);
while (*s && (n < (MBXDSIZE - 0x10)))
while (*s && (n < (ESC_MBXDSIZE - 0x10)))
{
*d = *s;
n++;

11
soes/esc_coe.h
View File

@ -77,14 +77,15 @@ typedef struct CC_PACKED
#define ATYPE_RXPDO 0x40
#define ATYPE_TXPDO 0x80
#define TX_PDO_OBJIDX 0x1c13
#define RX_PDO_OBJIDX 0x1c12
void ESC_coeprocess (void);
uint16_t sizeTXPDO (void);
uint16_t sizeRXPDO (void);
uint16_t sizeOfPDO (uint16_t index);
void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode);
extern void ESC_objecthandler (uint16_t index, uint8_t subindex);
extern int ESC_pre_objecthandler (uint16_t index, uint8_t subindex);
extern const _objectlist SDOobjects[];
extern const _objd SDO1C12[];
extern const _objd SDO1C13[];
#endif

18
soes/esc_foe.c
View File

@ -220,8 +220,8 @@ void FOE_abort (uint32_t code)
mbxhandle = ESC_claimbuffer ();
if (mbxhandle)
{
foembx = (_FOE *) & MBX[mbxhandle];
foembx->mbxheader.length = htoes (FOEHSIZE); /* Don't bother with error text for now. */
foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
foembx->mbxheader.length = htoes (ESC_FOEHSIZE); /* Don't bother with error text for now. */
foembx->mbxheader.mbxtype = MBXFOE;
foembx->foeheader.opcode = FOE_OP_ERR;
foembx->foeheader.errorcode = htoel (code);
@ -253,7 +253,7 @@ int FOE_send_data_packet ()
mbxhandle = ESC_claimbuffer ();
if (mbxhandle)
{
foembx = (_FOE *) & MBX[mbxhandle];
foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
data_len = FOE_fread (foembx->data, FOE_DATA_SIZE);
foembx->foeheader.opcode = FOE_OP_DATA;
foembx->foeheader.packetnumber = htoel (FOEvar.foepacket);
@ -284,8 +284,8 @@ int FOE_send_ack ()
if (mbxhandle)
{
DPRINT("FOE_send_ack\n");
foembx = (_FOE *) & MBX[mbxhandle];
foembx->mbxheader.length = htoes (FOEHSIZE);
foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
foembx->mbxheader.length = htoes (ESC_FOEHSIZE);
foembx->mbxheader.mbxtype = MBXFOE;
foembx->foeheader.opcode = FOE_OP_ACK;
foembx->foeheader.packetnumber = htoel (FOEvar.foepacket);
@ -407,7 +407,7 @@ void FOE_write ()
FOE_init ();
foembx = (_FOE *) &MBX[0];
data_len = etohs (foembx->mbxheader.length) - FOEHSIZE;
data_len = etohs (foembx->mbxheader.length) - ESC_FOEHSIZE;
password = etohl (foembx->foeheader.password);
/* Get an address we can write the file to, if possible. */
@ -448,7 +448,7 @@ void FOE_data ()
}
foembx = (_FOE*)&MBX[0];
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
data_len = etohs(foembx->mbxheader.length) - ESC_FOEHSIZE;
packet = etohl(foembx->foeheader.packetnumber);
if (packet != FOEvar.foepacket)
@ -476,7 +476,7 @@ void FOE_data ()
DPRINT("FOE_data no copied\n");
FOE_abort (FOE_ERR_PROGERROR);
}
else if (data_len == FOE_DATA_SIZE)
else if (data_len == ESC_FOE_DATA_SIZE)
{
DPRINT("FOE_data data_len == FOE_DATA_SIZE\n");
if (ncopied != data_len)
@ -579,7 +579,7 @@ void ESC_foeprocess (void)
{
foembx = (_FOE *) &MBX[0];
/* Verify the size of the file data. */
if (etohs (foembx->mbxheader.length) < FOEHSIZE)
if (etohs (foembx->mbxheader.length) < ESC_FOEHSIZE)
{
FOE_abort (MBXERR_SIZETOOSHORT);
}