1742 lines
61 KiB
C
1742 lines
61 KiB
C
/*
|
|
* Licensed under the GNU General Public License version 2 with exceptions. See
|
|
* LICENSE file in the project root for full license information
|
|
*/
|
|
|
|
/** \file
|
|
* \brief
|
|
* Configuration module for EtherCAT master.
|
|
*
|
|
* After successful initialisation with ec_init() or ec_init_redundant()
|
|
* the slaves can be auto configured with this module.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include "osal.h"
|
|
#include "oshw.h"
|
|
#include "ethercattype.h"
|
|
#include "ethercatbase.h"
|
|
#include "ethercatmain.h"
|
|
#include "ethercatcoe.h"
|
|
#include "ethercatsoe.h"
|
|
#include "ethercatconfig.h"
|
|
|
|
|
|
typedef struct
|
|
{
|
|
int thread_n;
|
|
int running;
|
|
ecx_contextt *context;
|
|
uint16 slave;
|
|
} ecx_mapt_t;
|
|
|
|
ecx_mapt_t ecx_mapt[EC_MAX_MAPT];
|
|
#if EC_MAX_MAPT > 1
|
|
OSAL_THREAD_HANDLE ecx_threadh[EC_MAX_MAPT];
|
|
#endif
|
|
|
|
#ifdef EC_VER1
|
|
/** Slave configuration structure */
|
|
typedef const struct
|
|
{
|
|
/** Manufacturer code of slave */
|
|
uint32 man;
|
|
/** ID of slave */
|
|
uint32 id;
|
|
/** Readable name */
|
|
char name[EC_MAXNAME + 1];
|
|
/** Data type */
|
|
uint8 Dtype;
|
|
/** Input bits */
|
|
uint16 Ibits;
|
|
/** Output bits */
|
|
uint16 Obits;
|
|
/** SyncManager 2 address */
|
|
uint16 SM2a;
|
|
/** SyncManager 2 flags */
|
|
uint32 SM2f;
|
|
/** SyncManager 3 address */
|
|
uint16 SM3a;
|
|
/** SyncManager 3 flags */
|
|
uint32 SM3f;
|
|
/** FMMU 0 activation */
|
|
uint8 FM0ac;
|
|
/** FMMU 1 activation */
|
|
uint8 FM1ac;
|
|
} ec_configlist_t;
|
|
|
|
#include "ethercatconfiglist.h"
|
|
#endif
|
|
|
|
/** standard SM0 flags configuration for mailbox slaves */
|
|
#define EC_DEFAULTMBXSM0 0x00010026
|
|
/** standard SM1 flags configuration for mailbox slaves */
|
|
#define EC_DEFAULTMBXSM1 0x00010022
|
|
/** standard SM0 flags configuration for digital output slaves */
|
|
#define EC_DEFAULTDOSM0 0x00010044
|
|
|
|
#ifdef EC_VER1
|
|
/** Find slave in standard configuration list ec_configlist[]
|
|
*
|
|
* @param[in] man = manufacturer
|
|
* @param[in] id = ID
|
|
* @return index in ec_configlist[] when found, otherwise 0
|
|
*/
|
|
int ec_findconfig( uint32 man, uint32 id)
|
|
{
|
|
int i = 0;
|
|
|
|
do
|
|
{
|
|
i++;
|
|
} while ( (ec_configlist[i].man != EC_CONFIGEND) &&
|
|
((ec_configlist[i].man != man) || (ec_configlist[i].id != id)) );
|
|
if (ec_configlist[i].man == EC_CONFIGEND)
|
|
{
|
|
i = 0;
|
|
}
|
|
return i;
|
|
}
|
|
#endif
|
|
|
|
void ecx_init_context(ecx_contextt *context)
|
|
{
|
|
int lp;
|
|
*(context->slavecount) = 0;
|
|
/* clean ec_slave array */
|
|
memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave);
|
|
memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup);
|
|
/* clear slave eeprom cache, does not actually read any eeprom */
|
|
ecx_siigetbyte(context, 0, EC_MAXEEPBUF);
|
|
for(lp = 0; lp < context->maxgroup; lp++)
|
|
{
|
|
/* default start address per group entry */
|
|
context->grouplist[lp].logstartaddr = lp << EC_LOGGROUPOFFSET;
|
|
}
|
|
}
|
|
|
|
int ecx_detect_slaves(ecx_contextt *context)
|
|
{
|
|
uint8 b;
|
|
uint16 w;
|
|
int wkc;
|
|
|
|
/* make special pre-init register writes to enable MAC[1] local administered bit *
|
|
* setting for old netX100 slaves */
|
|
b = 0x00;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */
|
|
b = EC_STATE_INIT | EC_STATE_ACK;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
|
|
/* netX100 should now be happy */
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
|
|
wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
|
|
if (wkc > 0)
|
|
{
|
|
/* this is strictly "less than" since the master is "slave 0" */
|
|
if (wkc < EC_MAXSLAVE)
|
|
{
|
|
*(context->slavecount) = wkc;
|
|
}
|
|
else
|
|
{
|
|
EC_PRINT("Error: too many slaves on network: num_slaves=%d, EC_MAXSLAVE=%d\n",
|
|
wkc, EC_MAXSLAVE);
|
|
return EC_SLAVECOUNTEXCEEDED;
|
|
}
|
|
}
|
|
return wkc;
|
|
}
|
|
|
|
static void ecx_set_slaves_to_default(ecx_contextt *context)
|
|
{
|
|
uint8 b;
|
|
uint16 w;
|
|
uint8 zbuf[64];
|
|
memset(&zbuf, 0x00, sizeof(zbuf));
|
|
b = 0x00;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DLPORT , sizeof(b) , &b, EC_TIMEOUTRET3); /* deact loop manual */
|
|
w = htoes(0x0004);
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_IRQMASK , sizeof(w) , &w, EC_TIMEOUTRET3); /* set IRQ mask */
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_RXERR , 8 , &zbuf, EC_TIMEOUTRET3); /* reset CRC counters */
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_FMMU0 , 16 * 3 , &zbuf, EC_TIMEOUTRET3); /* reset FMMU's */
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_SM0 , 8 * 4 , &zbuf, EC_TIMEOUTRET3); /* reset SyncM */
|
|
b = 0x00;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DCSYNCACT , sizeof(b) , &b, EC_TIMEOUTRET3); /* reset activation register */
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DCSYSTIME , 4 , &zbuf, EC_TIMEOUTRET3); /* reset system time+ofs */
|
|
w = htoes(0x1000);
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DCSPEEDCNT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC speedstart */
|
|
w = htoes(0x0c00);
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DCTIMEFILT , sizeof(w) , &w, EC_TIMEOUTRET3); /* DC filt expr */
|
|
b = 0x00;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS , sizeof(b) , &b, EC_TIMEOUTRET3); /* Ignore Alias register */
|
|
b = EC_STATE_INIT | EC_STATE_ACK;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL , sizeof(b) , &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
|
|
b = 2;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* force Eeprom from PDI */
|
|
b = 0;
|
|
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG , sizeof(b) , &b, EC_TIMEOUTRET3); /* set Eeprom to master */
|
|
}
|
|
|
|
#ifdef EC_VER1
|
|
static int ecx_config_from_table(ecx_contextt *context, uint16 slave)
|
|
{
|
|
int cindex;
|
|
ec_slavet *csl;
|
|
|
|
csl = &(context->slavelist[slave]);
|
|
cindex = ec_findconfig( csl->eep_man, csl->eep_id );
|
|
csl->configindex= cindex;
|
|
/* slave found in configuration table ? */
|
|
if (cindex)
|
|
{
|
|
csl->Dtype = ec_configlist[cindex].Dtype;
|
|
strcpy(csl->name ,ec_configlist[cindex].name);
|
|
csl->Ibits = ec_configlist[cindex].Ibits;
|
|
csl->Obits = ec_configlist[cindex].Obits;
|
|
if (csl->Obits)
|
|
{
|
|
csl->FMMU0func = 1;
|
|
}
|
|
if (csl->Ibits)
|
|
{
|
|
csl->FMMU1func = 2;
|
|
}
|
|
csl->FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
|
|
csl->FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
|
|
csl->SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
|
|
csl->SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
|
|
/* simple (no mailbox) output slave found ? */
|
|
if (csl->Obits && !csl->SM[2].StartAddr)
|
|
{
|
|
csl->SM[0].StartAddr = htoes(0x0f00);
|
|
csl->SM[0].SMlength = htoes((csl->Obits + 7) / 8);
|
|
csl->SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
|
|
csl->FMMU[0].FMMUactive = 1;
|
|
csl->FMMU[0].FMMUtype = 2;
|
|
csl->SMtype[0] = 3;
|
|
}
|
|
/* complex output slave */
|
|
else
|
|
{
|
|
csl->SM[2].SMlength = htoes((csl->Obits + 7) / 8);
|
|
csl->SMtype[2] = 3;
|
|
}
|
|
csl->SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
|
|
csl->SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
|
|
/* simple (no mailbox) input slave found ? */
|
|
if (csl->Ibits && !csl->SM[3].StartAddr)
|
|
{
|
|
csl->SM[1].StartAddr = htoes(0x1000);
|
|
csl->SM[1].SMlength = htoes((csl->Ibits + 7) / 8);
|
|
csl->SM[1].SMflags = htoel(0x00000000);
|
|
csl->FMMU[1].FMMUactive = 1;
|
|
csl->FMMU[1].FMMUtype = 1;
|
|
csl->SMtype[1] = 4;
|
|
}
|
|
/* complex input slave */
|
|
else
|
|
{
|
|
csl->SM[3].SMlength = htoes((csl->Ibits + 7) / 8);
|
|
csl->SMtype[3] = 4;
|
|
}
|
|
}
|
|
return cindex;
|
|
}
|
|
#else
|
|
static int ecx_config_from_table(ecx_contextt *context, uint16 slave)
|
|
{
|
|
(void)context;
|
|
(void)slave;
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
/* If slave has SII and same slave ID done before, use previous data.
|
|
* This is safe because SII is constant for same slave ID.
|
|
*/
|
|
static int ecx_lookup_prev_sii(ecx_contextt *context, uint16 slave)
|
|
{
|
|
int i, nSM;
|
|
if ((slave > 1) && (*(context->slavecount) > 0))
|
|
{
|
|
i = 1;
|
|
while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) ||
|
|
(context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) ||
|
|
(context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) &&
|
|
(i < slave))
|
|
{
|
|
i++;
|
|
}
|
|
if(i < slave)
|
|
{
|
|
context->slavelist[slave].CoEdetails = context->slavelist[i].CoEdetails;
|
|
context->slavelist[slave].FoEdetails = context->slavelist[i].FoEdetails;
|
|
context->slavelist[slave].EoEdetails = context->slavelist[i].EoEdetails;
|
|
context->slavelist[slave].SoEdetails = context->slavelist[i].SoEdetails;
|
|
if(context->slavelist[i].blockLRW > 0)
|
|
{
|
|
context->slavelist[slave].blockLRW = 1;
|
|
context->slavelist[0].blockLRW++;
|
|
}
|
|
context->slavelist[slave].Ebuscurrent = context->slavelist[i].Ebuscurrent;
|
|
context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
|
|
memcpy(context->slavelist[slave].name, context->slavelist[i].name, EC_MAXNAME + 1);
|
|
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
context->slavelist[slave].SM[nSM].StartAddr = context->slavelist[i].SM[nSM].StartAddr;
|
|
context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength;
|
|
context->slavelist[slave].SM[nSM].SMflags = context->slavelist[i].SM[nSM].SMflags;
|
|
}
|
|
context->slavelist[slave].FMMU0func = context->slavelist[i].FMMU0func;
|
|
context->slavelist[slave].FMMU1func = context->slavelist[i].FMMU1func;
|
|
context->slavelist[slave].FMMU2func = context->slavelist[i].FMMU2func;
|
|
context->slavelist[slave].FMMU3func = context->slavelist[i].FMMU3func;
|
|
EC_PRINT("Copy SII slave %d from %d.\n", slave, i);
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/** Enumerate and init all slaves.
|
|
*
|
|
* @param[in] context = context struct
|
|
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
|
* @return Workcounter of slave discover datagram = number of slaves found
|
|
*/
|
|
int ecx_config_init(ecx_contextt *context, uint8 usetable)
|
|
{
|
|
uint16 slave, ADPh, configadr, ssigen;
|
|
uint16 topology, estat;
|
|
int16 topoc, slavec, aliasadr;
|
|
uint8 b,h;
|
|
uint8 SMc;
|
|
uint32 eedat;
|
|
int wkc, cindex, nSM;
|
|
uint16 val16;
|
|
|
|
EC_PRINT("ec_config_init %d\n",usetable);
|
|
ecx_init_context(context);
|
|
wkc = ecx_detect_slaves(context);
|
|
if (wkc > 0)
|
|
{
|
|
ecx_set_slaves_to_default(context);
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
ADPh = (uint16)(1 - slave);
|
|
val16 = ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3); /* read interface type of slave */
|
|
context->slavelist[slave].Itype = etohs(val16);
|
|
/* a node offset is used to improve readability of network frames */
|
|
/* this has no impact on the number of addressable slaves (auto wrap around) */
|
|
ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET3); /* set node address of slave */
|
|
if (slave == 1)
|
|
{
|
|
b = 1; /* kill non ecat frames for first slave */
|
|
}
|
|
else
|
|
{
|
|
b = 0; /* pass all frames for following slaves */
|
|
}
|
|
ecx_APWRw(context->port, ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET3); /* set non ecat frame behaviour */
|
|
configadr = ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3);
|
|
configadr = etohs(configadr);
|
|
context->slavelist[slave].configadr = configadr;
|
|
ecx_FPRD(context->port, configadr, ECT_REG_ALIAS, sizeof(aliasadr), &aliasadr, EC_TIMEOUTRET3);
|
|
context->slavelist[slave].aliasadr = etohs(aliasadr);
|
|
ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3);
|
|
estat = etohs(estat);
|
|
if (estat & EC_ESTAT_R64) /* check if slave can read 8 byte chunks */
|
|
{
|
|
context->slavelist[slave].eep_8byte = 1;
|
|
}
|
|
ecx_readeeprom1(context, slave, ECT_SII_MANUF); /* Manuf */
|
|
}
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* Manuf */
|
|
context->slavelist[slave].eep_man = etohl(eedat);
|
|
ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */
|
|
}
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* ID */
|
|
context->slavelist[slave].eep_id = etohl(eedat);
|
|
ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */
|
|
}
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* revision */
|
|
context->slavelist[slave].eep_rev = etohl(eedat);
|
|
ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
|
|
}
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* write mailbox address and mailboxsize */
|
|
context->slavelist[slave].mbx_wo = (uint16)LO_WORD(etohl(eedat));
|
|
context->slavelist[slave].mbx_l = (uint16)HI_WORD(etohl(eedat));
|
|
if (context->slavelist[slave].mbx_l > 0)
|
|
{
|
|
ecx_readeeprom1(context, slave, ECT_SII_TXMBXADR); /* read mailbox offset */
|
|
}
|
|
}
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
if (context->slavelist[slave].mbx_l > 0)
|
|
{
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP); /* read mailbox offset */
|
|
context->slavelist[slave].mbx_ro = (uint16)LO_WORD(etohl(eedat)); /* read mailbox offset */
|
|
context->slavelist[slave].mbx_rl = (uint16)HI_WORD(etohl(eedat)); /*read mailbox length */
|
|
if (context->slavelist[slave].mbx_rl == 0)
|
|
{
|
|
context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l;
|
|
}
|
|
ecx_readeeprom1(context, slave, ECT_SII_MBXPROTO);
|
|
}
|
|
configadr = context->slavelist[slave].configadr;
|
|
val16 = ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3);
|
|
if ((etohs(val16) & 0x04) > 0) /* Support DC? */
|
|
{
|
|
context->slavelist[slave].hasdc = TRUE;
|
|
}
|
|
else
|
|
{
|
|
context->slavelist[slave].hasdc = FALSE;
|
|
}
|
|
topology = ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3); /* extract topology from DL status */
|
|
topology = etohs(topology);
|
|
h = 0;
|
|
b = 0;
|
|
if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */
|
|
{
|
|
h++;
|
|
b |= 0x01;
|
|
}
|
|
if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */
|
|
{
|
|
h++;
|
|
b |= 0x02;
|
|
}
|
|
if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */
|
|
{
|
|
h++;
|
|
b |= 0x04;
|
|
}
|
|
if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */
|
|
{
|
|
h++;
|
|
b |= 0x08;
|
|
}
|
|
/* ptype = Physical type*/
|
|
val16 = ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3);
|
|
context->slavelist[slave].ptype = LO_BYTE(etohs(val16));
|
|
context->slavelist[slave].topology = h;
|
|
context->slavelist[slave].activeports = b;
|
|
/* 0=no links, not possible */
|
|
/* 1=1 link , end of line */
|
|
/* 2=2 links , one before and one after */
|
|
/* 3=3 links , split point */
|
|
/* 4=4 links , cross point */
|
|
/* search for parent */
|
|
context->slavelist[slave].parent = 0; /* parent is master */
|
|
if (slave > 1)
|
|
{
|
|
topoc = 0;
|
|
slavec = slave - 1;
|
|
do
|
|
{
|
|
topology = context->slavelist[slavec].topology;
|
|
if (topology == 1)
|
|
{
|
|
topoc--; /* endpoint found */
|
|
}
|
|
if (topology == 3)
|
|
{
|
|
topoc++; /* split found */
|
|
}
|
|
if (topology == 4)
|
|
{
|
|
topoc += 2; /* cross found */
|
|
}
|
|
if (((topoc >= 0) && (topology > 1)) ||
|
|
(slavec == 1)) /* parent found */
|
|
{
|
|
context->slavelist[slave].parent = slavec;
|
|
slavec = 1;
|
|
}
|
|
slavec--;
|
|
}
|
|
while (slavec > 0);
|
|
}
|
|
(void)ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */
|
|
|
|
/* set default mailbox configuration if slave has mailbox */
|
|
if (context->slavelist[slave].mbx_l>0)
|
|
{
|
|
context->slavelist[slave].SMtype[0] = 1;
|
|
context->slavelist[slave].SMtype[1] = 2;
|
|
context->slavelist[slave].SMtype[2] = 3;
|
|
context->slavelist[slave].SMtype[3] = 4;
|
|
context->slavelist[slave].SM[0].StartAddr = htoes(context->slavelist[slave].mbx_wo);
|
|
context->slavelist[slave].SM[0].SMlength = htoes(context->slavelist[slave].mbx_l);
|
|
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
|
|
context->slavelist[slave].SM[1].StartAddr = htoes(context->slavelist[slave].mbx_ro);
|
|
context->slavelist[slave].SM[1].SMlength = htoes(context->slavelist[slave].mbx_rl);
|
|
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
|
|
eedat = ecx_readeeprom2(context, slave, EC_TIMEOUTEEP);
|
|
context->slavelist[slave].mbx_proto = (uint16)etohl(eedat);
|
|
}
|
|
cindex = 0;
|
|
/* use configuration table ? */
|
|
if (usetable == 1)
|
|
{
|
|
cindex = ecx_config_from_table(context, slave);
|
|
}
|
|
/* slave not in configuration table, find out via SII */
|
|
if (!cindex && !ecx_lookup_prev_sii(context, slave))
|
|
{
|
|
ssigen = ecx_siifind(context, slave, ECT_SII_GENERAL);
|
|
/* SII general section */
|
|
if (ssigen)
|
|
{
|
|
context->slavelist[slave].CoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x07);
|
|
context->slavelist[slave].FoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x08);
|
|
context->slavelist[slave].EoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x09);
|
|
context->slavelist[slave].SoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x0a);
|
|
if((ecx_siigetbyte(context, slave, ssigen + 0x0d) & 0x02) > 0)
|
|
{
|
|
context->slavelist[slave].blockLRW = 1;
|
|
context->slavelist[0].blockLRW++;
|
|
}
|
|
context->slavelist[slave].Ebuscurrent = ecx_siigetbyte(context, slave, ssigen + 0x0e);
|
|
context->slavelist[slave].Ebuscurrent += ecx_siigetbyte(context, slave, ssigen + 0x0f) << 8;
|
|
context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
|
|
}
|
|
/* SII strings section */
|
|
if (ecx_siifind(context, slave, ECT_SII_STRING) > 0)
|
|
{
|
|
ecx_siistring(context, context->slavelist[slave].name, slave, 1);
|
|
}
|
|
/* no name for slave found, use constructed name */
|
|
else
|
|
{
|
|
sprintf(context->slavelist[slave].name, "? M:%8.8x I:%8.8x",
|
|
(unsigned int)context->slavelist[slave].eep_man,
|
|
(unsigned int)context->slavelist[slave].eep_id);
|
|
}
|
|
/* SII SM section */
|
|
nSM = ecx_siiSM(context, slave, context->eepSM);
|
|
if (nSM>0)
|
|
{
|
|
context->slavelist[slave].SM[0].StartAddr = htoes(context->eepSM->PhStart);
|
|
context->slavelist[slave].SM[0].SMlength = htoes(context->eepSM->Plength);
|
|
context->slavelist[slave].SM[0].SMflags =
|
|
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
|
|
SMc = 1;
|
|
while ((SMc < EC_MAXSM) && ecx_siiSMnext(context, slave, context->eepSM, SMc))
|
|
{
|
|
context->slavelist[slave].SM[SMc].StartAddr = htoes(context->eepSM->PhStart);
|
|
context->slavelist[slave].SM[SMc].SMlength = htoes(context->eepSM->Plength);
|
|
context->slavelist[slave].SM[SMc].SMflags =
|
|
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
|
|
SMc++;
|
|
}
|
|
}
|
|
/* SII FMMU section */
|
|
if (ecx_siiFMMU(context, slave, context->eepFMMU))
|
|
{
|
|
if (context->eepFMMU->FMMU0 !=0xff)
|
|
{
|
|
context->slavelist[slave].FMMU0func = context->eepFMMU->FMMU0;
|
|
}
|
|
if (context->eepFMMU->FMMU1 !=0xff)
|
|
{
|
|
context->slavelist[slave].FMMU1func = context->eepFMMU->FMMU1;
|
|
}
|
|
if (context->eepFMMU->FMMU2 !=0xff)
|
|
{
|
|
context->slavelist[slave].FMMU2func = context->eepFMMU->FMMU2;
|
|
}
|
|
if (context->eepFMMU->FMMU3 !=0xff)
|
|
{
|
|
context->slavelist[slave].FMMU3func = context->eepFMMU->FMMU3;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (context->slavelist[slave].mbx_l > 0)
|
|
{
|
|
if (context->slavelist[slave].SM[0].StartAddr == 0x0000) /* should never happen */
|
|
{
|
|
EC_PRINT("Slave %d has no proper mailbox in configuration, try default.\n", slave);
|
|
context->slavelist[slave].SM[0].StartAddr = htoes(0x1000);
|
|
context->slavelist[slave].SM[0].SMlength = htoes(0x0080);
|
|
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
|
|
context->slavelist[slave].SMtype[0] = 1;
|
|
}
|
|
if (context->slavelist[slave].SM[1].StartAddr == 0x0000) /* should never happen */
|
|
{
|
|
EC_PRINT("Slave %d has no proper mailbox out configuration, try default.\n", slave);
|
|
context->slavelist[slave].SM[1].StartAddr = htoes(0x1080);
|
|
context->slavelist[slave].SM[1].SMlength = htoes(0x0080);
|
|
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
|
|
context->slavelist[slave].SMtype[1] = 2;
|
|
}
|
|
/* program SM0 mailbox in and SM1 mailbox out for slave */
|
|
/* writing both SM in one datagram will solve timing issue in old NETX */
|
|
ecx_FPWR(context->port, configadr, ECT_REG_SM0, sizeof(ec_smt) * 2,
|
|
&(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3);
|
|
}
|
|
/* some slaves need eeprom available to PDI in init->preop transition */
|
|
ecx_eeprom2pdi(context, slave);
|
|
/* User may override automatic state change */
|
|
if (context->manualstatechange == 0)
|
|
{
|
|
/* request pre_op for slave */
|
|
ecx_FPWRw(context->port,
|
|
configadr,
|
|
ECT_REG_ALCTL,
|
|
htoes(EC_STATE_PRE_OP | EC_STATE_ACK),
|
|
EC_TIMEOUTRET3); /* set preop status */
|
|
}
|
|
}
|
|
}
|
|
return wkc;
|
|
}
|
|
|
|
/* If slave has SII mapping and same slave ID done before, use previous mapping.
|
|
* This is safe because SII mapping is constant for same slave ID.
|
|
*/
|
|
static int ecx_lookup_mapping(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize)
|
|
{
|
|
int i, nSM;
|
|
if ((slave > 1) && (*(context->slavecount) > 0))
|
|
{
|
|
i = 1;
|
|
while(((context->slavelist[i].eep_man != context->slavelist[slave].eep_man) ||
|
|
(context->slavelist[i].eep_id != context->slavelist[slave].eep_id ) ||
|
|
(context->slavelist[i].eep_rev != context->slavelist[slave].eep_rev)) &&
|
|
(i < slave))
|
|
{
|
|
i++;
|
|
}
|
|
if(i < slave)
|
|
{
|
|
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
context->slavelist[slave].SM[nSM].SMlength = context->slavelist[i].SM[nSM].SMlength;
|
|
context->slavelist[slave].SMtype[nSM] = context->slavelist[i].SMtype[nSM];
|
|
}
|
|
*Osize = context->slavelist[i].Obits;
|
|
*Isize = context->slavelist[i].Ibits;
|
|
context->slavelist[slave].Obits = (uint16)*Osize;
|
|
context->slavelist[slave].Ibits = (uint16)*Isize;
|
|
EC_PRINT("Copy mapping slave %d from %d.\n", slave, i);
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int ecx_map_coe_soe(ecx_contextt *context, uint16 slave, int thread_n)
|
|
{
|
|
uint32 Isize, Osize;
|
|
int rval;
|
|
|
|
ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */
|
|
|
|
EC_PRINT(" >Slave %d, configadr %x, state %2.2x\n",
|
|
slave, context->slavelist[slave].configadr, context->slavelist[slave].state);
|
|
|
|
/* execute special slave configuration hook Pre-Op to Safe-OP */
|
|
if(context->slavelist[slave].PO2SOconfig) /* only if registered */
|
|
{
|
|
context->slavelist[slave].PO2SOconfig(slave);
|
|
}
|
|
if (context->slavelist[slave].PO2SOconfigx) /* only if registered */
|
|
{
|
|
context->slavelist[slave].PO2SOconfigx(context, slave);
|
|
}
|
|
/* if slave not found in configlist find IO mapping in slave self */
|
|
if (!context->slavelist[slave].configindex)
|
|
{
|
|
Isize = 0;
|
|
Osize = 0;
|
|
if (context->slavelist[slave].mbx_proto & ECT_MBXPROT_COE) /* has CoE */
|
|
{
|
|
rval = 0;
|
|
if (context->slavelist[slave].CoEdetails & ECT_COEDET_SDOCA) /* has Complete Access */
|
|
{
|
|
/* read PDO mapping via CoE and use Complete Access */
|
|
rval = ecx_readPDOmapCA(context, slave, thread_n, &Osize, &Isize);
|
|
}
|
|
if (!rval) /* CA not available or not succeeded */
|
|
{
|
|
/* read PDO mapping via CoE */
|
|
rval = ecx_readPDOmap(context, slave, &Osize, &Isize);
|
|
}
|
|
EC_PRINT(" CoE Osize:%u Isize:%u\n", Osize, Isize);
|
|
}
|
|
if ((!Isize && !Osize) && (context->slavelist[slave].mbx_proto & ECT_MBXPROT_SOE)) /* has SoE */
|
|
{
|
|
/* read AT / MDT mapping via SoE */
|
|
rval = ecx_readIDNmap(context, slave, &Osize, &Isize);
|
|
context->slavelist[slave].SM[2].SMlength = htoes((uint16)((Osize + 7) / 8));
|
|
context->slavelist[slave].SM[3].SMlength = htoes((uint16)((Isize + 7) / 8));
|
|
EC_PRINT(" SoE Osize:%u Isize:%u\n", Osize, Isize);
|
|
}
|
|
context->slavelist[slave].Obits = (uint16)Osize;
|
|
context->slavelist[slave].Ibits = (uint16)Isize;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
static int ecx_map_sii(ecx_contextt *context, uint16 slave)
|
|
{
|
|
uint32 Isize, Osize;
|
|
int nSM;
|
|
ec_eepromPDOt eepPDO;
|
|
|
|
Osize = context->slavelist[slave].Obits;
|
|
Isize = context->slavelist[slave].Ibits;
|
|
|
|
if (!Isize && !Osize) /* find PDO in previous slave with same ID */
|
|
{
|
|
(void)ecx_lookup_mapping(context, slave, &Osize, &Isize);
|
|
}
|
|
if (!Isize && !Osize) /* find PDO mapping by SII */
|
|
{
|
|
memset(&eepPDO, 0, sizeof(eepPDO));
|
|
Isize = ecx_siiPDO(context, slave, &eepPDO, 0);
|
|
EC_PRINT(" SII Isize:%u\n", Isize);
|
|
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
if (eepPDO.SMbitsize[nSM] > 0)
|
|
{
|
|
context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8);
|
|
context->slavelist[slave].SMtype[nSM] = 4;
|
|
EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]);
|
|
}
|
|
}
|
|
Osize = ecx_siiPDO(context, slave, &eepPDO, 1);
|
|
EC_PRINT(" SII Osize:%u\n", Osize);
|
|
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
if (eepPDO.SMbitsize[nSM] > 0)
|
|
{
|
|
context->slavelist[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8);
|
|
context->slavelist[slave].SMtype[nSM] = 3;
|
|
EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]);
|
|
}
|
|
}
|
|
}
|
|
context->slavelist[slave].Obits = (uint16)Osize;
|
|
context->slavelist[slave].Ibits = (uint16)Isize;
|
|
EC_PRINT(" ISIZE:%d %d OSIZE:%d\n",
|
|
context->slavelist[slave].Ibits, Isize,context->slavelist[slave].Obits);
|
|
|
|
return 1;
|
|
}
|
|
|
|
static int ecx_map_sm(ecx_contextt *context, uint16 slave)
|
|
{
|
|
uint16 configadr;
|
|
int nSM;
|
|
|
|
configadr = context->slavelist[slave].configadr;
|
|
|
|
EC_PRINT(" SM programming\n");
|
|
if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[0].StartAddr)
|
|
{
|
|
ecx_FPWR(context->port, configadr, ECT_REG_SM0,
|
|
sizeof(ec_smt), &(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3);
|
|
EC_PRINT(" SM0 Type:%d StartAddr:%4.4x Flags:%8.8x\n",
|
|
context->slavelist[slave].SMtype[0],
|
|
etohs(context->slavelist[slave].SM[0].StartAddr),
|
|
etohl(context->slavelist[slave].SM[0].SMflags));
|
|
}
|
|
if (!context->slavelist[slave].mbx_l && context->slavelist[slave].SM[1].StartAddr)
|
|
{
|
|
ecx_FPWR(context->port, configadr, ECT_REG_SM1,
|
|
sizeof(ec_smt), &context->slavelist[slave].SM[1], EC_TIMEOUTRET3);
|
|
EC_PRINT(" SM1 Type:%d StartAddr:%4.4x Flags:%8.8x\n",
|
|
context->slavelist[slave].SMtype[1],
|
|
etohs(context->slavelist[slave].SM[1].StartAddr),
|
|
etohl(context->slavelist[slave].SM[1].SMflags));
|
|
}
|
|
/* program SM2 to SMx */
|
|
for( nSM = 2 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
if (context->slavelist[slave].SM[nSM].StartAddr)
|
|
{
|
|
/* check if SM length is zero -> clear enable flag */
|
|
if( context->slavelist[slave].SM[nSM].SMlength == 0)
|
|
{
|
|
context->slavelist[slave].SM[nSM].SMflags =
|
|
htoel( etohl(context->slavelist[slave].SM[nSM].SMflags) & EC_SMENABLEMASK);
|
|
}
|
|
/* if SM length is non zero always set enable flag */
|
|
else
|
|
{
|
|
context->slavelist[slave].SM[nSM].SMflags =
|
|
htoel( etohl(context->slavelist[slave].SM[nSM].SMflags) | ~EC_SMENABLEMASK);
|
|
}
|
|
ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_SM0 + (nSM * sizeof(ec_smt))),
|
|
sizeof(ec_smt), &context->slavelist[slave].SM[nSM], EC_TIMEOUTRET3);
|
|
EC_PRINT(" SM%d Type:%d StartAddr:%4.4x Flags:%8.8x\n", nSM,
|
|
context->slavelist[slave].SMtype[nSM],
|
|
etohs(context->slavelist[slave].SM[nSM].StartAddr),
|
|
etohl(context->slavelist[slave].SM[nSM].SMflags));
|
|
}
|
|
}
|
|
if (context->slavelist[slave].Ibits > 7)
|
|
{
|
|
context->slavelist[slave].Ibytes = (context->slavelist[slave].Ibits + 7) / 8;
|
|
}
|
|
if (context->slavelist[slave].Obits > 7)
|
|
{
|
|
context->slavelist[slave].Obytes = (context->slavelist[slave].Obits + 7) / 8;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
#if EC_MAX_MAPT > 1
|
|
OSAL_THREAD_FUNC ecx_mapper_thread(void *param)
|
|
{
|
|
ecx_mapt_t *maptp;
|
|
maptp = param;
|
|
ecx_map_coe_soe(maptp->context, maptp->slave, maptp->thread_n);
|
|
maptp->running = 0;
|
|
}
|
|
|
|
static int ecx_find_mapt(void)
|
|
{
|
|
int p;
|
|
p = 0;
|
|
while((p < EC_MAX_MAPT) && ecx_mapt[p].running)
|
|
{
|
|
p++;
|
|
}
|
|
if(p < EC_MAX_MAPT)
|
|
{
|
|
return p;
|
|
}
|
|
else
|
|
{
|
|
return -1;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
static int ecx_get_threadcount(void)
|
|
{
|
|
int thrc, thrn;
|
|
thrc = 0;
|
|
for(thrn = 0 ; thrn < EC_MAX_MAPT ; thrn++)
|
|
{
|
|
thrc += ecx_mapt[thrn].running;
|
|
}
|
|
return thrc;
|
|
}
|
|
|
|
static void ecx_config_find_mappings(ecx_contextt *context, uint8 group)
|
|
{
|
|
int thrn, thrc;
|
|
uint16 slave;
|
|
|
|
for (thrn = 0; thrn < EC_MAX_MAPT; thrn++)
|
|
{
|
|
ecx_mapt[thrn].running = 0;
|
|
}
|
|
/* find CoE and SoE mapping of slaves in multiple threads */
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
#if EC_MAX_MAPT > 1
|
|
/* multi-threaded version */
|
|
while ((thrn = ecx_find_mapt()) < 0)
|
|
{
|
|
osal_usleep(1000);
|
|
}
|
|
ecx_mapt[thrn].context = context;
|
|
ecx_mapt[thrn].slave = slave;
|
|
ecx_mapt[thrn].thread_n = thrn;
|
|
ecx_mapt[thrn].running = 1;
|
|
osal_thread_create(&(ecx_threadh[thrn]), 128000,
|
|
&ecx_mapper_thread, &(ecx_mapt[thrn]));
|
|
#else
|
|
/* serialised version */
|
|
ecx_map_coe_soe(context, slave, 0);
|
|
#endif
|
|
}
|
|
}
|
|
/* wait for all threads to finish */
|
|
do
|
|
{
|
|
thrc = ecx_get_threadcount();
|
|
if (thrc)
|
|
{
|
|
osal_usleep(1000);
|
|
}
|
|
} while (thrc);
|
|
/* find SII mapping of slave and program SM */
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
ecx_map_sii(context, slave);
|
|
ecx_map_sm(context, slave);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap,
|
|
uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
|
|
{
|
|
int BitCount = 0;
|
|
int FMMUdone = 0;
|
|
int AddToInputsWKC = 0;
|
|
uint16 ByteCount = 0;
|
|
uint16 FMMUsize = 0;
|
|
uint8 SMc = 0;
|
|
uint16 EndAddr;
|
|
uint16 SMlength;
|
|
uint16 configadr;
|
|
uint8 FMMUc;
|
|
|
|
EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
|
|
|
|
configadr = context->slavelist[slave].configadr;
|
|
FMMUc = context->slavelist[slave].FMMUunused;
|
|
if (context->slavelist[slave].Obits) /* find free FMMU */
|
|
{
|
|
while (context->slavelist[slave].FMMU[FMMUc].LogStart)
|
|
{
|
|
FMMUc++;
|
|
}
|
|
}
|
|
/* search for SM that contribute to the input mapping */
|
|
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8)))
|
|
{
|
|
EC_PRINT(" FMMU %d\n", FMMUc);
|
|
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
|
|
{
|
|
SMc++;
|
|
}
|
|
EC_PRINT(" SM%d\n", SMc);
|
|
context->slavelist[slave].FMMU[FMMUc].PhysStart =
|
|
context->slavelist[slave].SM[SMc].StartAddr;
|
|
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
|
ByteCount += SMlength;
|
|
BitCount += SMlength * 8;
|
|
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
|
while ((BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1))) /* more SM for input */
|
|
{
|
|
SMc++;
|
|
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
|
|
{
|
|
SMc++;
|
|
}
|
|
/* if addresses from more SM connect use one FMMU otherwise break up in multiple FMMU */
|
|
if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
|
|
{
|
|
break;
|
|
}
|
|
EC_PRINT(" SM%d\n", SMc);
|
|
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
|
ByteCount += SMlength;
|
|
BitCount += SMlength * 8;
|
|
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
|
}
|
|
|
|
/* bit oriented slave */
|
|
if (!context->slavelist[slave].Ibytes)
|
|
{
|
|
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
|
*BitPos += context->slavelist[slave].Ibits - 1;
|
|
if (*BitPos > 7)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos -= 8;
|
|
}
|
|
FMMUsize = (uint16)(*LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1);
|
|
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
|
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
|
*BitPos += 1;
|
|
if (*BitPos > 7)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos -= 8;
|
|
}
|
|
}
|
|
/* byte oriented slave */
|
|
else
|
|
{
|
|
if (*BitPos)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos = 0;
|
|
}
|
|
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
|
*BitPos = 7;
|
|
FMMUsize = ByteCount;
|
|
if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes)
|
|
{
|
|
FMMUsize = (uint16)(context->slavelist[slave].Ibytes - FMMUdone);
|
|
}
|
|
*LogAddr += FMMUsize;
|
|
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
|
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
|
*BitPos = 0;
|
|
}
|
|
FMMUdone += FMMUsize;
|
|
if (context->slavelist[slave].FMMU[FMMUc].LogLength)
|
|
{
|
|
context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
|
|
context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
|
|
context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
|
|
/* program FMMU for input */
|
|
ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
|
|
sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
|
|
/* Set flag to add one for an input FMMU,
|
|
a single ESC can only contribute once */
|
|
AddToInputsWKC = 1;
|
|
}
|
|
if (!context->slavelist[slave].inputs)
|
|
{
|
|
if (group)
|
|
{
|
|
context->slavelist[slave].inputs =
|
|
(uint8 *)(pIOmap) +
|
|
etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) -
|
|
context->grouplist[group].logstartaddr;
|
|
}
|
|
else
|
|
{
|
|
context->slavelist[slave].inputs =
|
|
(uint8 *)(pIOmap) +
|
|
etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
|
|
}
|
|
context->slavelist[slave].Istartbit =
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit;
|
|
EC_PRINT(" Inputs %p startbit %d\n",
|
|
context->slavelist[slave].inputs,
|
|
context->slavelist[slave].Istartbit);
|
|
}
|
|
FMMUc++;
|
|
}
|
|
context->slavelist[slave].FMMUunused = FMMUc;
|
|
|
|
/* Add one WKC for an input if flag is true */
|
|
if (AddToInputsWKC)
|
|
context->grouplist[group].inputsWKC++;
|
|
}
|
|
|
|
static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOmap,
|
|
uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
|
|
{
|
|
int BitCount = 0;
|
|
int FMMUdone = 0;
|
|
int AddToOutputsWKC = 0;
|
|
uint16 ByteCount = 0;
|
|
uint16 FMMUsize = 0;
|
|
uint8 SMc = 0;
|
|
uint16 EndAddr;
|
|
uint16 SMlength;
|
|
uint16 configadr;
|
|
uint8 FMMUc;
|
|
|
|
EC_PRINT(" OUTPUT MAPPING\n");
|
|
|
|
FMMUc = context->slavelist[slave].FMMUunused;
|
|
configadr = context->slavelist[slave].configadr;
|
|
|
|
/* search for SM that contribute to the output mapping */
|
|
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8)))
|
|
{
|
|
EC_PRINT(" FMMU %d\n", FMMUc);
|
|
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
|
|
{
|
|
SMc++;
|
|
}
|
|
EC_PRINT(" SM%d\n", SMc);
|
|
context->slavelist[slave].FMMU[FMMUc].PhysStart =
|
|
context->slavelist[slave].SM[SMc].StartAddr;
|
|
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
|
ByteCount += SMlength;
|
|
BitCount += SMlength * 8;
|
|
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
|
while ((BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1))) /* more SM for output */
|
|
{
|
|
SMc++;
|
|
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
|
|
{
|
|
SMc++;
|
|
}
|
|
/* if addresses from more SM connect use one FMMU otherwise break up in multiple FMMU */
|
|
if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
|
|
{
|
|
break;
|
|
}
|
|
EC_PRINT(" SM%d\n", SMc);
|
|
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
|
ByteCount += SMlength;
|
|
BitCount += SMlength * 8;
|
|
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
|
}
|
|
|
|
/* bit oriented slave */
|
|
if (!context->slavelist[slave].Obytes)
|
|
{
|
|
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
|
*BitPos += context->slavelist[slave].Obits - 1;
|
|
if (*BitPos > 7)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos -= 8;
|
|
}
|
|
FMMUsize = (uint16)(*LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1);
|
|
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
|
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
|
*BitPos += 1;
|
|
if (*BitPos > 7)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos -= 8;
|
|
}
|
|
}
|
|
/* byte oriented slave */
|
|
else
|
|
{
|
|
if (*BitPos)
|
|
{
|
|
*LogAddr += 1;
|
|
*BitPos = 0;
|
|
}
|
|
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
|
*BitPos = 7;
|
|
FMMUsize = ByteCount;
|
|
if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes)
|
|
{
|
|
FMMUsize = (uint16)(context->slavelist[slave].Obytes - FMMUdone);
|
|
}
|
|
*LogAddr += FMMUsize;
|
|
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
|
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
|
*BitPos = 0;
|
|
}
|
|
FMMUdone += FMMUsize;
|
|
if (context->slavelist[slave].FMMU[FMMUc].LogLength)
|
|
{
|
|
context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
|
|
context->slavelist[slave].FMMU[FMMUc].FMMUtype = 2;
|
|
context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
|
|
/* program FMMU for output */
|
|
ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
|
|
sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
|
|
/* Set flag to add one for an output FMMU,
|
|
a single ESC can only contribute once */
|
|
AddToOutputsWKC = 1;
|
|
}
|
|
if (!context->slavelist[slave].outputs)
|
|
{
|
|
if (group)
|
|
{
|
|
context->slavelist[slave].outputs =
|
|
(uint8 *)(pIOmap) +
|
|
etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) -
|
|
context->grouplist[group].logstartaddr;
|
|
}
|
|
else
|
|
{
|
|
context->slavelist[slave].outputs =
|
|
(uint8 *)(pIOmap) +
|
|
etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
|
|
}
|
|
context->slavelist[slave].Ostartbit =
|
|
context->slavelist[slave].FMMU[FMMUc].LogStartbit;
|
|
EC_PRINT(" slave %d Outputs %p startbit %d\n",
|
|
slave,
|
|
context->slavelist[slave].outputs,
|
|
context->slavelist[slave].Ostartbit);
|
|
}
|
|
FMMUc++;
|
|
}
|
|
context->slavelist[slave].FMMUunused = FMMUc;
|
|
/* Add one WKC for an output if flag is true */
|
|
if (AddToOutputsWKC)
|
|
context->grouplist[group].outputsWKC++;
|
|
}
|
|
|
|
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
|
* in sequential order (legacy SOEM way).
|
|
*
|
|
*
|
|
* @param[in] context = context struct
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @param[in] group = group to map, 0 = all groups
|
|
* @return IOmap size
|
|
*/
|
|
int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|
{
|
|
uint16 slave, configadr;
|
|
uint8 BitPos;
|
|
uint32 LogAddr = 0;
|
|
uint32 oLogAddr = 0;
|
|
uint32 diff;
|
|
uint16 currentsegment = 0;
|
|
uint32 segmentsize = 0;
|
|
|
|
if ((*(context->slavecount) > 0) && (group < context->maxgroup))
|
|
{
|
|
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
|
|
LogAddr = context->grouplist[group].logstartaddr;
|
|
oLogAddr = LogAddr;
|
|
BitPos = 0;
|
|
context->grouplist[group].nsegments = 0;
|
|
context->grouplist[group].outputsWKC = 0;
|
|
context->grouplist[group].inputsWKC = 0;
|
|
|
|
/* Find mappings and program syncmanagers */
|
|
ecx_config_find_mappings(context, group);
|
|
|
|
/* do output mapping of slave and program FMMUs */
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
configadr = context->slavelist[slave].configadr;
|
|
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
/* create output mapping */
|
|
if (context->slavelist[slave].Obits)
|
|
{
|
|
ecx_config_create_output_mappings (context, pIOmap, group, slave, &LogAddr, &BitPos);
|
|
diff = LogAddr - oLogAddr;
|
|
oLogAddr = LogAddr;
|
|
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
|
{
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
|
{
|
|
currentsegment++;
|
|
segmentsize = diff;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
segmentsize += diff;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (BitPos)
|
|
{
|
|
LogAddr++;
|
|
oLogAddr = LogAddr;
|
|
BitPos = 0;
|
|
if ((segmentsize + 1) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
|
{
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
|
{
|
|
currentsegment++;
|
|
segmentsize = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
segmentsize += 1;
|
|
}
|
|
}
|
|
context->grouplist[group].outputs = pIOmap;
|
|
context->grouplist[group].Obytes = LogAddr - context->grouplist[group].logstartaddr;
|
|
context->grouplist[group].nsegments = currentsegment + 1;
|
|
context->grouplist[group].Isegment = currentsegment;
|
|
context->grouplist[group].Ioffset = (uint16)segmentsize;
|
|
if (!group)
|
|
{
|
|
context->slavelist[0].outputs = pIOmap;
|
|
context->slavelist[0].Obytes = LogAddr -
|
|
context->grouplist[group].logstartaddr; /* store output bytes in master record */
|
|
}
|
|
|
|
/* do input mapping of slave and program FMMUs */
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
configadr = context->slavelist[slave].configadr;
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
/* create input mapping */
|
|
if (context->slavelist[slave].Ibits)
|
|
{
|
|
|
|
ecx_config_create_input_mappings(context, pIOmap, group, slave, &LogAddr, &BitPos);
|
|
diff = LogAddr - oLogAddr;
|
|
oLogAddr = LogAddr;
|
|
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
|
{
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
|
{
|
|
currentsegment++;
|
|
segmentsize = diff;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
segmentsize += diff;
|
|
}
|
|
}
|
|
|
|
ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
|
|
/* User may override automatic state change */
|
|
if (context->manualstatechange == 0)
|
|
{
|
|
/* request safe_op for slave */
|
|
ecx_FPWRw(context->port,
|
|
configadr,
|
|
ECT_REG_ALCTL,
|
|
htoes(EC_STATE_SAFE_OP),
|
|
EC_TIMEOUTRET3); /* set safeop status */
|
|
}
|
|
if (context->slavelist[slave].blockLRW)
|
|
{
|
|
context->grouplist[group].blockLRW++;
|
|
}
|
|
context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
|
|
}
|
|
}
|
|
if (BitPos)
|
|
{
|
|
LogAddr++;
|
|
oLogAddr = LogAddr;
|
|
BitPos = 0;
|
|
if ((segmentsize + 1) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
|
{
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
|
{
|
|
currentsegment++;
|
|
segmentsize = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
segmentsize += 1;
|
|
}
|
|
}
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
context->grouplist[group].nsegments = currentsegment + 1;
|
|
context->grouplist[group].inputs = (uint8 *)(pIOmap) + context->grouplist[group].Obytes;
|
|
context->grouplist[group].Ibytes = LogAddr -
|
|
context->grouplist[group].logstartaddr -
|
|
context->grouplist[group].Obytes;
|
|
if (!group)
|
|
{
|
|
context->slavelist[0].inputs = (uint8 *)(pIOmap) + context->slavelist[0].Obytes;
|
|
context->slavelist[0].Ibytes = LogAddr -
|
|
context->grouplist[group].logstartaddr -
|
|
context->slavelist[0].Obytes; /* store input bytes in master record */
|
|
}
|
|
|
|
EC_PRINT("IOmapSize %d\n", LogAddr - context->grouplist[group].logstartaddr);
|
|
|
|
return (LogAddr - context->grouplist[group].logstartaddr);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
|
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
|
*
|
|
* @param[in] context = context struct
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @param[in] group = group to map, 0 = all groups
|
|
* @return IOmap size
|
|
*/
|
|
int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|
{
|
|
uint16 slave, configadr;
|
|
uint8 BitPos;
|
|
uint32 mLogAddr = 0;
|
|
uint32 siLogAddr = 0;
|
|
uint32 soLogAddr = 0;
|
|
uint32 tempLogAddr;
|
|
uint32 diff;
|
|
uint16 currentsegment = 0;
|
|
uint32 segmentsize = 0;
|
|
|
|
if ((*(context->slavecount) > 0) && (group < context->maxgroup))
|
|
{
|
|
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
|
|
mLogAddr = context->grouplist[group].logstartaddr;
|
|
siLogAddr = mLogAddr;
|
|
soLogAddr = mLogAddr;
|
|
BitPos = 0;
|
|
context->grouplist[group].nsegments = 0;
|
|
context->grouplist[group].outputsWKC = 0;
|
|
context->grouplist[group].inputsWKC = 0;
|
|
|
|
/* Find mappings and program syncmanagers */
|
|
ecx_config_find_mappings(context, group);
|
|
|
|
/* do IO mapping of slave and program FMMUs */
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
configadr = context->slavelist[slave].configadr;
|
|
siLogAddr = soLogAddr = mLogAddr;
|
|
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
/* create output mapping */
|
|
if (context->slavelist[slave].Obits)
|
|
{
|
|
|
|
ecx_config_create_output_mappings(context, pIOmap, group,
|
|
slave, &soLogAddr, &BitPos);
|
|
if (BitPos)
|
|
{
|
|
soLogAddr++;
|
|
BitPos = 0;
|
|
}
|
|
}
|
|
|
|
/* create input mapping */
|
|
if (context->slavelist[slave].Ibits)
|
|
{
|
|
ecx_config_create_input_mappings(context, pIOmap, group,
|
|
slave, &siLogAddr, &BitPos);
|
|
if (BitPos)
|
|
{
|
|
siLogAddr++;
|
|
BitPos = 0;
|
|
}
|
|
}
|
|
|
|
tempLogAddr = (siLogAddr > soLogAddr) ? siLogAddr : soLogAddr;
|
|
diff = tempLogAddr - mLogAddr;
|
|
mLogAddr = tempLogAddr;
|
|
|
|
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
|
{
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
|
{
|
|
currentsegment++;
|
|
segmentsize = diff;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
segmentsize += diff;
|
|
}
|
|
|
|
ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
|
|
/* User may override automatic state change */
|
|
if (context->manualstatechange == 0)
|
|
{
|
|
/* request safe_op for slave */
|
|
ecx_FPWRw(context->port,
|
|
configadr,
|
|
ECT_REG_ALCTL,
|
|
htoes(EC_STATE_SAFE_OP),
|
|
EC_TIMEOUTRET3);
|
|
}
|
|
if (context->slavelist[slave].blockLRW)
|
|
{
|
|
context->grouplist[group].blockLRW++;
|
|
}
|
|
context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
|
|
|
|
}
|
|
}
|
|
|
|
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
|
context->grouplist[group].nsegments = currentsegment + 1;
|
|
context->grouplist[group].Isegment = 0;
|
|
context->grouplist[group].Ioffset = 0;
|
|
|
|
context->grouplist[group].Obytes = soLogAddr - context->grouplist[group].logstartaddr;
|
|
context->grouplist[group].Ibytes = siLogAddr - context->grouplist[group].logstartaddr;
|
|
context->grouplist[group].outputs = pIOmap;
|
|
context->grouplist[group].inputs = (uint8 *)pIOmap + context->grouplist[group].Obytes;
|
|
|
|
/* Move calculated inputs with OBytes offset*/
|
|
for (slave = 1; slave <= *(context->slavecount); slave++)
|
|
{
|
|
if (!group || (group == context->slavelist[slave].group))
|
|
{
|
|
if(context->slavelist[slave].Ibits > 0)
|
|
{
|
|
context->slavelist[slave].inputs += context->grouplist[group].Obytes;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!group)
|
|
{
|
|
/* store output bytes in master record */
|
|
context->slavelist[0].outputs = pIOmap;
|
|
context->slavelist[0].Obytes = soLogAddr - context->grouplist[group].logstartaddr;
|
|
context->slavelist[0].inputs = (uint8 *)pIOmap + context->slavelist[0].Obytes;
|
|
context->slavelist[0].Ibytes = siLogAddr - context->grouplist[group].logstartaddr;
|
|
}
|
|
|
|
EC_PRINT("IOmapSize %d\n", context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
|
|
|
|
return (context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/** Recover slave.
|
|
*
|
|
* @param[in] context = context struct
|
|
* @param[in] slave = slave to recover
|
|
* @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3
|
|
* @return >0 if successful
|
|
*/
|
|
int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout)
|
|
{
|
|
int rval;
|
|
int wkc;
|
|
uint16 ADPh, configadr, readadr;
|
|
|
|
rval = 0;
|
|
configadr = context->slavelist[slave].configadr;
|
|
ADPh = (uint16)(1 - slave);
|
|
/* check if we found another slave than the requested */
|
|
readadr = 0xfffe;
|
|
wkc = ecx_APRD(context->port, ADPh, ECT_REG_STADR, sizeof(readadr), &readadr, timeout);
|
|
/* correct slave found, finished */
|
|
if(readadr == configadr)
|
|
{
|
|
return 1;
|
|
}
|
|
/* only try if no config address*/
|
|
if( (wkc > 0) && (readadr == 0))
|
|
{
|
|
/* clear possible slaves at EC_TEMPNODE */
|
|
ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0);
|
|
/* set temporary node address of slave */
|
|
if(ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(EC_TEMPNODE) , timeout) <= 0)
|
|
{
|
|
ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , 0);
|
|
return 0; /* slave fails to respond */
|
|
}
|
|
|
|
context->slavelist[slave].configadr = EC_TEMPNODE; /* temporary config address */
|
|
ecx_eeprom2master(context, slave); /* set Eeprom control to master */
|
|
|
|
/* check if slave is the same as configured before */
|
|
if ((ecx_FPRDw(context->port, EC_TEMPNODE, ECT_REG_ALIAS, timeout) ==
|
|
htoes(context->slavelist[slave].aliasadr)) &&
|
|
(ecx_readeeprom(context, slave, ECT_SII_ID, EC_TIMEOUTEEP) ==
|
|
htoel(context->slavelist[slave].eep_id)) &&
|
|
(ecx_readeeprom(context, slave, ECT_SII_MANUF, EC_TIMEOUTEEP) ==
|
|
htoel(context->slavelist[slave].eep_man)) &&
|
|
(ecx_readeeprom(context, slave, ECT_SII_REV, EC_TIMEOUTEEP) ==
|
|
htoel(context->slavelist[slave].eep_rev)))
|
|
{
|
|
rval = ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(configadr) , timeout);
|
|
context->slavelist[slave].configadr = configadr;
|
|
}
|
|
else
|
|
{
|
|
/* slave is not the expected one, remove config address*/
|
|
ecx_FPWRw(context->port, EC_TEMPNODE, ECT_REG_STADR, htoes(0) , timeout);
|
|
context->slavelist[slave].configadr = configadr;
|
|
}
|
|
}
|
|
|
|
return rval;
|
|
}
|
|
|
|
/** Reconfigure slave.
|
|
*
|
|
* @param[in] context = context struct
|
|
* @param[in] slave = slave to reconfigure
|
|
* @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3
|
|
* @return Slave state
|
|
*/
|
|
int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout)
|
|
{
|
|
int state, nSM, FMMUc;
|
|
uint16 configadr;
|
|
|
|
configadr = context->slavelist[slave].configadr;
|
|
if (ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_INIT) , timeout) <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
state = 0;
|
|
ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
|
|
/* check state change init */
|
|
state = ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE);
|
|
if(state == EC_STATE_INIT)
|
|
{
|
|
/* program all enabled SM */
|
|
for( nSM = 0 ; nSM < EC_MAXSM ; nSM++ )
|
|
{
|
|
if (context->slavelist[slave].SM[nSM].StartAddr)
|
|
{
|
|
ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_SM0 + (nSM * sizeof(ec_smt))),
|
|
sizeof(ec_smt), &context->slavelist[slave].SM[nSM], timeout);
|
|
}
|
|
}
|
|
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP) , timeout);
|
|
state = ecx_statecheck(context, slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); /* check state change pre-op */
|
|
if( state == EC_STATE_PRE_OP)
|
|
{
|
|
/* execute special slave configuration hook Pre-Op to Safe-OP */
|
|
if(context->slavelist[slave].PO2SOconfig) /* only if registered */
|
|
{
|
|
context->slavelist[slave].PO2SOconfig(slave);
|
|
}
|
|
if (context->slavelist[slave].PO2SOconfigx) /* only if registered */
|
|
{
|
|
context->slavelist[slave].PO2SOconfigx(context, slave);
|
|
}
|
|
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , timeout); /* set safeop status */
|
|
state = ecx_statecheck(context, slave, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); /* check state change safe-op */
|
|
/* program configured FMMU */
|
|
for( FMMUc = 0 ; FMMUc < context->slavelist[slave].FMMUunused ; FMMUc++ )
|
|
{
|
|
ecx_FPWR(context->port, configadr, (uint16)(ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc)),
|
|
sizeof(ec_fmmut), &context->slavelist[slave].FMMU[FMMUc], timeout);
|
|
}
|
|
}
|
|
}
|
|
|
|
return state;
|
|
}
|
|
|
|
#ifdef EC_VER1
|
|
/** Enumerate and init all slaves.
|
|
*
|
|
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
|
* @return Workcounter of slave discover datagram = number of slaves found
|
|
* @see ecx_config_init
|
|
*/
|
|
int ec_config_init(uint8 usetable)
|
|
{
|
|
return ecx_config_init(&ecx_context, usetable);
|
|
}
|
|
|
|
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
|
* in sequential order (legacy SOEM way).
|
|
*
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @param[in] group = group to map, 0 = all groups
|
|
* @return IOmap size
|
|
* @see ecx_config_map_group
|
|
*/
|
|
int ec_config_map_group(void *pIOmap, uint8 group)
|
|
{
|
|
return ecx_config_map_group(&ecx_context, pIOmap, group);
|
|
}
|
|
|
|
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
|
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
|
*
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @param[in] group = group to map, 0 = all groups
|
|
* @return IOmap size
|
|
* @see ecx_config_overlap_map_group
|
|
*/
|
|
int ec_config_overlap_map_group(void *pIOmap, uint8 group)
|
|
{
|
|
return ecx_config_overlap_map_group(&ecx_context, pIOmap, group);
|
|
}
|
|
|
|
/** Map all PDOs from slaves to IOmap with Outputs/Inputs
|
|
* in sequential order (legacy SOEM way).
|
|
*
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @return IOmap size
|
|
*/
|
|
int ec_config_map(void *pIOmap)
|
|
{
|
|
return ec_config_map_group(pIOmap, 0);
|
|
}
|
|
|
|
/** Map all PDOs from slaves to IOmap with Outputs/Inputs
|
|
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
|
*
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @return IOmap size
|
|
*/
|
|
int ec_config_overlap_map(void *pIOmap)
|
|
{
|
|
return ec_config_overlap_map_group(pIOmap, 0);
|
|
}
|
|
|
|
/** Enumerate / map and init all slaves.
|
|
*
|
|
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @return Workcounter of slave discover datagram = number of slaves found
|
|
*/
|
|
int ec_config(uint8 usetable, void *pIOmap)
|
|
{
|
|
int wkc;
|
|
wkc = ec_config_init(usetable);
|
|
if (wkc)
|
|
{
|
|
ec_config_map(pIOmap);
|
|
}
|
|
return wkc;
|
|
}
|
|
|
|
/** Enumerate / map and init all slaves.
|
|
*
|
|
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
|
* @param[out] pIOmap = pointer to IOmap
|
|
* @return Workcounter of slave discover datagram = number of slaves found
|
|
*/
|
|
int ec_config_overlap(uint8 usetable, void *pIOmap)
|
|
{
|
|
int wkc;
|
|
wkc = ec_config_init(usetable);
|
|
if (wkc)
|
|
{
|
|
ec_config_overlap_map(pIOmap);
|
|
}
|
|
return wkc;
|
|
}
|
|
|
|
/** Recover slave.
|
|
*
|
|
* @param[in] slave = slave to recover
|
|
* @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3
|
|
* @return >0 if successful
|
|
* @see ecx_recover_slave
|
|
*/
|
|
int ec_recover_slave(uint16 slave, int timeout)
|
|
{
|
|
return ecx_recover_slave(&ecx_context, slave, timeout);
|
|
}
|
|
|
|
/** Reconfigure slave.
|
|
*
|
|
* @param[in] slave = slave to reconfigure
|
|
* @param[in] timeout = local timeout f.e. EC_TIMEOUTRET3
|
|
* @return Slave state
|
|
* @see ecx_reconfig_slave
|
|
*/
|
|
int ec_reconfig_slave(uint16 slave, int timeout)
|
|
{
|
|
return ecx_reconfig_slave(&ecx_context, slave, timeout);
|
|
}
|
|
#endif
|