#include #include #include #include #include #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> 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); }