/** 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;
}
/** Map all PDOs in one group of slaves to IOmap.
*
* @param[out] pIOmap = pointer to IOmap
* @param[in] group = group to map, 0 = all groups
* @return IOmap size
*/
int ec_config_map_group(void *pIOmap, uint8 group)
{
uint16 slave, configadr;
int Isize, Osize, BitCount, ByteCount, FMMUsize, FMMUdone;
uint16 SMlength;
uint8 BitPos, EndAddr;
uint8 SMc, FMMUc;
uint32 LogAddr = 0;
uint32 oLogAddr = 0;
uint32 diff;
int nSM; //, rval;
ec_eepromPDOt eepPDO;
uint16 currentsegment = 0;
uint32 segmentsize = 0;
if ((ec_slavecount > 0) && (group < EC_MAXGROUP))
{
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
LogAddr = ec_group[group].logstartaddr;
oLogAddr = LogAddr;
BitPos = 0;
ec_group[group].nsegments = 0;
ec_group[group].expectedWKC = 0;
/* find output mapping of slave and program FMMU */
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 */
EC_PRINT(" >Slave %d, configadr %x, state %2.2x\n", slave, ec_slave[slave].configadr, ec_slave[slave].state);
if (!group || (group == ec_slave[slave].group))
{
/* 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_PRINT(" CoE Osize:%d Isize:%d\n", Osize, Isize);
}
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);
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)ec_siiPDO(slave, &eepPDO, 0);
EC_PRINT(" SII Isize:%d\n", Isize);
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] = 4;
EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]);
}
}
Osize = (int)ec_siiPDO(slave, &eepPDO, 1);
EC_PRINT(" SII Osize:%d\n", Osize);
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;
EC_PRINT(" SM%d length %d\n", nSM, eepPDO.SMbitsize[nSM]);
}
}
}
ec_slave[slave].Obits = Osize;
ec_slave[slave].Ibits = Isize;
EC_PRINT(" ISIZE:%d %d OSIZE:%d\n", ec_slave[slave].Ibits, Isize, ec_slave[slave].Obits);
}
EC_PRINT(" SM programming\n");
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);
EC_PRINT(" SM0 Type:%d StartAddr:%4.4x Flags:%8.8x\n", ec_slave[slave].SMtype[0],
ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMflags);
}
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);
EC_PRINT(" SM1 Type:%d StartAddr:%4.4x Flags:%8.8x\n", ec_slave[slave].SMtype[1],
ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMflags);
}
/* program SM2 to SMx */
for (nSM = 2; nSM < EC_MAXSM; nSM++)
{
if (ec_slave[slave].SM[nSM].StartAddr)
{
/* check if SM length is zero -> clear enable flag */
if (ec_slave[slave].SM[nSM].SMlength == 0)
ec_slave[slave].SM[nSM].SMflags = htoel( etohl(ec_slave[slave].SM[nSM].SMflags) & EC_SMENABLEMASK);
ec_FPWR(configadr, ECT_REG_SM0 + (nSM * sizeof(ec_smt)), sizeof(ec_smt), &ec_slave[slave].SM[nSM],
EC_TIMEOUTRET);
EC_PRINT(" SM%d Type:%d StartAddr:%4.4x Flags:%8.8x\n", nSM, ec_slave[slave].SMtype[nSM],
ec_slave[slave].SM[nSM].StartAddr, ec_slave[slave].SM[nSM].SMflags);
}
}
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 = ec_slave[slave].FMMUunused;
SMc = 0;
BitCount = 0;
ByteCount = 0;
EndAddr = 0;
FMMUsize = 0;
FMMUdone = 0;
/* create output mapping */
if (ec_slave[slave].Obits)
{
EC_PRINT(" OUTPUT MAPPING\n");
/* search for SM that contribute to the output mapping */
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((ec_slave[slave].Obits + 7) / 8)))
{
EC_PRINT(" FMMU %d\n", FMMUc);
while ((SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 3))
SMc++;
EC_PRINT(" SM%d\n", 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] != 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;
EC_PRINT(" SM%d\n", SMc);
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);
/* add two for an output FMMU */
ec_group[group].expectedWKC += 2;
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;
EC_PRINT(" slave %d Outputs %p startbit %d\n", slave, ec_slave[slave].outputs, ec_slave[slave].Ostartbit);
}
FMMUc++;
}
ec_slave[slave].FMMUunused = FMMUc;
diff = LogAddr - oLogAddr;
oLogAddr = LogAddr;
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
{
ec_group[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))
{
ec_group[group].IOsegment[currentsegment] = segmentsize;
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
{
currentsegment++;
segmentsize = 1;
}
}
else
segmentsize += 1;
}
ec_group[group].outputs = pIOmap;
ec_group[group].Obytes = LogAddr;
ec_group[group].nsegments = currentsegment + 1;
ec_group[group].Isegment = currentsegment;
ec_group[group].Ioffset = segmentsize;
if (!group)
{
ec_slave[0].outputs = pIOmap;
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 = ec_slave[slave].FMMUunused;
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)
{
EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
/* search for SM that contribute to the input mapping */
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((ec_slave[slave].Ibits + 7) / 8)))
{
EC_PRINT(" FMMU %d\n", FMMUc);
while ((SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 4))
SMc++;
EC_PRINT(" SM%d\n", 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].Ibits) && (SMc < (EC_MAXSM - 1))) /* more SM for input */
{
SMc++;
while ((SMc < (EC_MAXSM - 1)) && (ec_slave[slave].SMtype[SMc] != 4))
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;
EC_PRINT(" SM%d\n", SMc);
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);
/* add one for an input FMMU */
ec_group[group].expectedWKC += 1;
}
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;
EC_PRINT(" Inputs %p startbit %d\n", ec_slave[slave].inputs, ec_slave[slave].Istartbit);
}
FMMUc++;
}
ec_slave[slave].FMMUunused = FMMUc;
diff = LogAddr - oLogAddr;
oLogAddr = LogAddr;
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
{
ec_group[group].IOsegment[currentsegment] = segmentsize;
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
{
currentsegment++;
segmentsize = diff;
}
}
else
segmentsize += diff;
}
ec_eeprom2pdi(slave); /* set Eeprom control to PDI */
ec_FPWRw(configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP), EC_TIMEOUTRET); /* set safeop status */
}
if (BitPos)
{
LogAddr++;
oLogAddr = LogAddr;
BitPos = 0;
if ((segmentsize + 1) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
{
ec_group[group].IOsegment[currentsegment] = segmentsize;
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
{
currentsegment++;
segmentsize = 1;
}
}
else
segmentsize += 1;
}
ec_group[group].IOsegment[currentsegment] = segmentsize;
ec_group[group].nsegments = currentsegment + 1;
ec_group[group].inputs = pIOmap + ec_group[group].Obytes;
ec_group[group].Ibytes = LogAddr - ec_group[group].Obytes;
if (ec_slave[slave].blockLRW)
ec_group[group].blockLRW++;
ec_group[group].Ebuscurrent += ec_slave[slave].Ebuscurrent;
if (!group)
{
ec_slave[0].inputs = pIOmap + ec_slave[0].Obytes;
ec_slave[0].Ibytes = LogAddr - ec_slave[0].Obytes; /* store input bytes in master record */
}
EC_PRINT("IOmapSize %d\n", LogAddr - ec_group[group].logstartaddr);
return (LogAddr - ec_group[group].logstartaddr);
}
return 0;
}
