diff --git a/TODO b/TODO
index c03afc4c2c6dabc45b8406b4fe3ab8886f1cbd30..63f98af03ea57857d2e7027788fe13b63dd245af 100644
--- a/TODO
+++ b/TODO
@@ -9,8 +9,8 @@ $Id$
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 --git a/master/cdev.c b/master/cdev.c
index 7361e88be1a3566b5c13037bf343f005dca921e2..f91781ae6402543177f3096e7c1446c052c3f0ea 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -137,7 +137,7 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
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 --git a/master/ioctl.h b/master/ioctl.h
index 8f39fdda77e4e19d87d1c0fb823c3028dc14e9c2..4bb61ba6cbda807594cb661bb4c45b890d4dbc91 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -57,6 +57,7 @@ enum {
EC_IOCTL_SLAVE_STATE,
EC_IOCTL_SDO,
EC_IOCTL_SDO_ENTRY,
+ EC_IOCTL_SDO_UPLOAD,
};
/*****************************************************************************/
@@ -211,7 +212,7 @@ typedef struct {
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 {
/*****************************************************************************/
+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 --git a/tools/Master.cpp b/tools/Master.cpp
index 3be29309504ccdd80ce055e168efcf08dd3a1b11..eb457451a5082ddecd23c947b6c1e1b6816315a2 100644
--- a/tools/Master.cpp
+++ b/tools/Master.cpp
@@ -18,6 +18,72 @@ using namespace std;
#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::listSdos(int slavePosition, bool quiet)
/****************************************************************************/
+void Master::sdoUpload(
+ int slavePosition,
+ const string &dataTypeStr,
+ const vector<string> &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<string> &commandArgs
@@ -474,7 +673,7 @@ void Master::listSlaveSdos(
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::getSdo(
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 @@ string Master::slaveState(uint8_t state)
}
/****************************************************************************/
+
+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 --git a/tools/Master.h b/tools/Master.h
index 9c90b11ff302e94ee15d01608f961b0c2085e080..808cae4b640790b43f5deca5beae4836fd58b026 100644
--- a/tools/Master.h
+++ b/tools/Master.h
@@ -49,6 +49,7 @@ class Master
void showMaster();
void listPdos(int, bool = false);
void listSdos(int, bool = false);
+ void sdoUpload(int, const string &, const vector<string> &);
void requestStates(int, const vector<string> &);
void generateXml(int);
@@ -72,12 +73,15 @@ class Master
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 --git a/tools/main.cpp b/tools/main.cpp
index 4e1b60cec9220df8a49cbb81b07787d2b25923eb..1b8b7a9cf3bed06cfe5d65c47d398d0313ac22a1 100644
--- a/tools/main.cpp
+++ b/tools/main.cpp
@@ -16,16 +16,14 @@ using namespace std;
/*****************************************************************************/
#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<string> commandArgs;
static bool quiet = false;
+string dataTypeStr;
/*****************************************************************************/
@@ -41,6 +39,7 @@ void printUsage()
<< " 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 @@ void printUsage()
<< endl
<< " or 'all' for all domains (default)."
<< endl
+ << " --type -t <type> Forced Sdo data type." << endl
<< " --quiet -q Show less output." << endl
<< " --help -h Show this help." << endl;
}
@@ -70,13 +70,14 @@ void getOptions(int argc, char **argv)
{"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 @@ void getOptions(int argc, char **argv)
}
break;
+ case 't':
+ dataTypeStr = optarg;
+ break;
+
case 'q':
quiet = true;
break;
@@ -173,6 +178,8 @@ int main(int argc, char **argv)
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") {