259 lines
8.5 KiB
C
259 lines
8.5 KiB
C
#include <ethercat.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <pthread.h>
|
|
|
|
#include "ecmbind.h"
|
|
|
|
|
|
extern char ecd_iomap[4096];
|
|
extern int ecd_iomap_size;
|
|
|
|
ecd_pdo_entry_t ecd_pdo_map[1024];
|
|
int ecd_pdo_map_length;
|
|
|
|
|
|
|
|
|
|
/** Read PDO assign structure */
|
|
int ecd_read_pdoassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset)
|
|
{
|
|
ec_ODlistt ODlist;
|
|
ec_OElistt OElist;
|
|
|
|
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 = etohs(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);
|
|
|
|
ecd_pdo_map[ecd_pdo_map_length] = (ecd_pdo_entry_t){
|
|
slave,
|
|
obj_idx,
|
|
obj_subidx,
|
|
abs_offset,
|
|
abs_bit,
|
|
bitlen,
|
|
0,
|
|
""
|
|
};
|
|
|
|
if((wkc > 0) && OElist.Entries)
|
|
{
|
|
ecd_pdo_map[ecd_pdo_map_length].type = OElist.DataType[obj_subidx];
|
|
strncpy(ecd_pdo_map[ecd_pdo_map_length].name, OElist.Name[obj_subidx], 32);
|
|
|
|
//printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx]), OElist.Name[obj_subidx]);
|
|
}
|
|
/*
|
|
printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X) %s\n",
|
|
ecd_pdo_map[ecd_pdo_map_length].addr_offset,
|
|
ecd_pdo_map[ecd_pdo_map_length].addr_bit,
|
|
ecd_pdo_map[ecd_pdo_map_length].index,
|
|
ecd_pdo_map[ecd_pdo_map_length].subindex,
|
|
ecd_pdo_map[ecd_pdo_map_length].bitlength,
|
|
ecd_pdo_map[ecd_pdo_map_length].name
|
|
|
|
);
|
|
*/
|
|
ecd_pdo_map_length++;
|
|
|
|
bitoffset += bitlen;
|
|
};
|
|
};
|
|
};
|
|
};
|
|
/* return total found bitlength (PDO) */
|
|
return bsize;
|
|
}
|
|
|
|
|
|
int ecd_read_pdo_map(int slave)
|
|
{
|
|
int wkc, rdl;
|
|
int retVal = -1;
|
|
uint8 nSM, iSM, tSM;
|
|
int Tsize, outputs_bo, inputs_bo;
|
|
uint8 SMt_bug_add;
|
|
|
|
//printf("PDO mapping according to CoE :\n");
|
|
SMt_bug_add = 0;
|
|
outputs_bo = 0;
|
|
inputs_bo = 0;
|
|
rdl = sizeof(nSM); nSM = 0;
|
|
/* read SyncManager Communication Type object count */
|
|
wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
|
|
/* positive result from slave ? */
|
|
if ((wkc > 0) && (nSM > 2))
|
|
{
|
|
/* make nSM equal to number of defined SM */
|
|
nSM--;
|
|
/* limit to maximum number of SM defined, if true the slave can't be configured */
|
|
if (nSM > EC_MAXSM)
|
|
nSM = EC_MAXSM;
|
|
/* iterate for every SM type defined */
|
|
for (iSM = 2 ; iSM <= nSM ; iSM++)
|
|
{
|
|
rdl = sizeof(tSM); tSM = 0;
|
|
/* read SyncManager Communication Type */
|
|
wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM);
|
|
if (wkc > 0)
|
|
{
|
|
if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
|
|
{
|
|
SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
|
|
//printf("Activated SM type workaround, possible incorrect mapping.\n");
|
|
}
|
|
if(tSM)
|
|
tSM += SMt_bug_add; // only add if SMt > 0
|
|
|
|
if (tSM == 3) // outputs
|
|
{
|
|
/* read the assign RXPDO */
|
|
//printf(" SM%1d outputs\n addr b index: sub bitl data_type name\n", iSM);
|
|
Tsize = ecd_read_pdoassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].outputs - (uint8 *)&ecd_iomap[0]), outputs_bo );
|
|
outputs_bo += Tsize;
|
|
}
|
|
if (tSM == 4) // inputs
|
|
{
|
|
/* read the assign TXPDO */
|
|
//printf(" SM%1d inputs\n addr b index: sub bitl data_type name\n", iSM);
|
|
Tsize = ecd_read_pdoassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].inputs - (uint8 *)&ecd_iomap[0]), inputs_bo );
|
|
inputs_bo += Tsize;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* found some I/O bits ? */
|
|
if ((outputs_bo > 0) || (inputs_bo > 0))
|
|
retVal = 0;
|
|
return retVal;
|
|
}
|
|
|
|
int ecd_extract_map_value(int offset,int firstbit,int bitlength,char *buffer)
|
|
{
|
|
int n;
|
|
int bytelength = bitlength ? (7 + firstbit + bitlength) / 8 : 0;
|
|
|
|
memcpy(buffer, &ecd_iomap[offset], bytelength);
|
|
/*
|
|
fprintf(stderr, "IM: ");
|
|
print_hex(buffer, bytelength);
|
|
*/
|
|
buffer[bytelength] = 0;
|
|
|
|
if (firstbit)
|
|
{
|
|
for (n=0;n<bytelength-1;n++)
|
|
{
|
|
buffer[n] = ((buffer[n] >> firstbit) & (0xFFu >> firstbit) | (buffer[n+1] << (8-firstbit)));
|
|
}
|
|
buffer[n] = ((buffer[n] >> firstbit) & (0xFF >> firstbit));
|
|
}
|
|
|
|
/* fprintf(stderr, "\nFIN: ");
|
|
print_hex(buffer, bytelength);
|
|
fprintf(stderr, "\n");
|
|
fflush(stderr);
|
|
*/
|
|
return (bitlength);
|
|
}
|
|
|
|
int ecd_insert_map_value(int offset,int firstbit,int bitlength,char *buffer)
|
|
{
|
|
int n;
|
|
int bytelength = bitlength ? (7 + firstbit + bitlength) / 8 : 0;
|
|
|
|
char* im = malloc(bytelength);
|
|
if (!im)
|
|
return -1;
|
|
|
|
im[bytelength-1] = 0x00;
|
|
memcpy(im, buffer, bitlength / 8);
|
|
|
|
/* fprintf(stderr, "IM: ");
|
|
print_hex(im, bytelength);
|
|
*/
|
|
if (firstbit)
|
|
{
|
|
for (n=bytelength-1;n > 0;n++)
|
|
{
|
|
im[n-1] = ((im[n] << firstbit) | (im[n-1] >> (8-firstbit)));
|
|
}
|
|
im[n] = ((im[n] >> firstbit) & (0xFF >> firstbit));
|
|
}
|
|
/*
|
|
fprintf(stderr, "\nFIN: ");
|
|
print_hex(im, bytelength);
|
|
fprintf(stderr, "\n");
|
|
fflush(stderr);
|
|
*/
|
|
if (firstbit)
|
|
{
|
|
/* fprintf(stderr, "FIXME: unaligned bits not supported yet!\n");
|
|
fflush(stderr);
|
|
*/
|
|
} else {
|
|
/* fprintf(stderr, "Inserting at %d = ", offset);
|
|
print_hex(im, bytelength);
|
|
*/
|
|
memcpy(&ecd_iomap[offset], im, bytelength);
|
|
}
|
|
|
|
return (bitlength);
|
|
}
|
|
|
|
|
|
|