# HG changeset patch # User Florian Pose # Date 1212765205 0 # Node ID b0e89425774351197d939e53240d213d82efdbb8 # Parent 93807963b906bdc53490212b6cb46af4e5698d62 Implemented sdo_upload command. diff -r 93807963b906 -r b0e894257743 TODO --- a/TODO Fri Jun 06 13:03:56 2008 +0000 +++ b/TODO Fri Jun 06 15:13:25 2008 +0000 @@ -9,8 +9,8 @@ Version 1.4.0: * Slaves as array. +* ioctl() rights. * Replace all Sysfs files via the new ethercat tool. - - Sdo entry value read - Sdo entry value write - Slave alias write - Slave SII write diff -r 93807963b906 -r b0e894257743 master/cdev.c --- a/master/cdev.c Fri Jun 06 13:03:56 2008 +0000 +++ b/master/cdev.c Fri Jun 06 15:13:25 2008 +0000 @@ -137,7 +137,7 @@ long retval = 0; if (master->debug_level) - EC_DBG("ioctl(filp = %x, cmd = %u, arg = %u)\n", + EC_DBG("ioctl(filp = %x, cmd = %u, arg = %x)\n", (u32) filp, (u32) cmd, (u32) arg); // FIXME lock @@ -562,19 +562,29 @@ break; } - if (!(sdo = ec_slave_get_sdo_by_pos_const( - slave, data.sdo_position))) { - EC_ERR("Sdo %u does not exist in slave %u!\n", - data.sdo_position, data.slave_position); - retval = -EINVAL; - break; + if (data.sdo_spec <= 0) { + if (!(sdo = ec_slave_get_sdo_by_pos_const( + slave, -data.sdo_spec))) { + EC_ERR("Sdo %u does not exist in slave %u!\n", + -data.sdo_spec, data.slave_position); + retval = -EINVAL; + break; + } + } else { + if (!(sdo = ec_slave_get_sdo_const( + slave, data.sdo_spec))) { + EC_ERR("Sdo 0x%04X does not exist in slave %u!\n", + data.sdo_spec, data.slave_position); + retval = -EINVAL; + break; + } } if (!(entry = ec_sdo_get_entry_const( sdo, data.sdo_entry_subindex))) { - EC_ERR("Sdo entry %u does not exist in Sdo %u " - "in slave %u!\n", data.sdo_entry_subindex, - data.sdo_position, data.slave_position); + EC_ERR("Sdo entry 0x%04X:%02X does not exist " + "in slave %u!\n", sdo->index, + data.sdo_entry_subindex, data.slave_position); retval = -EINVAL; break; } @@ -598,6 +608,77 @@ break; } + case EC_IOCTL_SDO_UPLOAD: + { + ec_ioctl_sdo_upload_t data; + ec_master_sdo_request_t request; + + if (copy_from_user(&data, (void __user *) arg, sizeof(data))) { + retval = -EFAULT; + break; + } + + if (!(request.slave = ec_master_find_slave( + master, 0, data.slave_position))) { + EC_ERR("Slave %u does not exist!\n", data.slave_position); + retval = -EINVAL; + break; + } + + ec_sdo_request_init(&request.req); + ec_sdo_request_address(&request.req, + data.sdo_index, data.sdo_entry_subindex); + ecrt_sdo_request_read(&request.req); + + // schedule request. + down(&master->sdo_sem); + list_add_tail(&request.list, &master->slave_sdo_requests); + up(&master->sdo_sem); + + // wait for processing through FSM + if (wait_event_interruptible(master->sdo_queue, + request.req.state != EC_REQUEST_QUEUED)) { + // interrupted by signal + down(&master->sdo_sem); + if (request.req.state == EC_REQUEST_QUEUED) { + list_del(&request.req.list); + up(&master->sdo_sem); + retval = -EINTR; + break; + } + // request already processing: interrupt not possible. + up(&master->sdo_sem); + } + + // wait until master FSM has finished processing + wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY); + + if (request.req.state != EC_REQUEST_SUCCESS) { + retval = -EIO; + break; + } + + if (request.req.data_size > data.target_size) { + EC_ERR("Buffer too small.\n"); + retval = -EOVERFLOW; + break; + } + data.data_size = request.req.data_size; + + if (copy_to_user((void __user *) data.target, + request.req.data, data.data_size)) { + retval = -EFAULT; + break; + } + if (__copy_to_user((void __user *) arg, &data, sizeof(data))) { + retval = -EFAULT; + break; + } + + ec_sdo_request_clear(&request.req); + break; + } + default: retval = -ENOIOCTLCMD; } diff -r 93807963b906 -r b0e894257743 master/ioctl.h --- a/master/ioctl.h Fri Jun 06 13:03:56 2008 +0000 +++ b/master/ioctl.h Fri Jun 06 15:13:25 2008 +0000 @@ -57,6 +57,7 @@ EC_IOCTL_SLAVE_STATE, EC_IOCTL_SDO, EC_IOCTL_SDO_ENTRY, + EC_IOCTL_SDO_UPLOAD, }; /*****************************************************************************/ @@ -211,7 +212,7 @@ typedef struct { // inputs uint16_t slave_position; - uint16_t sdo_position; + int sdo_spec; // positive: index, negative: list position uint8_t sdo_entry_subindex; // outputs @@ -222,4 +223,18 @@ /*****************************************************************************/ +typedef struct { + // inputs + uint16_t slave_position; + uint16_t sdo_index; + uint8_t sdo_entry_subindex; + unsigned int target_size; + uint8_t *target; + + // outputs + unsigned int data_size; +} ec_ioctl_sdo_upload_t; + +/*****************************************************************************/ + #endif diff -r 93807963b906 -r b0e894257743 tools/Master.cpp --- a/tools/Master.cpp Fri Jun 06 13:03:56 2008 +0000 +++ b/tools/Master.cpp Fri Jun 06 15:13:25 2008 +0000 @@ -18,6 +18,72 @@ #include "Master.h" +#if __BYTE_ORDER == __LITTLE_ENDIAN + +#define le16tocpu(x) x +#define le32tocpu(x) x + +#elif __BYTE_ORDER == __BIG_ENDIAN + +#define le16tocpu(x) \ + ((uint16_t)( \ + (((uint16_t)(x) & 0x00ffU) << 8) | \ + (((uint16_t)(x) & 0xff00U) >> 8) )) +#define le32tocpu(x) \ + ((uint32_t)( \ + (((uint32_t)(x) & 0x000000ffUL) << 24) | \ + (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ + (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ + (((uint32_t)(x) & 0xff000000UL) >> 24) )) + +#endif + +/****************************************************************************/ + +struct CoEDataType { + const char *name; + uint16_t coeCode; + unsigned int byteSize; +}; + +static const CoEDataType dataTypes[] = { + {"int8", 0x0002, 1}, + {"int16", 0x0003, 2}, + {"int32", 0x0004, 4}, + {"uint8", 0x0005, 1}, + {"uint16", 0x0006, 2}, + {"uint32", 0x0007, 4}, + {"string", 0x0009, 0}, + {"raw", 0xffff, 0}, + {} +}; + +/****************************************************************************/ + +const CoEDataType *findDataType(const string &str) +{ + const CoEDataType *d; + + for (d = dataTypes; d->name; d++) + if (str == d->name) + return d; + + return NULL; +} + +/****************************************************************************/ + +const CoEDataType *findDataType(uint16_t code) +{ + const CoEDataType *d; + + for (d = dataTypes; d->name; d++) + if (code == d->coeCode) + return d; + + return NULL; +} + /****************************************************************************/ Master::Master() @@ -239,6 +305,139 @@ /****************************************************************************/ +void Master::sdoUpload( + int slavePosition, + const string &dataTypeStr, + const vector &commandArgs + ) +{ + stringstream strIndex, strSubIndex; + int number, sval; + ec_ioctl_sdo_upload_t data; + unsigned int i, uval; + const CoEDataType *dataType = NULL; + + if (slavePosition < 0) { + stringstream err; + err << "'sdo_upload' requires a slave! Please specify --slave."; + throw MasterException(err.str()); + } + data.slave_position = slavePosition; + + if (commandArgs.size() != 2) { + stringstream err; + err << "'sdo_upload' takes two arguments!"; + throw MasterException(err.str()); + } + + strIndex << commandArgs[0]; + strIndex >> hex >> number; + if (strIndex.fail() || number < 0x0000 || number > 0xffff) { + stringstream err; + err << "Invalid Sdo index '" << commandArgs[0] << "'!"; + throw MasterException(err.str()); + } + data.sdo_index = number; + + strSubIndex << commandArgs[1]; + strSubIndex >> hex >> number; + if (strSubIndex.fail() || number < 0x00 || number > 0xff) { + stringstream err; + err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; + throw MasterException(err.str()); + } + data.sdo_entry_subindex = number; + + if (dataTypeStr != "") { // data type specified + if (!(dataType = findDataType(dataTypeStr))) { + stringstream err; + err << "Invalid data type '" << dataTypeStr << "'!"; + throw MasterException(err.str()); + } + } else { // no data type specified: fetch from dictionary + ec_ioctl_sdo_entry_t entry; + unsigned int entryByteSize; + try { + getSdoEntry(&entry, slavePosition, + data.sdo_index, data.sdo_entry_subindex); + } catch (MasterException &e) { + stringstream err; + err << "Failed to determine Sdo entry data type. " + << "Please specify --type."; + throw MasterException(err.str()); + } + if (!(dataType = findDataType(entry.data_type))) { + stringstream err; + err << "Pdo entry has unknown data type 0x" + << hex << setfill('0') << setw(4) << entry.data_type << "!" + << " Please specify --type."; + throw MasterException(err.str()); + } + } + + if (dataType->byteSize) { + data.target_size = dataType->byteSize; + } else { + data.target_size = DefaultTargetSize; + } + + data.target = new uint8_t[data.target_size + 1]; + + if (ioctl(fd, EC_IOCTL_SDO_UPLOAD, &data) < 0) { + stringstream err; + err << "Failed to upload Sdo: " << strerror(errno); + delete [] data.target; + throw MasterException(err.str()); + } + + if (dataType->byteSize && data.data_size != dataType->byteSize) { + stringstream err; + err << "Data type mismatch. Expected " << dataType->name + << " with " << dataType->byteSize << " byte(s), but got " + << data.data_size << " byte(s)."; + throw MasterException(err.str()); + } + + cout << setfill('0'); + switch (dataType->coeCode) { + case 0x0002: // int8 + sval = *(int8_t *) data.target; + cout << sval << " 0x" << hex << setw(2) << sval << endl; + break; + case 0x0003: // int16 + sval = le16tocpu(*(int16_t *) data.target); + cout << sval << " 0x" << hex << setw(4) << sval << endl; + break; + case 0x0004: // int32 + sval = le32tocpu(*(int32_t *) data.target); + cout << sval << " 0x" << hex << setw(8) << sval << endl; + break; + case 0x0005: // uint8 + uval = (unsigned int) *(uint8_t *) data.target; + cout << uval << " 0x" << hex << setw(2) << uval << endl; + break; + case 0x0006: // uint16 + uval = le16tocpu(*(uint16_t *) data.target); + cout << uval << " 0x" << hex << setw(4) << uval << endl; + break; + case 0x0007: // uint32 + uval = le32tocpu(*(uint32_t *) data.target); + cout << uval << " 0x" << hex << setw(8) << uval << endl; + break; + case 0x0009: // string + cout << string((const char *) data.target, data.data_size) + << endl; + break; + default: + printRawData(data.target, data.data_size); + break; + } + + delete [] data.target; +} + +/****************************************************************************/ + void Master::requestStates( int slavePosition, const vector &commandArgs @@ -474,7 +673,7 @@ continue; for (j = 0; j <= sdo.max_subindex; j++) { - getSdoEntry(&entry, slavePosition, i, j); + getSdoEntry(&entry, slavePosition, -i, j); cout << " Entry 0x" << hex << setfill('0') << setw(2) @@ -807,25 +1006,18 @@ void Master::getSdoEntry( ec_ioctl_sdo_entry_t *entry, uint16_t slaveIndex, - uint16_t sdoPosition, + int sdoSpec, uint8_t entrySubindex ) { entry->slave_position = slaveIndex; - entry->sdo_position = sdoPosition; + entry->sdo_spec = sdoSpec; entry->sdo_entry_subindex = entrySubindex; if (ioctl(fd, EC_IOCTL_SDO_ENTRY, entry)) { stringstream err; err << "Failed to get Sdo entry: "; - if (errno == EINVAL) - err << "Either slave " << slaveIndex << " does not exist, " - << "or it contains less than " << sdoPosition + 1 - << " Sdos, or the Sdo at position " << sdoPosition - << " contains less than " << (unsigned int) entrySubindex + 1 - << " entries!" << endl; - else - err << strerror(errno); + err << strerror(errno); throw MasterException(err.str()); } } @@ -867,3 +1059,18 @@ } /****************************************************************************/ + +void Master::printRawData( + const uint8_t *data, + unsigned int size) +{ + cout << hex << setfill('0'); + while (size--) { + cout << "0x" << setw(2) << (unsigned int) *data++; + if (size) + cout << " "; + } + cout << endl; +} + +/****************************************************************************/ diff -r 93807963b906 -r b0e894257743 tools/Master.h --- a/tools/Master.h Fri Jun 06 13:03:56 2008 +0000 +++ b/tools/Master.h Fri Jun 06 15:13:25 2008 +0000 @@ -49,6 +49,7 @@ void showMaster(); void listPdos(int, bool = false); void listSdos(int, bool = false); + void sdoUpload(int, const string &, const vector &); void requestStates(int, const vector &); void generateXml(int); @@ -72,12 +73,15 @@ void getPdoEntry(ec_ioctl_pdo_entry_t *, uint16_t, uint8_t, uint8_t, uint8_t); void getSdo(ec_ioctl_sdo_t *, uint16_t, uint16_t); - void getSdoEntry(ec_ioctl_sdo_entry_t *, uint16_t, uint16_t, uint8_t); + void getSdoEntry(ec_ioctl_sdo_entry_t *, uint16_t, int, uint8_t); void requestState(uint16_t, uint8_t); static string slaveState(uint8_t); + static void printRawData(const uint8_t *, unsigned int); private: + enum {DefaultTargetSize = 1024}; + unsigned int index; int fd; }; diff -r 93807963b906 -r b0e894257743 tools/main.cpp --- a/tools/main.cpp Fri Jun 06 13:03:56 2008 +0000 +++ b/tools/main.cpp Fri Jun 06 15:13:25 2008 +0000 @@ -16,16 +16,14 @@ /*****************************************************************************/ #define DEFAULT_MASTER 0 -#define DEFAULT_COMMAND "slaves" -#define DEFAULT_SLAVEPOSITION -1 -#define DEFAULT_DOMAININDEX -1 static unsigned int masterIndex = DEFAULT_MASTER; -static int slavePosition = DEFAULT_SLAVEPOSITION; -static int domainIndex = DEFAULT_DOMAININDEX; -static string command = DEFAULT_COMMAND; +static int slavePosition = -1; +static int domainIndex = -1; +static string command; vector commandArgs; static bool quiet = false; +string dataTypeStr; /*****************************************************************************/ @@ -41,6 +39,7 @@ << " master Show master information." << endl << " pdos List Pdo mapping." << endl << " sdos List Sdo dictionaries." << endl + << " sdo_upload (su) Read Sdo entries." << endl << " state Request slave states." << endl << " xml Generate slave information xmls." << endl << "Global options:" << endl @@ -54,6 +53,7 @@ << endl << " or 'all' for all domains (default)." << endl + << " --type -t Forced Sdo data type." << endl << " --quiet -q Show less output." << endl << " --help -h Show this help." << endl; } @@ -70,13 +70,14 @@ {"master", required_argument, NULL, 'm'}, {"slave", required_argument, NULL, 's'}, {"domain", required_argument, NULL, 'd'}, + {"type", required_argument, NULL, 't'}, {"quiet", no_argument, NULL, 'q'}, {"help", no_argument, NULL, 'h'}, {} }; do { - c = getopt_long(argc, argv, "m:s:d:qh", longOptions, &optionIndex); + c = getopt_long(argc, argv, "m:s:d:t:qh", longOptions, &optionIndex); switch (c) { case 'm': @@ -120,6 +121,10 @@ } break; + case 't': + dataTypeStr = optarg; + break; + case 'q': quiet = true; break; @@ -173,6 +178,8 @@ master.listPdos(slavePosition, quiet); } else if (command == "sdos") { master.listSdos(slavePosition, quiet); + } else if (command == "sdo_upload" || command == "su") { + master.sdoUpload(slavePosition, dataTypeStr, commandArgs); } else if (command == "state") { master.requestStates(slavePosition, commandArgs); } else if (command == "xml") {