/** 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]),
(void *) &ESCvar.ALevent);
ESC_read (ESCREG_SM1, (void *) &ESCvar.SM[1], sizeof (ESCvar.SM[1]),
(void *) &ESCvar.ALevent);
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;
}
/** Validate the values of Sync Manager 2 & 3 that the current ESC values is
* equal to configured and calculated local values.
*
* @param[in] state = Requested state.
* @return = incoming state request if every thing checks out OK. = state (PREOP | ERROR) if something isn't correct.
*/
uint8_t ESC_checkSM23 (uint8_t state)
{
uint64_t sm2;
// printf("checking SM2 & SM3\n");
_ESCsm2 *SM;
// printf(".");
ESC_read (ESCREG_SM2, (void *) &ESCvar.SM[2], sizeof (ESCvar.SM[2]),
(void *) &ESCvar.ALevent);
ESC_read(ESCREG_SM2, (void*)&sm2, 8, (void*)&ESCvar.ALevent);
// printf(".");
ESC_read (ESCREG_SM3, (void *) &ESCvar.SM[3], sizeof (ESCvar.SM[3]),
(void *) &ESCvar.ALevent);
// printf(".\n");
SM = (_ESCsm2 *) & ESCvar.SM[2];
// printf("read SM2, comparing\n");
if ((etohs (SM->PSA) != SM2_sma) || (etohs (SM->Length) != SM2_sml)
|| (SM->Command != SM2_smc) || !(SM->ActESC & SM2_act))
{
ESCvar.SMtestresult = SMRESULT_ERRSM2;
/* fail state change */
// printf("SM2:\tread\tvs\tneeded\n_sma\t%x\tvs\t%x\n_sml\t%d\tvs\t%d\n_smc\t%x\tvs\t%x\n_act\t%x\tvs\t%x\n",SM->PSA,SM2_sma,SM->Length,SM2_sml,SM->Command,SM2_smc,SM->ActESC,SM2_act);
return (ESCpreop | ESCerror);
}
SM = (_ESCsm2 *) & ESCvar.SM[3];
if ((etohs (SM->PSA) != SM3_sma) || (etohs (SM->Length) != SM3_sml)
|| (SM->Command != SM3_smc) || !(SM->ActESC & SM3_act))
{
// printf("SM3:\tread\tvs\needed\n_sma\t%x\tvs\t%x\n_sml\t%d\tvs\t%d\n_smc\t%x\tvs\t%x\n_act\t%x\tvs\t%x\n",etohs (SM->PSA),SM3_sma,etohs (SM->Length),SM3_sml,SM->Command,SM3_smc,SM->ActESC,SM3_act);
ESCvar.SMtestresult = SMRESULT_ERRSM3;
/* fail state change */
return (ESCpreop | ESCerror);
}
return state;
}
void* EC_Header::add(void *ecatHeader, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, int prevlength)
{
EC_Header* self = static_cast<EC_Header*>(ecatHeader);
/* add new datagram to ethernet frame size */
self->elength_ = htoes(etohs(self->elength_) + sizeof(EC_Header)+length);
/* add "datagram follows" flag to previous subframe dlength */
self->dlength_ = htoes(etohs(self->dlength_) | EC_DATAGRAMFOLLOWS);
/* set new EtherCAT header position */
size_t offset = (prevlength - sizeof(EtherNetHeader)) - sizeof(self->elength_);
self = reinterpret_cast<EC_Header*>(static_cast<uint8*>(ecatHeader)+offset);
self->command_ = com;
self->index_ = idx;
self->ADP_ = htoes(ADP);
self->ADO_ = htoes(ADO);
if (more)
{
/* this is not the last datagram to add */
self->dlength_ = htoes(length | EC_DATAGRAMFOLLOWS);
}
else
{
/* this is the last datagram in the frame */
self->dlength_ = htoes(length);
}
return reinterpret_cast<uint8*>(self)+sizeof(EC_Header);
}
/** SoE read AT and MTD mapping.
*
* SoE has standard indexes defined for mapping. This function
* tries to read them and collect a full input and output mapping size
* of designated slave.
*
* @param[in] Slave = Slave number
* @param[out] Osize = Size in bits of output mapping (MTD) found
* @param[out] Isize = Size in bits of input mapping (AT) found
* @return >0 if mapping succesful.
*/
int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
{
int retVal = 0;
int wkc;
int psize;
uint16 entries, itemcount;
*Isize = 0;
*Osize = 0;
psize = sizeof(SoEmapping);
/* read output mapping via SoE */
wkc = ec_SoEread(slave, 0, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
{
/* command word (uint16) is always mapped but not in list */
*Osize = 16;
for (itemcount = 0 ; itemcount < entries ; itemcount++)
{
psize = sizeof(SoEattribute);
/* read attribute of each IDN in mapping list */
wkc = ec_SoEread(slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
if ((wkc > 0) && (!SoEattribute.list))
{
/* length : 0 = 8bit, 1 = 16bit .... */
*Osize += (int)8 << SoEattribute.length;
}
}
}
psize = sizeof(SoEmapping);
/* read input mapping via SoE */
wkc = ec_SoEread(slave, 0, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
{
/* status word (uint16) is always mapped but not in list */
*Isize = 16;
for (itemcount = 0 ; itemcount < entries ; itemcount++)
{
psize = sizeof(SoEattribute);
/* read attribute of each IDN in mapping list */
wkc = ec_SoEread(slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
if ((wkc > 0) && (!SoEattribute.list))
{
/* length : 0 = 8bit, 1 = 16bit .... */
*Isize += (int)8 << SoEattribute.length;
}
}
}
/* found some I/O bits ? */
if ((*Isize > 0) || (*Osize > 0))
{
retVal = 1;
}
return retVal;
}
void ec_bufT::receive_processdata(uint16 DCtO, uint16 DCl, uint16 stacklen, int wkc2, void* stackdata, int64* DCtime, int* wkc, int *valid_wkc, boolean* first)
{
ec_cmdtype command = getCommand();
switch (command)
{
case EC_CMD_LRD:/* fall-through */
case EC_CMD_LRW:
if (*first)
{
uint16 le_wkc = 0;
memcpy(stackdata, &(buf_[sizeof(EC_Header)]), DCl);
memcpy(&le_wkc, &(buf_[sizeof(EC_Header) + DCl]), sizeof(WorkCounter));
*wkc = etohs(le_wkc);
int64 le_DCtime = 0;
memcpy(&le_DCtime, &(buf_[DCtO]), sizeof(le_DCtime));
*(DCtime) = etohll(le_DCtime);
*first = FALSE;
}
else
{
/* copy input data back to process data buffer */
memcpy(stackdata, &(buf_[sizeof(EC_Header)]), stacklen);
*wkc += wkc2;
}
*valid_wkc = 1;
break;
case EC_CMD_LWR:
if (*first)
{
uint16 le_wkc = 0;
memcpy(&le_wkc, &(buf_[sizeof(EC_Header) + DCl]), sizeof(WorkCounter));
/* output WKC counts 2 times when using LRW, emulate the same for LWR */
*wkc = etohs(le_wkc) * 2;
int64 le_DCtime = 0;
memcpy(&le_DCtime, &(buf_[DCtO]), sizeof(le_DCtime));
*(DCtime) = etohll(le_DCtime);
*first = FALSE;
}
else
{
/* output WKC counts 2 times when using LRW, emulate the same for LWR */
*wkc += wkc2 * 2;
}
*valid_wkc = 1;
break;
default:
//nop
break;
}
}
/** Write local mailbox buffer ESCvar.MBX[n] to Send mailbox.
* Combined function for bootstrap and other states. State check decides
* which one to write.
*
* @param[in] n = Which local mailbox buffer n to send.
*/
void ESC_writembx (uint8_t n)
{
_MBX *MB = &MBX[n];
uint8_t dummy = 0;
uint16_t length;
length = etohs (MB->header.length);
if (ESCvar.ALstatus == ESCboot)
{
if (length > (MBX1_sml_b - MBXHSIZE))
{
length = MBX1_sml_b - MBXHSIZE;
}
ESC_write (MBX1_sma_b, MB, MBXHSIZE + length, (void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX1_sml_b)
{
ESC_write (MBX1_sme_b, &dummy, 1, (void *) &ESCvar.ALevent);
}
}
else
{
if (length > (MBX1_sml - MBXHSIZE))
{
length = MBX1_sml - MBXHSIZE;
}
ESC_write (MBX1_sma, MB, MBXHSIZE + length, (void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX1_sml)
{
ESC_write (MBX1_sme, &dummy, 1, (void *) &ESCvar.ALevent);
}
}
ESCvar.mbxfree = 0;
}
/** Read Watchdog Status register 0x440. Result Bit0 0= Expired, 1= Active or disabled.
*
* @return value of register Watchdog Status.
*/
uint8_t ESC_WDstatus (void)
{
uint16_t wdstatus;
ESC_read (ESCREG_WDSTATUS, &wdstatus, 2, (void *) &ESCvar.ALevent);
wdstatus = etohs (wdstatus);
return (uint8_t) wdstatus;
}
void FOE_read()
{
_FOE *foembx;
uint16_t data_len;
uint32_t password;
uint16_t res;
if(ESCvar.foestate != FOE_READY)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
FOE_init();
foembx = (_FOE*)&MBX[0];
//Get the length of the file name in octets.
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
password = etohl(foembx->foeheader.x.password);
res = FOE_fopen(foembx->y.filename, data_len, password, FOE_OP_RRQ);
if(res==0)
{
ESCvar.foepacket=1;
/* Attempt to send the packet */
res = FOE_send_data_packet();
if(res <= FOE_DATA_SIZE)
{
ESCvar.foestate = FOE_WAIT_FOR_ACK;
}
else
FOE_abort(res);
}
else
FOE_abort(res);
}
/**
* Initialize the slave stack.
*/
void ecat_slv_init (esc_cfg_t * config)
{
DPRINT ("Slave stack init started\n");
ESCvar.TXPDOsize = ESCvar.ESC_SM3_sml = sizeOfPDO(TX_PDO_OBJIDX);
ESCvar.RXPDOsize = ESCvar.ESC_SM2_sml = sizeOfPDO(RX_PDO_OBJIDX);
/* Init watchdog */
watchdog = config->watchdog_cnt;
/* Call stack configuration */
ESC_config (config);
/* Call HW init */
ESC_init (config);
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
}
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx();
ESC_stopinput();
ESC_stopoutput();
}
void FOE_write()
{
_FOE *foembx;
uint16_t data_len;
uint32_t password;
int16_t res;
if(ESCvar.foestate != FOE_READY)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
FOE_init();
foembx = (_FOE*)&MBX[0];
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
password = etohl(foembx->foeheader.x.password);
/* Get an address we can write the file to, if possible. */
res = FOE_fopen(foembx->y.filename, data_len, password, FOE_OP_WRQ);
if(res==0)
{
res = FOE_send_ack();
if(res)
FOE_abort(res);
else
ESCvar.foestate = FOE_WAIT_FOR_DATA;
}
else
FOE_abort(res);
}
/** ESC read function used by the Slave stack.
*
* @param[in] address = address of ESC register to read
* @param[out] buf = pointer to buffer to read in
* @param[in] len = number of bytes to read
*/
void ESC_read (uint16_t address, void *buf, uint16_t len)
{
if(use_all_interrupts == 0)
{
ESCvar.ALevent = etohs ((uint16_t)ecat0->AL_EVENT_REQ);
}
memcpy (buf, ESCADDR(address), len);
}
void ESC_foeprocess(void)
{
_MBXh *mbh;
_FOE *foembx;
//*leds=32;
if (!MBXrun)
return;
if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
{
mbh = (_MBXh *)&MBX[0];
if (mbh->mbxtype == MBXFOE)
ESCvar.xoe = MBXFOE;
}
if (ESCvar.xoe == MBXFOE)
{
foembx = (_FOE *)&MBX[0];
/* Verify the size of the file data. */
if(etohs(foembx->mbxheader.length) < FOEHSIZE)
FOE_abort(MBXERR_SIZETOOSHORT);
else
{
switch(foembx->foeheader.opcode)
{
case FOE_OP_RRQ:
foe_mode = FOE_READ;
FOE_read();
break;
case FOE_OP_ACK:
FOE_ack();
break;
case FOE_OP_WRQ:
foe_mode = FOE_WRITE;
FOE_write();
break;
case FOE_OP_DATA:
FOE_data();
break;
case FOE_OP_BUSY:
FOE_busy();
break;
case FOE_OP_ERR:
foe_mode = FOE_IDLE;
FOE_error();
break;
default:
foe_mode = FOE_IDLE;
FOE_abort(FOE_ERR_NOTDEFINED);
}
}
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
}
/** EPP periodic task of ESC side EEPROM emulation.
*
*/
void EEP_process (void)
{
eep_stat_t stat;
/* check for eeprom event */
if ((ESCvar.ALevent & ESCREG_ALEVENT_EEP) == 0) {
return;
}
while (1) {
/* read eeprom status */
ESC_read (ESCREG_EECONTSTAT, &stat, sizeof (eep_stat_t));
stat.contstat.reg = etohs(stat.contstat.reg);
stat.addr = etohl(stat.addr);
/* check busy flag, exit if job finished */
if (!stat.contstat.bits.busy) {
return;
}
/* clear error bits */
stat.contstat.bits.csumErr = 0;
stat.contstat.bits.eeLoading = 0;
stat.contstat.bits.ackErr = 0;
stat.contstat.bits.wrErr = 0;
/* process commands */
switch (stat.contstat.bits.cmdReg) {
case EEP_CMD_IDLE:
break;
case EEP_CMD_READ:
case EEP_CMD_RELOAD:
/* handle read request */
if (EEP_read (stat.addr * sizeof(uint16_t), eep_buf, EEP_READ_SIZE) != 0) {
stat.contstat.bits.ackErr = 1;
} else {
ESC_write (ESCREG_EEDATA, eep_buf, EEP_READ_SIZE);
}
break;
case EEP_CMD_WRITE:
/* handle write request */
ESC_read (ESCREG_EEDATA, eep_buf, EEP_WRITE_SIZE);
if (EEP_write (stat.addr * sizeof(uint16_t), eep_buf, EEP_WRITE_SIZE) != 0) {
stat.contstat.bits.ackErr = 1;
}
break;
default:
stat.contstat.bits.ackErr = 1;
}
/* acknowledge command */
stat.contstat.reg = htoes(stat.contstat.reg);
ESC_write (ESCREG_EECONTSTAT, &stat.contstat.reg, sizeof(uint16_t));
}
}
/** Add EtherCAT datagram to a standard ethernet frame with existing datagram(s).
*
* @param[out] frame = framebuffer
* @param[in] com = command
* @param[in] idx = index used for TX and RX buffers
* @param[in] more = TRUE if still more datagrams to follow
* @param[in] ADP = Address Position
* @param[in] ADO = Address Offset
* @param[in] length = length of datagram excluding EtherCAT header
* @param[in] data = databuffer to be copied in datagram
* @return Offset to data in rx frame, usefull to retrieve data after RX.
*/
int ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data)
{
ec_comt *datagramP;
uint8 *frameP;
uint16 prevlength;
frameP = frame;
/* copy previous frame size */
prevlength = ec_txbuflength[idx];
datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE];
/* add new datagram to ethernet frame size */
datagramP->elength = htoes( etohs(datagramP->elength) + EC_HEADERSIZE + length );
/* add "datagram follows" flag to previous subframe dlength */
datagramP->dlength = htoes( etohs(datagramP->dlength) | EC_DATAGRAMFOLLOWS );
/* set new EtherCAT header position */
datagramP = (ec_comt*)&frameP[prevlength - EC_ELENGTHSIZE];
datagramP->command = com;
datagramP->index = idx;
datagramP->ADP = htoes(ADP);
datagramP->ADO = htoes(ADO);
if (more)
{
/* this is not the last datagram to add */
datagramP->dlength = htoes(length | EC_DATAGRAMFOLLOWS);
}
else
{
/* this is the last datagram in the frame */
datagramP->dlength = htoes(length);
}
if (length > 0)
{
memcpy(&frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE], data, length);
}
/* set WKC to zero */
frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length] = 0x00;
frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length + 1] = 0x00;
/* set size of frame in buffer array */
ec_txbuflength[idx] = prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + EC_WKCSIZE + length;
/* return offset to data in rx frame
14 bytes smaller than tx frame due to stripping of ethernet header */
return prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE - ETH_HEADERSIZE;
}
/** Read Receive mailbox and store data in local ESCvar.MBX variable.
* Combined function for bootstrap and other states. State check decides
* which one to read.
*/
void ESC_readmbx (void)
{
_MBX *MB = &MBX[0];
uint16_t length;
if (ESCvar.ALstatus == ESCboot)
{
ESC_read (MBX0_sma_b, MB, MBXHSIZE, (void *) &ESCvar.ALevent);
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,
(void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX0_sml_b)
{
ESC_read (MBX0_sme_b, &length, 1, (void *) &ESCvar.ALevent);
}
}
else
{
ESC_read (MBX0_sma, MB, MBXHSIZE, (void *) &ESCvar.ALevent);
length = etohs (MB->header.length);
if (length > (MBX0_sml - MBXHSIZE))
{
length = MBX0_sml - MBXHSIZE;
}
ESC_read (MBX0_sma + MBXHSIZE, &(MB->b[0]), length,
(void *) &ESCvar.ALevent);
if (length + MBXHSIZE < MBX0_sml)
{
ESC_read (MBX0_sme, &length, 1, (void *) &ESCvar.ALevent);
}
}
MBXcontrol[0].state = MBXstate_inclaim;
}
/** Handler for incorrect or unsupported mailbox data. Write error response
* in Mailbox.
*/
void ESC_xoeprocess (void)
{
_MBXh *mbh;
if (!MBXrun)
{
return;
}
if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
{
mbh = (_MBXh *) & MBX[0];
if ((mbh->mbxtype == 0) || (etohs (mbh->length) == 0))
{
MBX_error (MBXERR_INVALIDHEADER);
}
else
{
MBX_error (MBXERR_UNSUPPORTEDPROTOCOL);
}
/* mailbox type not supported, drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
}
void FOE_data()
{
_FOE *foembx;
uint32_t packet;
// uint16_t data_len, ncopied;
uint16_t data_len;
int16_t res;
if(ESCvar.foestate != FOE_WAIT_FOR_DATA)
{
FOE_abort(FOE_ERR_ILLEGAL);
return;
}
foembx =(_FOE*)&MBX[0];
data_len = etohs(foembx->mbxheader.length) - FOEHSIZE;
packet = etohl(foembx->foeheader.x.packetnumber);
if(packet != ESCvar.foepacket)
FOE_abort(FOE_ERR_PACKETNO);
else if(ESCvar.fposition + data_len > ESCvar.fend)
FOE_abort(FOE_ERR_DISKFULL);
else
{
// ncopied = FOE_fwrite(foembx->y.data, data_len);
FOE_fwrite(foembx->y.data, data_len);
if (foe_mode == FOE_WRITE) res = FOE_send_ack(); else res=0;
if(data_len == FOE_DATA_SIZE)
{
//res = FOE_send_ack();
if(res) FOE_abort(res);
}
else
{
FOE_fclose();
FOE_init();
}
}
}
/** SOES main loop. Start by initializing the stack software followed by
* the application loop for cyclic read the EtherCAT state and staus, update
* of I/O.
*/
void soes (void *arg)
{
DPRINT ("SOES (Simple Open EtherCAT Slave)\n");
TXPDOsize = SM3_sml = sizeTXPDO ();
RXPDOsize = SM2_sml = sizeRXPDO ();
/* Setup post config hooks */
static esc_cfg_t config =
{
.pre_state_change_hook = NULL,
.post_state_change_hook = post_state_change_hook
};
ESC_config ((esc_cfg_t *)&config);
ESC_reset();
ESC_init (spi_name);
task_delay (tick_from_ms (200));
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
}
/* Pre FoE to set up Application information */
bootstrap_foe_init ();
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
DPRINT ("Application_loop GO\n");
/* application run loop */
while (1)
{
/* On init restore PDO mappings to default size */
if((ESCvar.ALstatus & 0x0f) == ESCinit)
{
txpdomap = DEFAULTTXPDOMAP;
rxpdomap = DEFAULTRXPDOMAP;
txpdoitems = DEFAULTTXPDOITEMS;
rxpdoitems = DEFAULTTXPDOITEMS;
}
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
/* Check the state machine */
ESC_state ();
/* If else to two separate execution paths
* If we're running BOOSTRAP
* - MailBox
* - FoE
* Else we're running normal execution
* - MailBox
* - CoE
*/
if(local_boot_state)
{
if (ESC_mbxprocess ())
{
ESC_foeprocess ();
ESC_xoeprocess ();
}
bootstrap_state ();
}
else
{
if (ESC_mbxprocess ())
{
ESC_coeprocess ();
ESC_xoeprocess ();
}
DIG_process ();
}
};
}
/** 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
*/
int ec_config_init(uint8 usetable)
{
uint16 w, slave, ADPh, configadr, mbx_wo, mbx_ro, mbx_l, ssigen;
uint16 topology, estat;
int16 topoc, slavec;
uint8 b,h;
uint8 zbuf[64];
uint8 SMc;
uint32 eedat;
int wkc, cindex, nSM;
ec_slavecount = 0;
/* clean ec_slave array */
memset(&ec_slave, 0x00, sizeof(ec_slave));
memset(&zbuf, 0x00, sizeof(zbuf));
w = 0x0000;
wkc = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
if (wkc > 0)
{
ec_slavecount = wkc;
b = 0x00;
ec_BWR(0x0000, ECT_REG_DLPORT, sizeof(b), &b, EC_TIMEOUTRET); /* deact loop manual */
w = htoes(0x0004);
ec_BWR(0x0000, ECT_REG_IRQMASK, sizeof(w), &w, EC_TIMEOUTRET); /* set IRQ mask */
ec_BWR(0x0000, ECT_REG_RXERR, 8, &zbuf, EC_TIMEOUTRET); /* reset CRC counters */
ec_BWR(0x0000, ECT_REG_FMMU0, 16 * 3, &zbuf, EC_TIMEOUTRET); /* reset FMMU's */
ec_BWR(0x0000, ECT_REG_SM0, 8 * 4, &zbuf, EC_TIMEOUTRET); /* reset SyncM */
ec_BWR(0x0000, ECT_REG_DCSYSTIME, 4, &zbuf, EC_TIMEOUTRET); /* reset system time+ofs */
w = htoes(0x1000);
ec_BWR(0x0000, ECT_REG_DCSPEEDCNT, sizeof(w), &w, EC_TIMEOUTRET); /* DC speedstart */
w = htoes(0x0c00);
ec_BWR(0x0000, ECT_REG_DCTIMEFILT, sizeof(w), &w, EC_TIMEOUTRET); /* DC filt expr */
b = 0x00;
ec_BWR(0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ec_BWR(0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET); /* Reset all slaves to Init */
b = 2;
ec_BWR(0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET); /* force Eeprom from PDI */
b = 0;
ec_BWR(0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET); /* set Eeprom to master */
for (slave = 1; slave <= ec_slavecount; slave++)
{
ADPh = (uint16)(1 - slave);
ec_slave[slave].Itype = etohs(ec_APRDw(ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET)); /* read interface type of slave */
/* a node offset is used to improve readibility of network frames */
/* this has no impact on the number of addressable slaves (auto wrap around) */
ec_APWRw(ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET); /* 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 */
}
ec_APWRw(ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET); /* set non ecat frame behaviour */
configadr = etohs(ec_APRDw(ADPh, ECT_REG_STADR, EC_TIMEOUTRET));
ec_slave[slave].configadr = configadr;
ec_FPRD(configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET);
estat = etohs(estat);
if ((estat & (1 << 6))) /* check if slave can read 8 byte chunks */
{
ec_slave[slave].eep_8byte = 1;
}
ec_readeeprom1(slave, ECT_SII_MANUF); /* Manuf */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_man = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* Manuf */
ec_readeeprom1(slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_id = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* ID */
ec_readeeprom1(slave, ECT_SII_REV); /* revision */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
ec_slave[slave].eep_rev = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* revision */
ec_readeeprom1(slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
eedat = etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */
ec_slave[slave].mbx_wo = (uint16)LO_WORD(eedat);
ec_slave[slave].mbx_l = (uint16)HI_WORD(eedat);
if (ec_slave[slave].mbx_l > 0)
{
ec_readeeprom1(slave, ECT_SII_TXMBXADR); /* read mailbox offset */
}
}
for (slave = 1; slave <= ec_slavecount; slave++)
{
if (ec_slave[slave].mbx_l > 0)
{
ec_slave[slave].mbx_ro = (uint16)etohl(ec_readeeprom2(slave, EC_TIMEOUTEEP)); /* read mailbox offset */
}
configadr = ec_slave[slave].configadr;
mbx_ro = ec_slave[slave].mbx_ro;
mbx_wo = ec_slave[slave].mbx_wo;
mbx_l = ec_slave[slave].mbx_l;
if ((etohs(ec_FPRDw(configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET)) & 0x04) > 0) /* Support DC? */
{
ec_slave[slave].hasdc = TRUE;
}
else
{
ec_slave[slave].hasdc = FALSE;
}
topology = etohs(ec_FPRDw(configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET)); /* extract topology from DL status */
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*/
ec_slave[slave].ptype = LO_BYTE(etohs(ec_FPRDw(configadr, ECT_REG_PORTDES, EC_TIMEOUTRET)));
ec_slave[slave].topology = h;
ec_slave[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 */
ec_slave[slave].parent = 0; /* parent is master */
if (slave > 1)
{
topoc = 0;
slavec = slave - 1;
do
{
topology = ec_slave[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 */
{
ec_slave[slave].parent = slavec;
slavec = 1;
}
slavec--;
}
while (slavec > 0);
}
w = ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */
/* set default mailbox configuration if slave has mailbox */
if (ec_slave[slave].mbx_l>0)
{
ec_slave[slave].SMtype[0] = 0;
ec_slave[slave].SMtype[1] = 1;
ec_slave[slave].SMtype[2] = 2;
ec_slave[slave].SMtype[3] = 3;
ec_slave[slave].SM[0].StartAddr = htoes(ec_slave[slave].mbx_wo);
ec_slave[slave].SM[0].SMlength = htoes(ec_slave[slave].mbx_l);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
ec_slave[slave].SM[1].StartAddr = htoes(ec_slave[slave].mbx_ro);
ec_slave[slave].SM[1].SMlength = htoes(ec_slave[slave].mbx_l);
ec_slave[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
ec_slave[slave].mbx_proto = ec_readeeprom (slave, ECT_SII_MBXPROTO, EC_TIMEOUTEEP);
}
cindex = 0;
/* use configuration table ? */
if (usetable)
{
cindex = ec_findconfig( ec_slave[slave].eep_man, ec_slave[slave].eep_id );
ec_slave[slave].configindex= cindex;
}
/* slave found in configuration table ? */
if (cindex)
{
ec_slave[slave].Dtype = ec_configlist[cindex].Dtype;
strcpy( ec_slave[slave].name ,ec_configlist[cindex].name);
ec_slave[slave].Ibits = ec_configlist[cindex].Ibits;
ec_slave[slave].Obits = ec_configlist[cindex].Obits;
if (ec_slave[slave].Obits)
{
ec_slave[slave].FMMU0func = 1;
}
if (ec_slave[slave].Ibits)
{
ec_slave[slave].FMMU1func = 2;
}
ec_slave[slave].FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
ec_slave[slave].FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
ec_slave[slave].SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
ec_slave[slave].SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
/* simple (no mailbox) output slave found ? */
if (ec_slave[slave].Obits && !ec_slave[slave].SM[2].StartAddr)
{
ec_slave[slave].SM[0].StartAddr = htoes(0x0f00);
ec_slave[slave].SM[0].SMlength = htoes((ec_slave[slave].Obits + 7) / 8);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
ec_slave[slave].FMMU[0].FMMUactive = 1;
ec_slave[slave].FMMU[0].FMMUtype = 2;
ec_slave[slave].SMtype[0] = 2;
}
/* complex output slave */
else
{
ec_slave[slave].SM[2].SMlength = htoes((ec_slave[slave].Obits + 7) / 8);
ec_slave[slave].SMtype[2] = 2;
}
ec_slave[slave].SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
ec_slave[slave].SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
/* simple (no mailbox) input slave found ? */
if (ec_slave[slave].Ibits && !ec_slave[slave].SM[3].StartAddr)
{
ec_slave[slave].SM[1].StartAddr = htoes(0x1000);
ec_slave[slave].SM[1].SMlength = htoes((ec_slave[slave].Ibits + 7) / 8);
ec_slave[slave].SM[1].SMflags = htoel(0x00000000);
ec_slave[slave].FMMU[1].FMMUactive = 1;
ec_slave[slave].FMMU[1].FMMUtype = 1;
ec_slave[slave].SMtype[1] = 3;
}
/* complex input slave */
else
{
ec_slave[slave].SM[3].SMlength = htoes((ec_slave[slave].Ibits + 7) / 8);
ec_slave[slave].SMtype[3] = 3;
}
}
/* slave not in configuration table, find out via SII */
else
{
ssigen = ec_siifind(slave, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
ec_slave[slave].CoEdetails = ec_siigetbyte(slave, ssigen + 0x07);
ec_slave[slave].FoEdetails = ec_siigetbyte(slave, ssigen + 0x08);
ec_slave[slave].EoEdetails = ec_siigetbyte(slave, ssigen + 0x09);
ec_slave[slave].SoEdetails = ec_siigetbyte(slave, ssigen + 0x0a);
if((ec_siigetbyte(slave, ssigen + 0x0d) & 0x02) > 0)
{
ec_slave[slave].blockLRW = 1;
ec_slave[0].blockLRW++;
}
ec_slave[slave].Ebuscurrent = ec_siigetbyte(slave, ssigen + 0x0e);
ec_slave[slave].Ebuscurrent += ec_siigetbyte(slave, ssigen + 0x0f) << 8;
ec_slave[0].Ebuscurrent += ec_slave[slave].Ebuscurrent;
}
/* SII strings section */
if (ec_siifind(slave, ECT_SII_STRING) > 0)
{
ec_siistring(ec_slave[slave].name, slave, 1);
}
/* no name for slave found, use constructed name */
else
{
sprintf(ec_slave[slave].name, "? M:%8.8x I:%8.8x",
(unsigned int)ec_slave[slave].eep_man, (unsigned int)ec_slave[slave].eep_id);
}
/* SII SM section */
nSM = ec_siiSM (slave,&ec_SM);
if (nSM>0)
{
ec_slave[slave].SM[0].StartAddr = htoes(ec_SM.PhStart);
ec_slave[slave].SM[0].SMlength = htoes(ec_SM.Plength);
ec_slave[slave].SM[0].SMflags = htoel((ec_SM.Creg) + (ec_SM.Activate << 16));
SMc = 1;
while ((SMc < EC_MAXSM) && ec_siiSMnext(slave, &ec_SM, SMc))
{
ec_slave[slave].SM[SMc].StartAddr = htoes(ec_SM.PhStart);
ec_slave[slave].SM[SMc].SMlength = htoes(ec_SM.Plength);
ec_slave[slave].SM[SMc].SMflags = htoel((ec_SM.Creg) + (ec_SM.Activate << 16));
SMc++;
}
}
/* SII FMMU section */
if (ec_siiFMMU(slave, &ec_FMMU))
{
if (ec_FMMU.FMMU0 !=0xff)
ec_slave[slave].FMMU0func = ec_FMMU.FMMU0;
if (ec_FMMU.FMMU1 !=0xff)
ec_slave[slave].FMMU1func = ec_FMMU.FMMU1;
if (ec_FMMU.FMMU2 !=0xff)
ec_slave[slave].FMMU2func = ec_FMMU.FMMU2;
if (ec_FMMU.FMMU3 !=0xff)
ec_slave[slave].FMMU3func = ec_FMMU.FMMU3;
}
}
if (ec_slave[slave].mbx_l > 0)
{
if (ec_slave[slave].SM[0].StartAddr == 0x0000) /* should never happen */
{
ec_slave[slave].SM[0].StartAddr = htoes(0x1000);
ec_slave[slave].SM[0].SMlength = htoes(0x0080);
ec_slave[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
ec_slave[slave].SMtype[0] = 0;
}
if (ec_slave[slave].SM[1].StartAddr == 0x0000) /* should never happen */
{
ec_slave[slave].SM[1].StartAddr = htoes(0x1080);
ec_slave[slave].SM[1].SMlength = htoes(0x0080);
ec_slave[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
ec_slave[slave].SMtype[1] = 1;
}
/* program SM0 mailbox in for slave */
ec_FPWR (configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET);
/* program SM1 mailbox out for slave */
// usleep(1000); // was needed for NETX (needs internal time after SM update)
ec_FPWR (configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET);
}
ec_FPWRw(configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP | EC_STATE_ACK) , EC_TIMEOUTRET); /* set preop status */
}
}
return wkc;
}
/** FoE read, blocking.
*
* @param[in] slave = Slave number.
* @param[in] filename = Filename of file to read.
* @param[in] password = password.
* @param[in,out] psize = Size in bytes of file buffer, returns bytes read from file.
* @param[out] p = Pointer to file buffer
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
* @return Workcounter from last slave response
*/
int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout)
{
ec_FOEt *FOEp, *aFOEp;
int wkc;
int32 dataread = 0;
int32 buffersize, packetnumber, prevpacket = 0;
uint16 fnsize, maxdata, segmentdata;
ec_mbxbuft MbxIn, MbxOut;
uint8 cnt;
boolean worktodo;
buffersize = *psize;
ec_clearmbx(&MbxIn);
/* Empty slave out mailbox if something is in. Timout set to 0 */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
ec_clearmbx(&MbxOut);
aFOEp = (ec_FOEt *)&MbxIn;
FOEp = (ec_FOEt *)&MbxOut;
fnsize = strlen(filename);
maxdata = ec_slave[slave].mbx_l - 12;
if (fnsize > maxdata)
fnsize = maxdata;
FOEp->MbxHeader.length = htoes(0x0006 + fnsize);
FOEp->MbxHeader.address = htoes(0x0000);
FOEp->MbxHeader.priority = 0x00;
/* get new mailbox count value, used as session handle */
cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
ec_slave[slave].mbx_cnt = cnt;
FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */
FOEp->OpCode = ECT_FOE_READ;
FOEp->Password = htoel(password);
/* copy filename in mailbox */
memcpy(&FOEp->FileName[0], filename, fnsize);
/* send FoE request to slave */
wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
do
{
worktodo = FALSE;
/* clean mailboxbuffer */
ec_clearmbx(&MbxIn);
/* read slave response */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
/* slave response should be FoE */
if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE)
{
if(aFOEp->OpCode == ECT_FOE_DATA)
{
segmentdata = etohs(aFOEp->MbxHeader.length) - 0x0006;
packetnumber = etohl(aFOEp->PacketNumber);
if ((packetnumber == ++prevpacket) && (dataread + segmentdata <= buffersize))
{
memcpy(p, &aFOEp->Data[0], segmentdata);
dataread += segmentdata;
p += segmentdata;
if (segmentdata == maxdata)
worktodo = TRUE;
FOEp->MbxHeader.length = htoes(0x0006);
FOEp->MbxHeader.address = htoes(0x0000);
FOEp->MbxHeader.priority = 0x00;
/* get new mailbox count value */
cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
ec_slave[slave].mbx_cnt = cnt;
FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */
FOEp->OpCode = ECT_FOE_ACK;
FOEp->PacketNumber = htoel(packetnumber);
/* send FoE ack to slave */
wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc <= 0)
worktodo = FALSE;
}
else
{
/* FoE error */
wkc = -EC_ERR_TYPE_FOE_BUF2SMALL;
}
}
else
if(aFOEp->OpCode == ECT_FOE_ERROR)
{
/* FoE error */
wkc = -EC_ERR_TYPE_FOE_ERROR;
}
else
{
/* unexpected mailbox received */
wkc = -EC_ERR_TYPE_PACKET_ERROR;
}
}
else
{
/* unexpected mailbox received */
wkc = -EC_ERR_TYPE_PACKET_ERROR;
}
*psize = dataread;
}
} while (worktodo);
}
return wkc;
}
/** Map all PDOs from slaves to IOmap.
*
* @param[out] pIOmap = pointer to IOmap
* @return IOmap size
*/
int ec_config_map(void *pIOmap)
{
uint16 slave, configadr;
int Isize, Osize, BitCount, ByteCount, FMMUsize, FMMUdone;
uint16 SMlength;
uint8 BitPos, EndAddr;
uint8 SMc, FMMUc;
uint32 LogAddr = 0;
int nSM, rval;
ec_eepromPDOt eepPDO;
if (ec_slavecount > 0)
{
LogAddr = 0;
BitPos = 0;
/* find output mapping of slave and program FMMU0 */
for (slave = 1; slave <= ec_slavecount; slave++)
{
configadr = ec_slave[slave].configadr;
ec_statecheck(slave, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); //* check state change pre-op */
/* if slave not found in configlist find IO mapping in slave self */
if (!ec_slave[slave].configindex)
{
Isize = 0;
Osize = 0;
if (ec_slave[slave].mbx_proto & ECT_MBXPROT_COE) /* has CoE */
{
if (ec_slave[slave].CoEdetails & ECT_COEDET_SDOCA) /* has Complete Access */
{
/* read PDO mapping via CoE and use Complete Access */
rval = ec_readPDOmapCA(slave, &Osize, &Isize);
}
else
{
/* read PDO mapping via CoE */
rval = ec_readPDOmap(slave, &Osize, &Isize);
}
ec_slave[slave].SM[2].SMlength = htoes((Osize + 7) / 8);
ec_slave[slave].SM[3].SMlength = htoes((Isize + 7) / 8);
}
if ((!Isize && !Osize) && (ec_slave[slave].mbx_proto & ECT_MBXPROT_SOE)) /* has SoE */
{
/* read AT / MDT mapping via SoE */
rval = ec_readIDNmap(slave, &Osize, &Isize);
ec_slave[slave].SM[2].SMlength = htoes((Osize + 7) / 8);
ec_slave[slave].SM[3].SMlength = htoes((Isize + 7) / 8);
}
if (!Isize && !Osize) /* find PDO mapping by SII */
{
Isize = ec_siiPDO (slave, &eepPDO, 0);
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
{
if (eepPDO.SMbitsize[nSM] > 0)
{
ec_slave[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8);
ec_slave[slave].SMtype[nSM] = 3;
}
}
Osize = ec_siiPDO (slave, &eepPDO, 1);
for( nSM=0 ; nSM < EC_MAXSM ; nSM++ )
{
if (eepPDO.SMbitsize[nSM] > 0)
{
ec_slave[slave].SM[nSM].SMlength = htoes((eepPDO.SMbitsize[nSM] + 7) / 8);
ec_slave[slave].SMtype[nSM] = 2;
}
}
}
ec_slave[slave].Obits = Osize;
ec_slave[slave].Ibits = Isize;
}
if (!ec_slave[slave].mbx_l && ec_slave[slave].SM[0].StartAddr)
{
ec_FPWR (configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET);
}
if (!ec_slave[slave].mbx_l && ec_slave[slave].SM[1].StartAddr)
{
ec_FPWR (configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET);
}
if (ec_slave[slave].SM[2].StartAddr)
{
/* check if SM length is zero -> clear enable flag */
if( ec_slave[slave].SM[2].SMlength == 0)
ec_slave[slave].SM[2].SMflags = htoel( etohl(ec_slave[slave].SM[2].SMflags) & EC_SMENABLEMASK);
ec_FPWR (configadr, ECT_REG_SM2, sizeof(ec_smt), &ec_slave[slave].SM[2], EC_TIMEOUTRET);
}
// usleep(1000); // was needed for NETX (needs internal time after SM update)
if (ec_slave[slave].SM[3].StartAddr)
{
/* check if SM length is zero -> clear enable flag */
if( ec_slave[slave].SM[3].SMlength == 0)
ec_slave[slave].SM[3].SMflags = htoel( etohl(ec_slave[slave].SM[3].SMflags) & EC_SMENABLEMASK);
ec_FPWR (configadr, ECT_REG_SM3, sizeof(ec_smt), &ec_slave[slave].SM[3], EC_TIMEOUTRET);
}
if (ec_slave[slave].Ibits > 7)
ec_slave[slave].Ibytes = (ec_slave[slave].Ibits + 7) / 8;
if (ec_slave[slave].Obits > 7)
ec_slave[slave].Obytes = (ec_slave[slave].Obits + 7) / 8;
FMMUc = 0;
SMc = 0;
BitCount = 0;
ByteCount = 0;
EndAddr = 0;
FMMUsize = 0;
FMMUdone = 0;
/* create output mapping */
if (ec_slave[slave].Obits)
{
/* search for SM that contribute to the output mapping */
while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((ec_slave[slave].Obits + 7) / 8)))
{
while ( (SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 2)) SMc++;
ec_slave[slave].FMMU[FMMUc].PhysStart = ec_slave[slave].SM[SMc].StartAddr;
SMlength = etohs(ec_slave[slave].SM[SMc].SMlength);
ByteCount += SMlength;
BitCount += SMlength * 8;
EndAddr = etohs(ec_slave[slave].SM[SMc].StartAddr) + SMlength;
while ( (BitCount < ec_slave[slave].Obits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for output */
{
SMc++;
while ( (SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 2)) SMc++;
/* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
if ( etohs(ec_slave[slave].SM[SMc].StartAddr) > EndAddr ) break;
SMlength = etohs(ec_slave[slave].SM[SMc].SMlength);
ByteCount += SMlength;
BitCount += SMlength * 8;
EndAddr = etohs(ec_slave[slave].SM[SMc].StartAddr) + SMlength;
}
/* bit oriented slave */
if (!ec_slave[slave].Obytes)
{
ec_slave[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
ec_slave[slave].FMMU[FMMUc].LogStartbit = BitPos;
BitPos += ec_slave[slave].Obits - 1;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
FMMUsize = LogAddr - etohl(ec_slave[slave].FMMU[FMMUc].LogStart) + 1;
ec_slave[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
ec_slave[slave].FMMU[FMMUc].LogEndbit = BitPos;
BitPos ++;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
}
/* byte oriented slave */
else
{
if (BitPos)
{
LogAddr++;
BitPos = 0;
}
ec_slave[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
ec_slave[slave].FMMU[FMMUc].LogStartbit = BitPos;
BitPos = 7;
FMMUsize = ByteCount;
if ((FMMUsize + FMMUdone)> ec_slave[slave].Obytes)
FMMUsize = ec_slave[slave].Obytes - FMMUdone;
LogAddr += FMMUsize;
ec_slave[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
ec_slave[slave].FMMU[FMMUc].LogEndbit = BitPos;
BitPos = 0;
}
FMMUdone += FMMUsize;
ec_slave[slave].FMMU[FMMUc].PhysStartBit = 0;
ec_slave[slave].FMMU[FMMUc].FMMUtype = 2;
ec_slave[slave].FMMU[FMMUc].FMMUactive = 1;
/* program FMMU for output */
ec_FPWR (configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), sizeof(ec_fmmut),
&ec_slave[slave].FMMU[FMMUc], EC_TIMEOUTRET);
if (!ec_slave[slave].outputs)
{
ec_slave[slave].outputs = pIOmap + etohl(ec_slave[slave].FMMU[FMMUc].LogStart);
ec_slave[slave].Ostartbit = ec_slave[slave].FMMU[FMMUc].LogStartbit;
}
FMMUc++;
}
}
}
ec_slave[0].outputs = pIOmap;
if (BitPos)
{
LogAddr++;
BitPos = 0;
}
ec_slave[0].Obytes = LogAddr; /* store output bytes in master record */
/* do input mapping of slave and program FMMUs */
for (slave = 1; slave <= ec_slavecount; slave++)
{
configadr = ec_slave[slave].configadr;
FMMUc = 1;
if (ec_slave[slave].Obits) /* find free FMMU */
{
while ( ec_slave[slave].FMMU[FMMUc].LogStart ) FMMUc++;
}
SMc = 0;
BitCount = 0;
ByteCount = 0;
EndAddr = 0;
FMMUsize = 0;
FMMUdone = 0;
/* create input mapping */
if (ec_slave[slave].Ibits)
{
/* search for SM that contribute to the input mapping */
while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((ec_slave[slave].Ibits + 7) / 8)))
{
while ( (SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 3)) SMc++;
ec_slave[slave].FMMU[FMMUc].PhysStart = ec_slave[slave].SM[SMc].StartAddr;
SMlength = etohs(ec_slave[slave].SM[SMc].SMlength);
ByteCount += SMlength;
BitCount += SMlength * 8;
EndAddr = etohs(ec_slave[slave].SM[SMc].StartAddr) + SMlength;
while ( (BitCount < ec_slave[slave].Obits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for input */
{
SMc++;
while ( (SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 3)) SMc++;
/* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
if ( etohs(ec_slave[slave].SM[SMc].StartAddr) > EndAddr ) break;
SMlength = etohs(ec_slave[slave].SM[SMc].SMlength);
ByteCount += SMlength;
BitCount += SMlength * 8;
EndAddr = etohs(ec_slave[slave].SM[SMc].StartAddr) + SMlength;
}
/* bit oriented slave */
if (!ec_slave[slave].Ibytes)
{
ec_slave[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
ec_slave[slave].FMMU[FMMUc].LogStartbit = BitPos;
BitPos += ec_slave[slave].Ibits - 1;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
FMMUsize = LogAddr - etohl(ec_slave[slave].FMMU[FMMUc].LogStart) + 1;
ec_slave[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
ec_slave[slave].FMMU[FMMUc].LogEndbit = BitPos;
BitPos ++;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
}
/* byte oriented slave */
else
{
if (BitPos)
{
LogAddr++;
BitPos = 0;
}
ec_slave[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
ec_slave[slave].FMMU[FMMUc].LogStartbit = BitPos;
BitPos = 7;
FMMUsize = ByteCount;
if ((FMMUsize + FMMUdone)> ec_slave[slave].Ibytes)
FMMUsize = ec_slave[slave].Ibytes - FMMUdone;
LogAddr += FMMUsize;
ec_slave[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
ec_slave[slave].FMMU[FMMUc].LogEndbit = BitPos;
BitPos = 0;
}
FMMUdone += FMMUsize;
if (ec_slave[slave].FMMU[FMMUc].LogLength)
{
ec_slave[slave].FMMU[FMMUc].PhysStartBit = 0;
ec_slave[slave].FMMU[FMMUc].FMMUtype = 1;
ec_slave[slave].FMMU[FMMUc].FMMUactive = 1;
/* program FMMU for input */
ec_FPWR (configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc), sizeof(ec_fmmut),
&ec_slave[slave].FMMU[FMMUc], EC_TIMEOUTRET);
}
if (!ec_slave[slave].inputs)
{
ec_slave[slave].inputs = pIOmap + etohl(ec_slave[slave].FMMU[FMMUc].LogStart);
ec_slave[slave].Istartbit = ec_slave[slave].FMMU[FMMUc].LogStartbit;
}
FMMUc++;
}
}
ec_eeprom2pdi(slave); /* set Eeprom control to PDI */
ec_FPWRw(configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP) , EC_TIMEOUTRET); /* set safeop status */
}
ec_slave[0].inputs = pIOmap + ec_slave[0].Obytes;
if (BitPos)
{
LogAddr++;
BitPos = 0;
}
ec_slave[0].Ibytes = LogAddr - ec_slave[0].Obytes; /* store input bytes in master record */
}
return LogAddr;
}
/** Read Configured Station Address register 0x010 assigned by the Master.
*
*/
void ESC_address (void)
{
ESC_read (ESCREG_ADDRESS, (void *) &ESCvar.address, sizeof (ESCvar.address),
(void *) &ESCvar.ALevent);
ESCvar.address = etohs (ESCvar.address);
}
/** Non blocking receive frame function. Uses RX buffer and index to combine
* read frame with transmitted frame. To compensate for received frames that
* are out-of-order all frames are stored in their respective indexed buffer.
* If a frame was placed in the buffer previously, the function retreives it
* from that buffer index without calling ec_recvpkt. If the requested index
* is not already in the buffer it calls ec_recvpkt to fetch it. There are
* three options now, 1 no frame read, so exit. 2 frame read but other
* than requested index, store in buffer and exit. 3 frame read with matching
* index, store in buffer, set completed flag in buffer status and exit.
*
* @param[in] port = port context struct
* @param[in] idx = requested index of frame
* @param[in] stacknumber = 0=primary 1=secondary stack
* @return Workcounter if a frame is found with corresponding index, otherwise
* EC_NOFRAME or EC_OTHERFRAME.
*/
int ecx_inframe(ecx_portt *port, int idx, int stacknumber)
{
uint16 l;
int rval;
uint8 idxf;
ec_etherheadert *ehp;
ec_comt *ecp;
ec_stackT *stack;
ec_bufT *rxbuf;
if (!stacknumber)
{
stack = &(port->stack);
}
else
{
stack = &(port->redport->stack);
}
rval = EC_NOFRAME;
rxbuf = &(*stack->rxbuf)[idx];
/* check if requested index is already in buffer ? */
if ((idx < EC_MAXBUF) && ( (*stack->rxbufstat)[idx] == EC_BUF_RCVD))
{
l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8);
/* return WKC */
rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8));
/* mark as completed */
(*stack->rxbufstat)[idx] = EC_BUF_COMPLETE;
}
else
{
mtx_lock (port->rx_mutex);
/* non blocking call to retrieve frame from socket */
if (ecx_recvpkt(port, stacknumber))
{
rval = EC_OTHERFRAME;
ehp =(ec_etherheadert*)(stack->tempbuf);
/* check if it is an EtherCAT frame */
if (ehp->etype == oshw_htons(ETH_P_ECAT))
{
ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]);
l = etohs(ecp->elength) & 0x0fff;
idxf = ecp->index;
/* found index equals reqested index ? */
if (idxf == idx)
{
/* yes, put it in the buffer array (strip ethernet header) */
memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE);
/* return WKC */
rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8));
/* mark as completed */
(*stack->rxbufstat)[idx] = EC_BUF_COMPLETE;
/* store MAC source word 1 for redundant routing info */
(*stack->rxsa)[idx] = oshw_ntohs(ehp->sa1);
}
else
{
/* check if index exist and someone is waiting for it */
if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX)
{
rxbuf = &(*stack->rxbuf)[idxf];
/* put it in the buffer array (strip ethernet header) */
memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE);
/* mark as received */
(*stack->rxbufstat)[idxf] = EC_BUF_RCVD;
(*stack->rxsa)[idxf] = oshw_ntohs(ehp->sa1);
}
else
{
/* strange things happend */
}
}
}
}
mtx_unlock (port->rx_mutex);
}
/* WKC if mathing frame found */
return rval;
}
/** SoE read, blocking.
*
* The IDN object of the selected slave and DriveNo is read. If a response
* is larger than the mailbox size then the response is segmented. The function
* will combine all segments and copy them to the parameter buffer.
*
* @param[in] slave = Slave number
* @param[in] driveNo = Drive number in slave
* @param[in] elementflags = Flags to select what properties of IDN are to be transfered.
* @param[in] idn = IDN.
* @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE.
* @param[out] p = Pointer to parameter buffer
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
* @return Workcounter from last slave response
*/
int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
{
ec_SoEt *SoEp, *aSoEp;
uint16 totalsize, framedatasize;
int wkc;
uint8 *bp;
uint8 *mp;
uint16 *errorcode;
ec_mbxbuft MbxIn, MbxOut;
uint8 cnt;
boolean NotLast;
ec_clearmbx(&MbxIn);
/* Empty slave out mailbox if something is in. Timeout set to 0 */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
ec_clearmbx(&MbxOut);
aSoEp = (ec_SoEt *)&MbxIn;
SoEp = (ec_SoEt *)&MbxOut;
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
SoEp->MbxHeader.address = htoes(0x0000);
SoEp->MbxHeader.priority = 0x00;
/* get new mailbox count value, used as session handle */
cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
ec_slave[slave].mbx_cnt = cnt;
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
SoEp->opCode = ECT_SOE_READREQ;
SoEp->incomplete = 0;
SoEp->error = 0;
SoEp->driveNo = driveNo;
SoEp->elementflags = elementflags;
SoEp->idn = htoes(idn);
totalsize = 0;
bp = p;
mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
NotLast = TRUE;
/* send SoE request to slave */
wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
while (NotLast)
{
/* clean mailboxbuffer */
ec_clearmbx(&MbxIn);
/* read slave response */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
/* slave response should be SoE, ReadRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 0) &&
(aSoEp->driveNo == driveNo) &&
(aSoEp->elementflags == elementflags))
{
framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
totalsize += framedatasize;
/* Does parameter fit in parameter buffer ? */
if (totalsize <= *psize)
{
/* copy parameter data in parameter buffer */
memcpy(bp, mp, framedatasize);
/* increment buffer pointer */
bp += framedatasize;
}
else
{
framedatasize -= totalsize - *psize;
totalsize = *psize;
/* copy parameter data in parameter buffer */
if (framedatasize > 0) memcpy(bp, mp, framedatasize);
}
if (!aSoEp->incomplete)
{
NotLast = FALSE;
*psize = totalsize;
}
}
/* other slave response */
else
{
NotLast = FALSE;
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ec_SoEerror(slave, idn, *errorcode);
}
else
{
ec_packeterror(slave, idn, 0, 1); /* Unexpected frame returned */
}
wkc = 0;
}
}
else
{
NotLast = FALSE;
ec_packeterror(slave, idn, 0, 4); /* no response */
}
}
}
return wkc;
}
/** SoE write, blocking.
*
* The IDN object of the selected slave and DriveNo is written. If a response
* is larger than the mailbox size then the response is segmented.
*
* @param[in] slave = Slave number
* @param[in] driveNo = Drive number in slave
* @param[in] elementflags = Flags to select what properties of IDN are to be transfered.
* @param[in] idn = IDN.
* @param[in] psize = Size in bytes of parameter buffer.
* @param[out] p = Pointer to parameter buffer
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
* @return Workcounter from last slave response
*/
int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
{
ec_SoEt *SoEp, *aSoEp;
uint16 framedatasize, maxdata;
int wkc;
uint8 *mp;
uint8 *hp;
uint16 *errorcode;
ec_mbxbuft MbxIn, MbxOut;
uint8 cnt;
boolean NotLast;
ec_clearmbx(&MbxIn);
/* Empty slave out mailbox if something is in. Timeout set to 0 */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
ec_clearmbx(&MbxOut);
aSoEp = (ec_SoEt *)&MbxIn;
SoEp = (ec_SoEt *)&MbxOut;
SoEp->MbxHeader.address = htoes(0x0000);
SoEp->MbxHeader.priority = 0x00;
SoEp->opCode = ECT_SOE_WRITEREQ;
SoEp->error = 0;
SoEp->driveNo = driveNo;
SoEp->elementflags = elementflags;
hp = p;
mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
maxdata = ec_slave[slave].mbx_l - sizeof(ec_SoEt);
NotLast = TRUE;
while (NotLast)
{
framedatasize = psize;
NotLast = FALSE;
SoEp->idn = htoes(idn);
SoEp->incomplete = 0;
if (framedatasize > maxdata)
{
framedatasize = maxdata; /* segmented transfer needed */
NotLast = TRUE;
SoEp->incomplete = 1;
SoEp->fragmentsleft = psize / maxdata;
}
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
/* get new mailbox counter, used for session handle */
cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
ec_slave[slave].mbx_cnt = cnt;
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
/* copy parameter data to mailbox */
memcpy(mp, hp, framedatasize);
hp += framedatasize;
psize -= framedatasize;
/* send SoE request to slave */
wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
if (!NotLast || !ec_mbxempty(slave, timeout))
{
/* clean mailboxbuffer */
ec_clearmbx(&MbxIn);
/* read slave response */
wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
NotLast = FALSE;
/* slave response should be SoE, WriteRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_WRITERES) &&
(aSoEp->error == 0) &&
(aSoEp->driveNo == driveNo) &&
(aSoEp->elementflags == elementflags))
{
/* SoE write succeeded */
}
/* other slave response */
else
{
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ec_SoEerror(slave, idn, *errorcode);
}
else
{
ec_packeterror(slave, idn, 0, 1); /* Unexpected frame returned */
}
wkc = 0;
}
}
else
{
ec_packeterror(slave, idn, 0, 4); /* no response */
}
}
}
}
return wkc;
}
/** Read PDO assign structure */
int si_PDOassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset)
{
uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
uint8 subcnt;
int wkc, bsize = 0, rdl;
int32 rdat2;
uint8 bitlen, obj_subidx;
uint16 obj_idx;
int abs_offset, abs_bit;
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign subindex 0 ( = number of PDO's) */
wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
rdat = etohs(rdat);
/* positive result from slave ? */
if ((wkc > 0) && (rdat > 0))
{
/* number of available sub indexes */
nidx = rdat;
bsize = 0;
/* read all PDO's */
for (idxloop = 1; idxloop <= nidx; idxloop++)
{
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign */
wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
/* result is index of PDO */
idx = etohl(rdat);
if (idx > 0)
{
rdl = sizeof(subcnt); subcnt = 0;
/* read number of subindexes of PDO */
wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
subidx = subcnt;
/* for each subindex */
for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
{
rdl = sizeof(rdat2); rdat2 = 0;
/* read SDO that is mapped in PDO */
wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
rdat2 = etohl(rdat2);
/* extract bitlength of SDO */
bitlen = LO_BYTE(rdat2);
bsize += bitlen;
obj_idx = (uint16)(rdat2 >> 16);
obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff);
abs_offset = mapoffset + (bitoffset / 8);
abs_bit = bitoffset % 8;
ODlist.Slave = slave;
ODlist.Index[0] = obj_idx;
OElist.Entries = 0;
wkc = 0;
/* read object entry from dictionary if not a filler (0x0000:0x00) */
if(obj_idx || obj_subidx)
wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist);
printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen);
if((wkc > 0) && OElist.Entries)
{
printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx]), OElist.Name[obj_subidx]);
}
else
printf("\n");
bitoffset += bitlen;
};
};
};
/** Map all PDOs in one group of slaves to IOmap.
*
* @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;
int Isize, Osize, BitCount, ByteCount, FMMUsize, FMMUdone;
uint16 SMlength, EndAddr;
uint8 BitPos;
uint8 SMc, FMMUc;
uint32 LogAddr = 0;
uint32 oLogAddr = 0;
uint32 diff;
int nSM, rval;
ec_eepromPDOt eepPDO;
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 output mapping of slave and program FMMU */
for (slave = 1; slave <= *(context->slavecount); slave++)
{
configadr = context->slavelist[slave].configadr;
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 (!group || (group == context->slavelist[slave].group))
{
/* 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, &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:%d Isize:%d\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((Osize + 7) / 8);
context->slavelist[slave].SM[3].SMlength = htoes((Isize + 7) / 8);
EC_PRINT(" SoE Osize:%d Isize:%d\n", Osize, Isize);
}
if (!Isize && !Osize) /* find PDO mapping by SII */
{
memset(&eepPDO, 0, sizeof(eepPDO));
Isize = (int)ecx_siiPDO(context, slave, &eepPDO, 0);
EC_PRINT(" SII Isize:%d\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 = (int)ecx_siiPDO(context, slave, &eepPDO, 1);
EC_PRINT(" SII Osize:%d\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 = Osize;
context->slavelist[slave].Ibits = Isize;
EC_PRINT(" ISIZE:%d %d OSIZE:%d\n",
context->slavelist[slave].Ibits, Isize,context->slavelist[slave].Obits);
}
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],
context->slavelist[slave].SM[0].StartAddr,
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],
context->slavelist[slave].SM[1].StartAddr,
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);
}
ecx_FPWR(context->port, configadr, 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],
context->slavelist[slave].SM[nSM].StartAddr,
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;
}
FMMUc = context->slavelist[slave].FMMUunused;
SMc = 0;
BitCount = 0;
ByteCount = 0;
EndAddr = 0;
FMMUsize = 0;
FMMUdone = 0;
/* create output mapping */
if (context->slavelist[slave].Obits)
{
EC_PRINT(" OUTPUT MAPPING\n");
/* 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 mutiple 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++;
BitPos -= 8;
}
FMMUsize = 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 ++;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
}
/* byte oriented slave */
else
{
if (BitPos)
{
LogAddr++;
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 = 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;
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);
context->grouplist[group].outputsWKC++;
if (!context->slavelist[slave].outputs)
{
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;
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].nsegments = currentsegment + 1;
context->grouplist[group].Isegment = currentsegment;
context->grouplist[group].Ioffset = segmentsize;
if (!group)
{
context->slavelist[0].outputs = pIOmap;
context->slavelist[0].Obytes = LogAddr; /* 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;
FMMUc = context->slavelist[slave].FMMUunused;
if (context->slavelist[slave].Obits) /* find free FMMU */
{
while ( context->slavelist[slave].FMMU[FMMUc].LogStart ) FMMUc++;
}
SMc = 0;
BitCount = 0;
ByteCount = 0;
EndAddr = 0;
FMMUsize = 0;
FMMUdone = 0;
/* create input mapping */
if (context->slavelist[slave].Ibits)
{
EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
/* 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 mutiple 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++;
BitPos -= 8;
}
FMMUsize = 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 ++;
if (BitPos > 7)
{
LogAddr++;
BitPos -= 8;
}
}
/* byte oriented slave */
else
{
if (BitPos)
{
LogAddr++;
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 = 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);
/* add one for an input FMMU */
context->grouplist[group].inputsWKC++;
}
if (!context->slavelist[slave].inputs)
{
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;
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 */
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].Obytes;
if (!group)
{
context->slavelist[0].inputs = (uint8 *)(pIOmap) + context->slavelist[0].Obytes;
context->slavelist[0].Ibytes = LogAddr - 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;
}
/** 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 w, slave, ADPh, configadr, ssigen;
uint16 topology, estat;
int16 topoc, slavec, aliasadr;
uint8 b,h;
uint8 zbuf[64];
uint8 SMc;
uint32 eedat;
int wkc, cindex, nSM, lp;
EC_PRINT("ec_config_init %d\n",usetable);
*(context->slavecount) = 0;
/* clean ec_slave array */
memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave);
memset(&zbuf, 0x00, sizeof(zbuf));
memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup);
/* clear slave eeprom cache */
ecx_siigetbyte(context, 0, EC_MAXEEPBUF);
for(lp = 0; lp < context->maxgroup; lp++)
{
context->grouplist[lp].logstartaddr = lp << 16; /* default start address per group entry */
}
/* 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 */
wkc = ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
printf("wkc = %d\n",wkc);
w = 0x0000;
wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
if (wkc > 0)
{
*(context->slavecount) = wkc;
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 */
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 */
for (slave = 1; slave <= *(context->slavecount); slave++)
{
ADPh = (uint16)(1 - slave);
context->slavelist[slave].Itype =
etohs(ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3)); /* read interface type of slave */
/* a node offset is used to improve readibility 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 = etohs(ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3));
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++)
{
context->slavelist[slave].eep_man =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* Manuf */
ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_id =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* ID */
ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_rev =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* revision */
ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */
context->slavelist[slave].mbx_wo = (uint16)LO_WORD(eedat);
context->slavelist[slave].mbx_l = (uint16)HI_WORD(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 = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* read mailbox offset */
context->slavelist[slave].mbx_ro = (uint16)LO_WORD(eedat); /* read mailbox offset */
context->slavelist[slave].mbx_rl = (uint16)HI_WORD(eedat); /*read mailbox length */
if (context->slavelist[slave].mbx_rl == 0)
{
context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l;
}
}
configadr = context->slavelist[slave].configadr;
if ((etohs(ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3)) & 0x04) > 0) /* Support DC? */
{
context->slavelist[slave].hasdc = TRUE;
}
else
{
context->slavelist[slave].hasdc = FALSE;
}
topology = etohs(ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3)); /* extract topology from DL status */
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*/
context->slavelist[slave].ptype =
LO_BYTE(etohs(ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3)));
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);
}
w = 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);
context->slavelist[slave].mbx_proto =
ecx_readeeprom(context, slave, ECT_SII_MBXPROTO, EC_TIMEOUTEEP);
}
cindex = 0;
#ifdef EC_VER1
/* use configuration table ? */
if (usetable)
{
cindex = ec_findconfig( context->slavelist[slave].eep_man, context->slavelist[slave].eep_id );
context->slavelist[slave].configindex= cindex;
}
/* slave found in configuration table ? */
if (cindex)
{
context->slavelist[slave].Dtype = ec_configlist[cindex].Dtype;
strcpy(context->slavelist[slave].name ,ec_configlist[cindex].name);
context->slavelist[slave].Ibits = ec_configlist[cindex].Ibits;
context->slavelist[slave].Obits = ec_configlist[cindex].Obits;
if (context->slavelist[slave].Obits)
{
context->slavelist[slave].FMMU0func = 1;
}
if (context->slavelist[slave].Ibits)
{
context->slavelist[slave].FMMU1func = 2;
}
context->slavelist[slave].FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
context->slavelist[slave].FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
context->slavelist[slave].SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
context->slavelist[slave].SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
/* simple (no mailbox) output slave found ? */
if (context->slavelist[slave].Obits && !context->slavelist[slave].SM[2].StartAddr)
{
context->slavelist[slave].SM[0].StartAddr = htoes(0x0f00);
context->slavelist[slave].SM[0].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
context->slavelist[slave].FMMU[0].FMMUactive = 1;
context->slavelist[slave].FMMU[0].FMMUtype = 2;
context->slavelist[slave].SMtype[0] = 3;
}
/* complex output slave */
else
{
context->slavelist[slave].SM[2].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SMtype[2] = 3;
}
context->slavelist[slave].SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
context->slavelist[slave].SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
/* simple (no mailbox) input slave found ? */
if (context->slavelist[slave].Ibits && !context->slavelist[slave].SM[3].StartAddr)
{
context->slavelist[slave].SM[1].StartAddr = htoes(0x1000);
context->slavelist[slave].SM[1].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SM[1].SMflags = htoel(0x00000000);
context->slavelist[slave].FMMU[1].FMMUactive = 1;
context->slavelist[slave].FMMU[1].FMMUtype = 1;
context->slavelist[slave].SMtype[1] = 4;
}
/* complex input slave */
else
{
context->slavelist[slave].SM[3].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SMtype[3] = 4;
}
}
/* slave not in configuration table, find out via SII */
else
#endif
{
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);
}
/* 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;
}
/** The state handler acting on ALControl Bit(0) and SyncManager Activation BIT(4)
* events in the Al Event Request register 0x220.
*
*/
void ESC_state (void)
{
uint8_t ac, an, as, ax, ax23;
uint8_t handle_smchanged = 0;
/* Do we have a state change request pending */
if (ESCvar.ALevent & ESCREG_ALEVENT_CONTROL)
{
ESC_read (ESCREG_ALCONTROL, (void *) &ESCvar.ALcontrol,
sizeof (ESCvar.ALcontrol), (void *) &ESCvar.ALevent);
ESCvar.ALcontrol = etohs (ESCvar.ALcontrol);
}
/* Have at least on Sync Manager changed */
else if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
handle_smchanged = 1;
}
else
{
/* nothing to do */
return;
}
/* Mask state request bits + Error ACK */
ac = ESCvar.ALcontrol & ESCREG_AL_STATEMASK;
as = ESCvar.ALstatus & ESCREG_AL_STATEMASK;
an = as;
if (((ac & ESCerror) || (ac == ESCinit)))
{
/* if error bit confirmed reset */
ac &= ESCREG_AL_ERRACKMASK;
an &= ESCREG_AL_ERRACKMASK;
}
/* Enter SM changed handling for all steps but Init and Boot when Mailboxes
* is up and running
*/
if (handle_smchanged && (as & ESCREG_AL_ALLBUTINITMASK) &&
!(as == ESCboot) && MBXrun)
{
/* Validate Sync Managers, reading the Activation register will
* acknowledge the SyncManager Activation event making us enter
* this execution path.
*/
ax = ESC_checkmbx (as);
ax23 = ESC_checkSM23 (as);
if ((an & ESCerror) && !(ac & ESCerror))
{
/* if in error then stay there */
return;
}
/* Have we been forced to step down to INIT we will stop mailboxes,
* update AL Status Code and exit ESC_state
*/
if (ax == (ESCinit | ESCerror))
{
/* If we have activated Inputs and Outputs we need to disable them */
if (App.state)
{
ESC_stopoutput ();
ESC_stopinput ();
}
/* Stop mailboxes and update ALStatus code */
ESC_stopmbx ();
ESC_ALerror (ALERR_INVALIDMBXCONFIG);
MBXrun = 0;
ESC_ALstatus (ax);
return;
}
/* Have we been forced to step down to PREOP we will stop inputs
* and outputs, update AL Status Code and exit ESC_state
*/
if ((App.state) && (ax23 == (ESCpreop | ESCerror)))
{
ESC_stopoutput ();
ESC_stopinput ();
if (ESCvar.SMtestresult & SMRESULT_ERRSM3)
{
ESC_ALerror (ALERR_INVALIDINPUTSM);
}
else
{
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
ESC_ALstatus (ax23);
return;
}
}
/* Error state not acked, leave original */
if ((an & ESCerror) && !(ac & ESCerror))
{
return;
}
/* Mask high bits ALcommand, low bits ALstatus */
as = (ac << 4) | (as & 0x0f);
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->pre_state_change_hook)
{
esc_cfg->pre_state_change_hook (&as, &an);
}
/* Switch through the state change requested via AlControl from
* current state read in AL status
*/
switch (as)
{
case INIT_TO_INIT:
case PREOP_TO_PREOP:
case OP_TO_OP:
{
break;
}
case INIT_TO_PREOP:
{
/* get station address */
ESC_address ();
an = ESC_startmbx (ac);
break;
}
case INIT_TO_BOOT:
case BOOT_TO_BOOT:
{
/* get station address */
ESC_address ();
an = ESC_startmbxboot (ac);
break;
}
case INIT_TO_SAFEOP:
case INIT_TO_OP:
{
an = ESCinit | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_INIT:
{
ESC_stopoutput ();
}
case SAFEOP_TO_INIT:
{
ESC_stopinput ();
}
case PREOP_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case BOOT_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case PREOP_TO_BOOT:
case BOOT_TO_PREOP:
case BOOT_TO_SAFEOP:
case BOOT_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case PREOP_TO_SAFEOP:
case SAFEOP_TO_SAFEOP:
{
SM2_sml = sizeRXPDO ();
SM3_sml = sizeTXPDO ();
an = ESC_startinput (ac);
if (an == ac)
{
ESC_SMenable (2);
}
break;
}
case PREOP_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_PREOP:
{
ESC_stopoutput ();
}
case SAFEOP_TO_PREOP:
{
ESC_stopinput ();
an = ESCpreop;
break;
}
case SAFEOP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case SAFEOP_TO_OP:
{
an = ESC_startoutput (ac);
break;
}
case OP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
ESC_stopoutput ();
break;
}
case OP_TO_SAFEOP:
{
an = ESCsafeop;
ESC_stopoutput ();
break;
}
default:
{
if (an == ESCop)
{
ESC_stopoutput ();
an = ESCsafeop;
}
if (as == ESCsafeop)
{
ESC_stopinput ();
}
an |= ESCerror;
ESC_ALerror (ALERR_UNKNOWNSTATE);
break;
}
}
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->post_state_change_hook)
{
esc_cfg->post_state_change_hook (&as, &an);
}
if (!(an & ESCerror) && (ESCvar.ALerror))
{
/* clear error */
ESC_ALerror (ALERR_NONE);
}
ESC_ALstatus (an);
}
/** Mailbox routine for implementing the low-level part of the mailbox protocol
* used by Application Layers running on-top of mailboxes. It takes care of sending
* a mailbox, re-sending a mailbox, reading a mailbox and handles a mailbox full event.
*
* @return =0 if nothing to do. =1 if something to be handled by mailbox protocols.
*/
uint8_t ESC_mbxprocess (void)
{
uint8_t mbxhandle = 0;
_MBX *MB = &MBX[0];
if (!MBXrun)
{
/* nothing to do */
return 0;
}
/* SM0/1 access or SMn change event */
if (ESCvar.ALevent & ESCREG_ALEVENT_SM_MASK)
{
ESC_SMstatus (0);
ESC_SMstatus (1);
}
/* outmbx read by master */
if (ESCvar.mbxoutpost && ESCvar.SM[1].IntR)
{
ESC_ackmbxread ();
/* dispose old backup */
if (ESCvar.mbxbackup)
{
MBXcontrol[ESCvar.mbxbackup].state = MBXstate_idle;
}
/* if still to do */
if (MBXcontrol[ESCvar.mbxoutpost].state == MBXstate_again)
{
ESC_writembx (ESCvar.mbxoutpost);
}
/* create new backup */
MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_backup;
ESCvar.mbxbackup = ESCvar.mbxoutpost;
ESCvar.mbxoutpost = 0;
return 0;
}
/* repeat request */
if (ESCvar.SM[1].ECrep != ESCvar.toggle)
{
if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
{
/* if outmbx empty */
if (!ESCvar.mbxoutpost)
{
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
}
else
{
/* reset mailbox */
ESC_SMdisable (1);
/* have to resend later */
MBXcontrol[ESCvar.mbxoutpost].state = MBXstate_again;
/* activate mailbox */
ESC_SMenable (1);
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
}
ESCvar.toggle = ESCvar.SM[1].ECrep;
ESCvar.SM[1].PDIrep = ESCvar.toggle;
ESC_SMwritepdi (1);
}
return 0;
}
/* if the outmailbox is free check if we have something to send */
if (ESCvar.txcue && (ESCvar.mbxfree || !ESCvar.SM[1].MBXstat))
{
/* check out request mbx */
mbxhandle = ESC_outreqbuffer ();
/* outmbx empty and outreq mbx available */
if (mbxhandle)
{
ESC_writembx (mbxhandle);
/* change state */
MBXcontrol[mbxhandle].state = MBXstate_outpost;
ESCvar.mbxoutpost = mbxhandle;
if (ESCvar.txcue)
{
ESCvar.txcue--;
}
}
}
/* read mailbox if full and no xoe in progress */
if (ESCvar.SM[0].MBXstat && !MBXcontrol[0].state && !ESCvar.mbxoutpost
&& !ESCvar.xoe)
{
ESC_readmbx ();
ESCvar.SM[0].MBXstat = 0;
if (etohs (MB->header.length) == 0)
{
MBX_error (MBXERR_INVALIDHEADER);
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
if ((MB->header.mbxcnt != 0) && (MB->header.mbxcnt == ESCvar.mbxincnt))
{
/* drop mailbox */
MBXcontrol[0].state = MBXstate_idle;
}
ESCvar.mbxincnt = MB->header.mbxcnt;
return 1;
}
/* ack changes in non used SM */
if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
ESC_SMack (4);
ESC_SMack (5);
ESC_SMack (6);
ESC_SMack (7);
}
return 0;
}
