# HG changeset patch # User Florian Pose # Date 1213013479 0 # Node ID 5868944a645645e20ca2114be8fe3a20c5730807 # Parent a560a124a92e9d8718be41e1e5c7da6230b6b22f Implemented Sdo download via ethercat command. diff -r a560a124a92e -r 5868944a6456 TODO --- a/TODO Mon Jun 09 10:31:05 2008 +0000 +++ b/TODO Mon Jun 09 12:11:19 2008 +0000 @@ -10,7 +10,6 @@ * Slaves as array. * Replace all Sysfs files via the new ethercat tool. - - Sdo entry value write - Slave alias write - Slave SII write - Slave info (flags, mailbox, general) @@ -36,7 +35,6 @@ kernel threads to user space daemon with a TCP interface replacing the cdev). * Implement user space realtime interface via cdev. -* SDO write access from user space. * Mailbox protocol handlers. * Mailbox state machine using toggle bits. * Mailbox gateway. diff -r a560a124a92e -r 5868944a6456 master/cdev.c --- a/master/cdev.c Mon Jun 09 10:31:05 2008 +0000 +++ b/master/cdev.c Mon Jun 09 12:11:19 2008 +0000 @@ -684,6 +684,82 @@ break; } + case EC_IOCTL_SDO_DOWNLOAD: + { + ec_ioctl_sdo_download_t data; + ec_master_sdo_request_t request; + + if (!(filp->f_mode & FMODE_WRITE)) + return -EPERM; + + 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; + } + + // copy data to download + if (!data.data_size) { + EC_ERR("Zero data size!\n"); + retval = -EINVAL; + break; + } + + ec_sdo_request_init(&request.req); + ec_sdo_request_address(&request.req, + data.sdo_index, data.sdo_entry_subindex); + if (ec_sdo_request_alloc(&request.req, data.data_size)) { + ec_sdo_request_clear(&request.req); + retval = -ENOMEM; + break; + } + if (copy_from_user(request.req.data, + (void __user *) data.data, data.data_size)) { + ec_sdo_request_clear(&request.req); + retval = -EFAULT; + break; + } + request.req.data_size = data.data_size; + ecrt_sdo_request_write(&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; + } + + ec_sdo_request_clear(&request.req); + break; + } + default: retval = -ENOTTY; } diff -r a560a124a92e -r 5868944a6456 master/ioctl.h --- a/master/ioctl.h Mon Jun 09 10:31:05 2008 +0000 +++ b/master/ioctl.h Mon Jun 09 12:11:19 2008 +0000 @@ -66,6 +66,7 @@ #define EC_IOCTL_SDO EC_IOWR(0x0b, ec_ioctl_sdo_t) #define EC_IOCTL_SDO_ENTRY EC_IOWR(0x0c, ec_ioctl_sdo_entry_t) #define EC_IOCTL_SDO_UPLOAD EC_IOWR(0x0d, ec_ioctl_sdo_upload_t) +#define EC_IOCTL_SDO_DOWNLOAD EC_IOW(0x0e, ec_ioctl_sdo_download_t) /*****************************************************************************/ @@ -244,4 +245,15 @@ /*****************************************************************************/ +typedef struct { + // inputs + uint16_t slave_position; + uint16_t sdo_index; + uint8_t sdo_entry_subindex; + unsigned int data_size; + uint8_t *data; +} ec_ioctl_sdo_download_t; + +/*****************************************************************************/ + #endif diff -r a560a124a92e -r 5868944a6456 tools/Master.cpp --- a/tools/Master.cpp Mon Jun 09 10:31:05 2008 +0000 +++ b/tools/Master.cpp Mon Jun 09 12:11:19 2008 +0000 @@ -18,24 +18,33 @@ #include "Master.h" -#if __BYTE_ORDER == __LITTLE_ENDIAN - -#define le16tocpu(x) x -#define le32tocpu(x) x - -#elif __BYTE_ORDER == __BIG_ENDIAN - -#define le16tocpu(x) \ +#define swap16(x) \ ((uint16_t)( \ (((uint16_t)(x) & 0x00ffU) << 8) | \ (((uint16_t)(x) & 0xff00U) >> 8) )) -#define le32tocpu(x) \ +#define swap32(x) \ ((uint32_t)( \ (((uint32_t)(x) & 0x000000ffUL) << 24) | \ (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ (((uint32_t)(x) & 0xff000000UL) >> 24) )) +#if __BYTE_ORDER == __LITTLE_ENDIAN + +#define le16tocpu(x) x +#define le32tocpu(x) x + +#define cputole16(x) x +#define cputole32(x) x + +#elif __BYTE_ORDER == __BIG_ENDIAN + +#define le16tocpu(x) swap16(x) +#define le32tocpu(x) swap32(x) + +#define cputole16(x) swap16(x) +#define cputole32(x) swap32(x) + #endif /****************************************************************************/ @@ -300,6 +309,165 @@ /****************************************************************************/ +void Master::sdoDownload( + int slavePosition, + const string &dataTypeStr, + const vector &commandArgs + ) +{ + stringstream strIndex, strSubIndex, strValue, err; + int number, sval; + ec_ioctl_sdo_download_t data; + unsigned int i, uval; + const CoEDataType *dataType = NULL; + + if (slavePosition < 0) { + err << "'sdo_download' requires a slave! Please specify --slave."; + throw MasterException(err.str()); + } + data.slave_position = slavePosition; + + if (commandArgs.size() != 3) { + err << "'sdo_download' takes 3 arguments!"; + throw MasterException(err.str()); + } + + strIndex << commandArgs[0]; + strIndex >> hex >> number; + if (strIndex.fail() || number < 0x0000 || number > 0xffff) { + 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) { + err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; + throw MasterException(err.str()); + } + data.sdo_entry_subindex = number; + + if (dataTypeStr != "") { // data type specified + if (!(dataType = findDataType(dataTypeStr))) { + 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; + + open(Read); + + try { + getSdoEntry(&entry, slavePosition, + data.sdo_index, data.sdo_entry_subindex); + } catch (MasterException &e) { + err << "Failed to determine Sdo entry data type. " + << "Please specify --type."; + throw MasterException(err.str()); + } + if (!(dataType = findDataType(entry.data_type))) { + 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.data_size = dataType->byteSize; + } else { + data.data_size = DefaultBufferSize; + } + + data.data = new uint8_t[data.data_size + 1]; + + strValue << commandArgs[2]; + + switch (dataType->coeCode) { + case 0x0002: // int8 + strValue >> sval; + if ((uint32_t) sval > 0xff) { + delete [] data.data; + err << "Invalid value for type '" + << dataType->name << "'!"; + throw MasterException(err.str()); + } + *data.data = (int8_t) sval; + break; + case 0x0003: // int16 + strValue >> sval; + if ((uint32_t) sval > 0xffff) { + delete [] data.data; + err << "Invalid value for type '" + << dataType->name << "'!"; + throw MasterException(err.str()); + } + *(int16_t *) data.data = cputole16(sval); + break; + case 0x0004: // int32 + strValue >> sval; + *(int32_t *) data.data = cputole32(sval); + break; + case 0x0005: // uint8 + strValue >> uval; + if ((uint32_t) uval > 0xff) { + delete [] data.data; + err << "Invalid value for type '" + << dataType->name << "'!"; + throw MasterException(err.str()); + } + *data.data = (uint8_t) uval; + break; + case 0x0006: // uint16 + strValue >> uval; + if ((uint32_t) uval > 0xffff) { + delete [] data.data; + err << "Invalid value for type '" + << dataType->name << "'!"; + throw MasterException(err.str()); + } + *(uint16_t *) data.data = cputole16(uval); + break; + case 0x0007: // uint32 + strValue >> uval; + *(uint32_t *) data.data = cputole32(uval); + break; + case 0x0009: // string + if (strValue.str().size() >= data.data_size) { + err << "String too big"; + throw MasterException(err.str()); + } + data.data_size = strValue.str().size(); + strValue >> (char *) data.data; + break; + default: + break; + } + + if (strValue.fail()) { + err << "Invalid value argument '" << commandArgs[2] << "'!"; + throw MasterException(err.str()); + } + + cerr << "dt " << dataType->name << endl; + printRawData(data.data, data.data_size); + + open(ReadWrite); + + if (ioctl(fd, EC_IOCTL_SDO_DOWNLOAD, &data) < 0) { + stringstream err; + err << "Failed to download Sdo: " << strerror(errno); + delete [] data.data; + throw MasterException(err.str()); + } + + delete [] data.data; +} + +/****************************************************************************/ + void Master::sdoUpload( int slavePosition, const string &dataTypeStr, @@ -376,7 +544,7 @@ if (dataType->byteSize) { data.target_size = dataType->byteSize; } else { - data.target_size = DefaultTargetSize; + data.target_size = DefaultBufferSize; } data.target = new uint8_t[data.target_size + 1]; diff -r a560a124a92e -r 5868944a6456 tools/Master.h --- a/tools/Master.h Mon Jun 09 10:31:05 2008 +0000 +++ b/tools/Master.h Mon Jun 09 12:11:19 2008 +0000 @@ -48,6 +48,7 @@ void showMaster(); void listPdos(int, bool = false); void listSdos(int, bool = false); + void sdoDownload(int, const string &, const vector &); void sdoUpload(int, const string &, const vector &); void requestStates(int, const vector &); void generateXml(int); @@ -83,7 +84,7 @@ static void printRawData(const uint8_t *, unsigned int); private: - enum {DefaultTargetSize = 1024}; + enum {DefaultBufferSize = 1024}; unsigned int index; int fd; diff -r a560a124a92e -r 5868944a6456 tools/main.cpp --- a/tools/main.cpp Mon Jun 09 10:31:05 2008 +0000 +++ b/tools/main.cpp Mon Jun 09 12:11:19 2008 +0000 @@ -39,7 +39,8 @@ << " master Show master information." << endl << " pdos List Pdo mapping." << endl << " sdos List Sdo dictionaries." << endl - << " sdo_upload (su) Read Sdo entries." << endl + << " sdo_download (sd) Write an Sdo entry." << endl + << " sdo_upload (su) Read an Sdo entry." << endl << " state Request slave states." << endl << " xml Generate slave information xmls." << endl << "Global options:" << endl @@ -178,6 +179,8 @@ master.listPdos(slavePosition, quiet); } else if (command == "sdos") { master.listSdos(slavePosition, quiet); + } else if (command == "sdo_download" || command == "sd") { + master.sdoDownload(slavePosition, dataTypeStr, commandArgs); } else if (command == "sdo_upload" || command == "su") { master.sdoUpload(slavePosition, dataTypeStr, commandArgs); } else if (command == "state") {