add support for dynamic pdos

Add support for dynamic pdos, i.e. pdos that can be configured by the
EtherCAT master.
pull/52/head
Hans-Erik Floryd 2019-01-20 15:35:23 +01:00
parent 6fb665a701
commit 5b33b077ca
6 changed files with 495 additions and 138 deletions

View File

@ -11,6 +11,8 @@ add_library (soes
esc_eoe.h
esc_eep.c
esc_eep.h
ecat_slv.c
ecat_slv.h
${HAL_SOURCES}
)

View File

@ -6,8 +6,13 @@
#include "esc_foe.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 */
extern _ESCvar ESCvar;
extern _ESCvar ESCvar;
extern _SMmap SMmap2[];
extern _SMmap SMmap3[];
/* Private variables */
static volatile int watchdog;
@ -16,26 +21,37 @@ static volatile int watchdog;
*
* @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.
* @return SDO abort code, or 0 on success
*/
int ESC_pre_objecthandler (uint16_t index,
uint32_t ESC_pre_objecthandler (uint16_t index,
uint8_t subindex,
void * data,
size_t size,
bool isCA)
{
int result = 1;
int abort = 0;
if(ESCvar.pre_object_download_hook)
if (IS_RXPDO (index) ||
IS_TXPDO (index) ||
index == RX_PDO_OBJIDX ||
index == TX_PDO_OBJIDX)
{
result = (ESCvar.pre_object_download_hook)(index,
if (subindex > 0 && COE_maxSub (index) != 0)
{
abort = ABORT_SUBINDEX0_NOT_ZERO;
}
}
if (ESCvar.pre_object_download_hook)
{
abort = (ESCvar.pre_object_download_hook) (index,
subindex,
data,
size,
isCA);
}
return result;
return abort;
}
/** Mandatory: Hook called from the slave stack SDO Download handler to act on
@ -46,25 +62,9 @@ int ESC_pre_objecthandler (uint16_t index,
*/
void ESC_objecthandler (uint16_t index, uint8_t subindex, bool isCA)
{
switch (index)
if (ESCvar.post_object_download_hook != NULL)
{
default:
{
if ((index >= 0x1600 && index < 0x1800) || index == RX_PDO_OBJIDX)
{
;
}
else if ((index >= 0x1A00 && index < 0x1C00) || index == TX_PDO_OBJIDX)
{
;
}
else if(ESCvar.post_object_download_hook != NULL)
{
(ESCvar.post_object_download_hook)(index, subindex, isCA);
}
break;
}
(ESCvar.post_object_download_hook)(index, subindex, isCA);
}
}
@ -92,7 +92,8 @@ void TXPDO_update (void)
}
else
{
ESC_write (ESC_SM3_sma, ESCvar.txpdosaddress , ESCvar.ESC_SM3_sml);
COE_pdoPack (ESCvar.txpdos_address, ESCvar.sm3mappings, SMmap3);
ESC_write (ESC_SM3_sma, ESCvar.txpdos_address , ESCvar.ESC_SM3_sml);
}
}
@ -106,7 +107,8 @@ void RXPDO_update (void)
}
else
{
ESC_read (ESC_SM2_sma, ESCvar.rxpdosaddress, ESCvar.ESC_SM2_sml);
ESC_read (ESC_SM2_sma, ESCvar.rxpdos_address, ESCvar.ESC_SM2_sml);
COE_pdoUnpack (ESCvar.rxpdos_address, ESCvar.sm2mappings, SMmap2);
}
}
@ -255,9 +257,6 @@ void ecat_slv_init (esc_cfg_t * config)
{
DPRINT ("Slave stack init started\n");
ESCvar.ESC_SM3_sml = sizeOfPDO(TX_PDO_OBJIDX);
ESCvar.ESC_SM2_sml = sizeOfPDO(RX_PDO_OBJIDX);
/* Init watchdog */
watchdog = config->watchdog_cnt;

View File

@ -978,7 +978,7 @@ void ESC_state (void)
{
/* get station address */
ESC_address ();
COE_initDefaultSyncMgrPara ();
COE_initDefaultValues ();
an = ESC_startmbx (ac);
break;
}
@ -1036,8 +1036,24 @@ void ESC_state (void)
case PREOP_TO_SAFEOP:
case SAFEOP_TO_SAFEOP:
{
ESCvar.ESC_SM2_sml = sizeOfPDO (RX_PDO_OBJIDX);
ESCvar.ESC_SM3_sml = sizeOfPDO (TX_PDO_OBJIDX);
ESCvar.ESC_SM2_sml = sizeOfPDO (RX_PDO_OBJIDX, &ESCvar.sm2mappings,
SMmap2, ESCvar.rxpdos_mappings);
if (ESCvar.sm2mappings < 0)
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
break;
}
ESCvar.ESC_SM3_sml = sizeOfPDO (TX_PDO_OBJIDX, &ESCvar.sm3mappings,
SMmap3, ESCvar.txpdos_mappings);
if (ESCvar.sm3mappings < 0)
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDINPUTSM);
break;
}
an = ESC_startinput (ac);
if (an == ac)
{
@ -1118,7 +1134,7 @@ void ESC_state (void)
}
ESC_ALstatus (an);
DPRINT ("state %x\n", an);
}
/** Function copying the application configuration variable
* data to the stack local variable.
@ -1135,8 +1151,10 @@ void ESC_config (esc_cfg_t * cfg)
ESCvar.mbxsizeboot = cfg->mbxsizeboot;
ESCvar.mbxbuffers = cfg->mbxbuffers;
ESCvar.rxpdosaddress = cfg->rxpdosaddress;
ESCvar.txpdosaddress = cfg->txpdosaddress;
ESCvar.rxpdos_address = cfg->rxpdos_address;
ESCvar.rxpdos_mappings = cfg->rxpdos_mappings;
ESCvar.txpdos_address = cfg->txpdos_address;
ESCvar.txpdos_mappings = cfg->txpdos_mappings;
ESCvar.mb[0] = cfg->mb[0];
ESCvar.mb[1] = cfg->mb[1];

View File

@ -12,6 +12,7 @@
#define __esc__
#include <cc.h>
#include <esc_coe.h>
#define ESCREG_ADDRESS 0x0010
#define ESCREG_DLSTATUS 0x0110
@ -124,6 +125,8 @@
#define ABORT_UNSUPPORTED 0x06010000
#define ABORT_WRITEONLY 0x06010001
#define ABORT_READONLY 0x06010002
#define ABORT_SUBINDEX0_NOT_ZERO 0x06010003
#define ABORT_EXCEEDS_MBOX_SIZE 0x06010005
#define ABORT_NOOBJECT 0x06020000
#define ABORT_TYPEMISMATCH 0x06070010
#define ABORT_NOSUBINDEX 0x06090011
@ -237,8 +240,10 @@ typedef struct esc_cfg
size_t mbxsize;
size_t mbxsizeboot;
int mbxbuffers;
void * rxpdosaddress;
void * txpdosaddress;
void * rxpdos_address;
int rxpdos_mappings;
void * txpdos_address;
int txpdos_mappings;
sm_cfg_t mb[2];
sm_cfg_t mb_boot[2];
sm_cfg_t pdosm[2];
@ -357,8 +362,10 @@ typedef struct
size_t mbxsize;
size_t mbxsizeboot;
int mbxbuffers;
void * rxpdosaddress;
void * txpdosaddress;
void * rxpdos_address;
int rxpdos_mappings;
void * txpdos_address;
int txpdos_mappings;
sm_cfg_t mb[2];
sm_cfg_t mbboot[2];
sm_cfg_t pdosm[2];
@ -408,6 +415,8 @@ typedef struct
uint8_t toggle;
int sm2mappings;
int sm3mappings;
uint8_t SMtestresult;
uint32_t PrevTime;
@ -650,6 +659,8 @@ extern void APP_safeoutput ();
extern _ESCvar ESCvar;
extern _MBXcontrol MBXcontrol[];
extern uint8_t MBX[];
extern _SMmap SMmap2[];
extern _SMmap SMmap3[];
/* ATOMIC operations are used when running interrupt driven */
#ifndef CC_ATOMIC_SET

View File

@ -18,6 +18,10 @@
#define BITS2BYTES(b) ((b + 7) >> 3)
/* Fetch value from object dictionary */
#define OBJ_VALUE_FETCH(v, o) \
((o).data ? *(__typeof__ (v) *)(o).data : (__typeof__ (v))(o).value)
/** Search for an object sub-index.
*
* @param[in] nidx = local array index of object we want to find sub-index to
@ -61,117 +65,98 @@ int32_t SDO_findobject (uint16_t index)
return n;
}
/** Init default values for SDO Sync Objects
*
*/
void COE_initDefaultSyncMgrPara (void)
{
uint32_t i,j;
const _objd *objd;
int32_t n = 0;
/* 1C3x */
for(i = 0x1C32; i <= 0x1C33; i ++)
{
/* Look if index is present */
n = SDO_findobject(i);
if(n < 0)
{
continue;
}
/* Load default values */
objd = SDOobjects[n].objdesc;
for(j = 1; j <= SDOobjects[n].maxsub; j++ )
{
if(objd[j].data != NULL)
{
*(uint32_t *)objd[j].data = objd[j].value;
}
if(objd[j].subindex >= SDOobjects[n].maxsub)
{
break;
}
}
}
/* Look if index is present */
n = SDO_findobject(0x10F1);
if(n >= 0)
{
/* Load default values */
objd = SDOobjects[n].objdesc;
for(j = 1; j <= objd[0].value; j++ )
{
if(objd[j].data != NULL)
{
*(uint32_t *)objd[j].data = objd[j].value;
}
}
}
}
/** Calculate the size in Bytes of RxPDO or TxPDOs by adding the objects
* in SyncManager
* SDO 1C1x.
/**
* Calculate the size in Bytes of RxPDO or TxPDOs by adding the
* objects in SyncManager SDO 1C1x.
*
* @param[in] index = SM index
* @param[out] nmappings = number of mapped objects in SM, or -1 if
* mapping is invalid
* @param[out] mappings = list of mapped objects in SM
* @param[out] max_mappings = max number of mapped objects in SM
* @return size of RxPDO or TxPDOs in Bytes.
*/
uint16_t sizeOfPDO (uint16_t index)
uint16_t sizeOfPDO (uint16_t index, int * nmappings,_SMmap * mappings,
int max_mappings)
{
uint16_t size = 0, hobj, l, si, c, sic;
uint16_t offset = 0, hobj;
uint8_t si, sic, c;
int16_t nidx;
const _objd *objd;
const _objd *objd1c1x;
int mapIx = 0;
if ((index != RX_PDO_OBJIDX) && (index != TX_PDO_OBJIDX))
{
return 0;
}
nidx = SDO_findobject (index);
if(nidx < 0)
{
return size;
}
else if((index != RX_PDO_OBJIDX) && (index != TX_PDO_OBJIDX))
{
return size;
return 0;
}
objd1c1x = objd = SDOobjects[nidx].objdesc;
objd1c1x = SDOobjects[nidx].objdesc;
if (objd1c1x[0].data)
{
si = *((uint8_t *) objd1c1x[0].data);
}
else
{
si = (uint8_t) objd1c1x[0].value;
}
si = OBJ_VALUE_FETCH (si, objd1c1x[0]);
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;
}
hobj = OBJ_VALUE_FETCH (hobj, objd1c1x[sic]);
nidx = SDO_findobject (hobj);
if (nidx > 0)
if (nidx >= 0)
{
uint8_t maxsub;
objd = SDOobjects[nidx].objdesc;
l = (uint8_t) objd->value;
for (c = 1; c <= l; c++)
maxsub = OBJ_VALUE_FETCH (maxsub, objd[0]);
for (c = 1; c <= maxsub; c++)
{
size += ((objd + c)->value & 0xff);
uint32_t value = OBJ_VALUE_FETCH (value, objd[c]);
uint16_t index = value >> 16;
uint8_t subindex = (value >> 8) & 0xFF;
uint8_t bitlength = value & 0xFF;
const _objd * mapping;
if (mapIx == max_mappings)
{
/* Too many mapped objects */
*nmappings = -1;
return 0;
}
DPRINT ("%04x:%02x @ %d\n", index, subindex, offset);
nidx = SDO_findobject (index);
if (nidx >= 0)
{
int16_t nsub;
nsub = SDO_findsubindex (nidx, subindex);
if (nsub < 0)
{
mapping = NULL;
}
mapping = &SDOobjects[nidx].objdesc[nsub];
}
else
{
mapping = NULL;
}
mappings[mapIx].obj = mapping;
mappings[mapIx++].offset = offset;
offset += bitlength;
}
}
}
}
return BITS2BYTES (size);
*nmappings = mapIx;
return BITS2BYTES (offset);
}
/** Copy to mailbox.
@ -391,6 +376,8 @@ void SDO_download (void)
uint16_t size, actsize;
const _objd *objd;
uint32_t *mbxdata;
uint32_t abort;
coesdo = (_COEsdo *) &MBX[0];
index = etohs (coesdo->index);
subindex = coesdo->subindex;
@ -421,11 +408,14 @@ void SDO_download (void)
actsize = ((objd + nsub)->bitlength + 7) >> 3;
if (actsize == size)
{
if (ESC_pre_objecthandler (index,
subindex,
mbxdata,
size,
false))
abort = ESC_pre_objecthandler (
index,
subindex,
mbxdata,
size,
false
);
if (abort == 0)
{
copy2mbx (mbxdata, (objd + nsub)->data, size);
MBXout = ESC_claimbuffer ();
@ -442,8 +432,12 @@ void SDO_download (void)
coeres->size = htoel (0);
MBXcontrol[MBXout].state = MBXstate_outreq;
}
/* external object write handler */
ESC_objecthandler (index, subindex, false);
/* external object write handler */
ESC_objecthandler (index, subindex, false);
}
else
{
SDO_abort (index, subindex, abort);
}
}
else
@ -897,3 +891,318 @@ void ESC_coeprocess (void)
}
}
}
/**
* Get value from bitmap
*
* This function gets a value from a bitmap.
*
* @param[in] bitmap = bitmap containing value
* @param[in] offset = start offset
* @param[in] length = number of bits to get
* @return bitslice value
*/
static uint64_t COE_bitsliceGet (uint64_t * bitmap, int offset, int length)
{
const int word_offset = offset / 64;
const int bit_offset = offset % 64;
const uint64_t mask = (length == 64) ? UINT64_MAX : (1ULL << length) - 1;
uint64_t w0;
uint64_t w1 = 0;
/* Get the least significant word */
w0 = bitmap[word_offset];
w0 = w0 >> bit_offset;
/* Get the most significant word, if required */
if (length + bit_offset > 64)
{
w1 = bitmap[word_offset + 1];
w1 = w1 << (64 - bit_offset);
}
w0 = (w1 | w0);
return (w0 & mask);
}
/**
* Set value in bitmap
*
* This function sets a value in a bitmap.
*
* @param[in] bitmap = bitmap to contain value
* @param[in] offset = start offset
* @param[in] length = number of bits to set
* @param[in] value = value to set
*/
static void COE_bitsliceSet (uint64_t * bitmap, int offset, int length,
uint64_t value)
{
const int word_offset = offset / 64;
const int bit_offset = offset % 64;
const uint64_t mask = (length == 64) ? UINT64_MAX : (1ULL << length) - 1;
const uint64_t mask0 = mask << bit_offset;
uint64_t v0 = value << bit_offset;
uint64_t w0 = bitmap[word_offset];
/* Set the least significant word */
w0 = (w0 & ~mask0) | (v0 & mask0);
bitmap[word_offset] = w0;
/* Set the most significant word, if required */
if (length + bit_offset > 64)
{
const uint64_t mask1 = mask >> (64 - bit_offset);
uint64_t v1 = value >> (64 - bit_offset);
uint64_t w1 = bitmap[word_offset + 1];
w1 = (w1 & ~mask1) | (v1 & mask1);
bitmap[word_offset + 1] = w1;
}
}
/**
* Get object value
*
* This function atomically gets an object value.
*
* @param[in] obj = object description
* @return object value
*/
static uint64_t COE_getValue (const _objd * obj)
{
uint64_t value = 0;
/* TODO: const data */
switch(obj->datatype)
{
case DTYPE_BIT1:
case DTYPE_BIT2:
case DTYPE_BIT3:
case DTYPE_BIT4:
case DTYPE_BIT5:
case DTYPE_BIT6:
case DTYPE_BIT7:
case DTYPE_BIT8:
case DTYPE_BOOLEAN:
case DTYPE_UNSIGNED8:
case DTYPE_INTEGER8:
value = *(uint8_t *)obj->data;
break;
case DTYPE_UNSIGNED16:
case DTYPE_INTEGER16:
value = *(uint16_t *)obj->data;
break;
case DTYPE_REAL32:
case DTYPE_UNSIGNED32:
case DTYPE_INTEGER32:
value = *(uint32_t *)obj->data;
break;
case DTYPE_REAL64:
case DTYPE_UNSIGNED64:
case DTYPE_INTEGER64:
/* FIXME: must be atomic */
value = *(uint64_t *)obj->data;
break;
default:
CC_ASSERT (0);
}
return value;
}
/**
* Set object value
*
* This function atomically sets an object value.
*
* @param[in] obj = object description
* @param[in] value = new value
*/
static void COE_setValue (const _objd * obj, uint64_t value)
{
switch(obj->datatype)
{
case DTYPE_BIT1:
case DTYPE_BIT2:
case DTYPE_BIT3:
case DTYPE_BIT4:
case DTYPE_BIT5:
case DTYPE_BIT6:
case DTYPE_BIT7:
case DTYPE_BIT8:
case DTYPE_BOOLEAN:
case DTYPE_UNSIGNED8:
case DTYPE_INTEGER8:
*(uint8_t *)obj->data = value & UINT8_MAX;
break;
case DTYPE_UNSIGNED16:
case DTYPE_INTEGER16:
*(uint16_t *)obj->data = value & UINT16_MAX;
break;
case DTYPE_REAL32:
case DTYPE_UNSIGNED32:
case DTYPE_INTEGER32:
*(uint32_t *)obj->data = value & UINT32_MAX;
break;
case DTYPE_REAL64:
case DTYPE_UNSIGNED64:
case DTYPE_INTEGER64:
/* FIXME: must be atomic */
*(uint64_t *)obj->data = value;
break;
default:
DPRINT ("ignored\n");
break;
}
}
/**
* Init default values for SDO objects
*/
void COE_initDefaultValues (void)
{
int i;
const _objd *objd;
int n;
uint8_t maxsub;
for (n = 0; SDOobjects[n].index != 0xffff; n++)
{
objd = SDOobjects[n].objdesc;
maxsub = SDOobjects[n].maxsub;
i = 0;
do
{
if (objd[i].data != NULL)
{
/* TODO: bitlength > 64 */
COE_setValue (&objd[i], objd[i].value);
DPRINT ("%04x:%02x = %x\n", SDOobjects[n].index, objd[i].subindex, objd[i].value);
}
} while (objd[i++].subindex < maxsub);
}
}
/**
* Pack process data
*
* This function reads mapped objects and constructs the process data
* inputs (TXPDO).
*
* @param[in] buffer = input process data
* @param[in] nmappings = number of mappings in sync manager
* @param[in] mappings = list of mapped objects in sync manager
*/
void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * mappings)
{
int ix;
/* Check that buffer is aligned on 64-bit boundary */
CC_ASSERT (((uintptr_t)buffer & 0x07) == 0);
for (ix = 0; ix < nmappings; ix++)
{
const _objd * obj = mappings[ix].obj;
uint16_t offset = mappings[ix].offset;
if (obj != NULL)
{
if (obj->bitlength > 64)
{
memcpy (
&buffer[BITS2BYTES (offset)],
obj->data,
BITS2BYTES (obj->bitlength)
);
}
else
{
/* Atomically get object value */
uint64_t value = COE_getValue (obj);
COE_bitsliceSet (
(uint64_t *)buffer,
offset,
obj->bitlength,
value
);
}
}
}
}
/**
* Unpack process data
*
* This function unpacks process data output (RXPDO) and writes to the
* mapped objects.
*
* @param[in] buffer = output process data
* @param[in] nmappings = number of mappings in sync manager
* @param[in] mappings = list of mapped objects in sync manager
*/
void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * mappings)
{
int ix;
/* Check that buffer is aligned on 64-bit boundary */
CC_ASSERT (((uintptr_t)buffer & 0x07) == 0);
for (ix = 0; ix < nmappings; ix++)
{
const _objd * obj = mappings[ix].obj;
uint16_t offset = mappings[ix].offset;
if (obj != NULL)
{
if (obj->bitlength > 64)
{
memcpy (
obj->data,
&buffer[BITS2BYTES (offset)],
BITS2BYTES (obj->bitlength)
);
}
else
{
/* Atomically set object value */
uint64_t value = COE_bitsliceGet (
(uint64_t *)buffer,
offset,
obj->bitlength
);
COE_setValue (obj, value);
}
}
}
}
/**
* Fetch max subindex
*
* This function fetches the value of subindex 0 (max subindex).
*
* @param[in] index = object index
*/
uint8_t COE_maxSub (uint16_t index)
{
int nidx;
uint8_t maxsub;
nidx = SDO_findobject (index);
if (nidx == -1)
return 0;
maxsub = OBJ_VALUE_FETCH (maxsub, SDOobjects[nidx].objdesc[0]);
return maxsub;
}

View File

@ -38,6 +38,12 @@ typedef struct CC_PACKED
} _objectlist;
CC_PACKED_END
typedef struct
{
const _objd * obj;
uint16_t offset;
} _SMmap;
#define OBJH_READ 0
#define OBJH_WRITE 1
@ -75,21 +81,33 @@ CC_PACKED_END
#define DTYPE_BIT7 0x0036
#define DTYPE_BIT8 0x0037
#define ATYPE_RO 0x07
#define ATYPE_RW 0x3F
#define ATYPE_RWpre 0x0F
#define ATYPE_Rpre 0x01
#define ATYPE_Rsafe 0x02
#define ATYPE_Rop 0x04
#define ATYPE_Wpre 0x08
#define ATYPE_Wsafe 0x10
#define ATYPE_Wop 0x20
#define ATYPE_RXPDO 0x40
#define ATYPE_TXPDO 0x80
#define ATYPE_RO (ATYPE_Rpre | ATYPE_Rsafe | ATYPE_Rop)
#define ATYPE_RW (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop | ATYPE_RO)
#define ATYPE_RWpre (ATYPE_Wpre | ATYPE_RO)
#define TX_PDO_OBJIDX 0x1c13
#define RX_PDO_OBJIDX 0x1c12
void ESC_coeprocess (void);
uint16_t sizeOfPDO (uint16_t index);
uint16_t sizeOfPDO (uint16_t index, int * nmappings, _SMmap * sm, int max_mappings);
void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode);
void COE_initDefaultSyncMgrPara (void);
void COE_initDefaultValues (void);
void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm);
void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * sm);
uint8_t COE_maxSub (uint16_t index);
extern void ESC_objecthandler (uint16_t index, uint8_t subindex, bool isCA);
extern int ESC_pre_objecthandler (uint16_t index,
extern uint32_t ESC_pre_objecthandler (uint16_t index,
uint8_t subindex,
void * data,
size_t size,