--- a/tool/Makefile.am Mon Jul 21 22:06:33 2008 +0000
+++ b/tool/Makefile.am Mon Jul 21 22:20:23 2008 +0000
@@ -34,7 +34,9 @@
bin_PROGRAMS = ethercat
ethercat_SOURCES = \
- Master.cpp Master.h \
+ MasterDevice.cpp MasterDevice.h \
+ cmd_alias.cpp \
+ cmd_config.cpp \
main.cpp
ethercat_CXXFLAGS = -I../master -Wall
--- a/tool/Master.cpp Mon Jul 21 22:06:33 2008 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,2175 +0,0 @@
-/*****************************************************************************
- *
- * $Id$
- *
- ****************************************************************************/
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <sys/ioctl.h>
-
-#include <iostream>
-#include <iomanip>
-#include <sstream>
-#include <fstream>
-#include <cctype> // toupper()
-#include <list>
-using namespace std;
-
-#include "Master.h"
-
-#define swap16(x) \
- ((uint16_t)( \
- (((uint16_t)(x) & 0x00ffU) << 8) | \
- (((uint16_t)(x) & 0xff00U) >> 8) ))
-#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
-
-/****************************************************************************/
-
-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()
-{
- index = 0;
- verbosity = Normal;
- fd = -1;
-}
-
-/****************************************************************************/
-
-Master::~Master()
-{
- close();
-}
-
-/****************************************************************************/
-
-void Master::setIndex(unsigned int i)
-{
- index = i;
-}
-
-/****************************************************************************/
-
-void Master::setVerbosity(Verbosity v)
-{
- verbosity = v;
-}
-
-/*****************************************************************************/
-
-/**
- * Writes the Secondary slave address (alias) to the slave's SII.
- */
-void Master::writeAlias(
- int slavePosition,
- bool force,
- const vector<string> &commandArgs
- )
-{
- uint16_t alias;
- stringstream err, strAlias;
- int number;
-
- if (commandArgs.size() != 1) {
- stringstream err;
- err << "'alias' takes exactly one argument!";
- throw MasterException(err.str());
- }
-
- strAlias << commandArgs[0];
- strAlias
- >> resetiosflags(ios::basefield) // guess base from prefix
- >> number;
- if (strAlias.fail() || number < 0x0000 || number > 0xffff) {
- err << "Invalid alias '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
- alias = number;
-
- if (slavePosition == -1) {
- unsigned int numSlaves, i;
-
- if (!force) {
- err << "This will write the alias addresses of all slaves to "
- << alias << "! Please specify --force to proceed.";
- throw MasterException(err.str());
- }
-
- open(ReadWrite);
- numSlaves = slaveCount();
-
- for (i = 0; i < numSlaves; i++) {
- writeSlaveAlias(i, alias);
- }
- } else {
- open(ReadWrite);
- writeSlaveAlias(slavePosition, alias);
- }
-}
-
-/*****************************************************************************/
-
-bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)
-{
- return a.alias < b.alias
- || (a.alias == b.alias && a.position < b.position);
-}
-
-/** Lists the bus configuration.
- */
-void Master::showConfigs()
-{
- ec_ioctl_master_t master;
- unsigned int i;
- ec_ioctl_config_t config;
- ConfigList configList;
-
- open(Read);
- getMaster(&master);
-
- for (i = 0; i < master.config_count; i++) {
- getConfig(&config, i);
- configList.push_back(config);
- }
-
- configList.sort();
-
- if (verbosity == Verbose) {
- showDetailedConfigs(configList);
- } else {
- listConfigs(configList);
- }
-}
-
-/****************************************************************************/
-
-void Master::outputData(int domainIndex)
-{
- open(Read);
-
- if (domainIndex == -1) {
- unsigned int i;
- ec_ioctl_master_t master;
-
- getMaster(&master);
-
- for (i = 0; i < master.domain_count; i++) {
- outputDomainData(i);
- }
- } else {
- outputDomainData(domainIndex);
- }
-}
-
-/****************************************************************************/
-
-void Master::setDebug(const vector<string> &commandArgs)
-{
- stringstream str;
- int debugLevel;
-
- if (commandArgs.size() != 1) {
- stringstream err;
- err << "'debug' takes exactly one argument!";
- throw MasterException(err.str());
- }
-
- str << commandArgs[0];
- str >> resetiosflags(ios::basefield) // guess base from prefix
- >> debugLevel;
-
- if (str.fail()) {
- stringstream err;
- err << "Invalid debug level '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
-
- open(ReadWrite);
-
- if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {
- stringstream err;
- err << "Failed to set debug level: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::showDomains(int domainIndex)
-{
- open(Read);
-
- if (domainIndex == -1) {
- unsigned int i;
- ec_ioctl_master_t master;
-
- getMaster(&master);
-
- for (i = 0; i < master.domain_count; i++) {
- showDomain(i);
- }
- } else {
- showDomain(domainIndex);
- }
-}
-
-/****************************************************************************/
-
-void Master::showMaster()
-{
- ec_ioctl_master_t data;
- stringstream err;
- unsigned int i;
-
- open(Read);
- getMaster(&data);
-
- cout
- << "Master" << index << endl
- << " Phase: ";
-
- switch (data.phase) {
- case 0: cout << "Waiting for device..."; break;
- case 1: cout << "Idle"; break;
- case 2: cout << "Operation"; break;
- default:
- err << "Invalid master phase " << data.phase;
- throw MasterException(err.str());
- }
-
- cout << endl
- << " Slaves: " << data.slave_count << endl;
-
- for (i = 0; i < 2; i++) {
- cout << " Device" << i << ": ";
- if (data.devices[i].address[0] == 0x00
- && data.devices[i].address[1] == 0x00
- && data.devices[i].address[2] == 0x00
- && data.devices[i].address[3] == 0x00
- && data.devices[i].address[4] == 0x00
- && data.devices[i].address[5] == 0x00) {
- cout << "None.";
- } else {
- cout << hex << setfill('0')
- << setw(2) << (unsigned int) data.devices[i].address[0] << ":"
- << setw(2) << (unsigned int) data.devices[i].address[1] << ":"
- << setw(2) << (unsigned int) data.devices[i].address[2] << ":"
- << setw(2) << (unsigned int) data.devices[i].address[3] << ":"
- << setw(2) << (unsigned int) data.devices[i].address[4] << ":"
- << setw(2) << (unsigned int) data.devices[i].address[5] << " ("
- << (data.devices[i].attached ? "attached" : "waiting...")
- << ")" << endl << dec
- << " Tx count: " << data.devices[i].tx_count << endl
- << " Rx count: " << data.devices[i].rx_count;
- }
- cout << endl;
- }
-}
-
-/****************************************************************************/
-
-void Master::listPdos(int slavePosition)
-{
- open(Read);
-
- if (slavePosition == -1) {
- unsigned int numSlaves = slaveCount(), i;
-
- for (i = 0; i < numSlaves; i++) {
- listSlavePdos(i, true);
- }
- } else {
- listSlavePdos(slavePosition, false);
- }
-}
-
-/****************************************************************************/
-
-void Master::listSdos(int slavePosition)
-{
- open(Read);
-
- if (slavePosition == -1) {
- unsigned int numSlaves = slaveCount(), i;
-
- for (i = 0; i < numSlaves; i++) {
- listSlaveSdos(i, true);
- }
- } else {
- listSlaveSdos(slavePosition, false);
- }
-}
-
-/****************************************************************************/
-
-void Master::sdoDownload(
- int slavePosition,
- const string &dataTypeStr,
- const vector<string> &commandArgs
- )
-{
- stringstream strIndex, strSubIndex, strValue, err;
- ec_ioctl_slave_sdo_download_t data;
- unsigned int number;
- 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
- >> resetiosflags(ios::basefield) // guess base from prefix
- >> data.sdo_index;
- if (strIndex.fail()) {
- err << "Invalid Sdo index '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
-
- strSubIndex << commandArgs[1];
- strSubIndex
- >> resetiosflags(ios::basefield) // guess base from prefix
- >> number;
- if (strSubIndex.fail() || 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_slave_sdo_entry_t entry;
-
- open(ReadWrite);
-
- 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];
- strValue >> resetiosflags(ios::basefield); // guess base from prefix
- strValue.exceptions(ios::failbit);
-
- try {
- switch (dataType->coeCode) {
- case 0x0002: // int8
- {
- int16_t val; // uint8_t is interpreted as char
- strValue >> val;
- if (val > 127 || val < -128)
- throw ios::failure("Value out of range");
- *data.data = val;
- break;
- }
- case 0x0003: // int16
- {
- int16_t val;
- strValue >> val;
- *(int16_t *) data.data = cputole16(val);
- break;
- }
- case 0x0004: // int32
- {
- int32_t val;
- strValue >> val;
- *(int32_t *) data.data = cputole32(val);
- break;
- }
- case 0x0005: // uint8
- {
- uint16_t val; // uint8_t is interpreted as char
- strValue >> val;
- if (val > 0xff)
- throw ios::failure("Value out of range");
- *data.data = val;
- break;
- }
- case 0x0006: // uint16
- {
- uint16_t val;
- strValue >> val;
- *(uint16_t *) data.data = cputole16(val);
- break;
- }
- case 0x0007: // uint32
- {
- uint32_t val;
- strValue >> val;
- *(uint32_t *) data.data = cputole32(val);
- break;
- }
- case 0x0009: // string
- if (strValue.str().size() >= data.data_size) {
- err << "String too large";
- throw MasterException(err.str());
- }
- data.data_size = strValue.str().size();
- strValue >> (char *) data.data;
- break;
-
- default:
- delete [] data.data;
- err << "Unknown data type 0x" << hex << dataType->coeCode;
- throw MasterException(err.str());
- }
- } catch (ios::failure &e) {
- delete [] data.data;
- err << "Invalid value argument '" << commandArgs[2]
- << "' for type '" << dataType->name << "'!";
- throw MasterException(err.str());
- }
-
- open(ReadWrite);
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &data) < 0) {
- stringstream err;
- err << "Failed to download Sdo: ";
- if (errno == EIO && data.abort_code) {
- err << "Abort code 0x" << hex << setfill('0')
- << setw(8) << data.abort_code;
- } else {
- err << strerror(errno);
- }
- delete [] data.data;
- throw MasterException(err.str());
- }
-
- delete [] data.data;
-}
-
-/****************************************************************************/
-
-void Master::sdoUpload(
- int slavePosition,
- const string &dataTypeStr,
- const vector<string> &commandArgs
- )
-{
- stringstream strIndex, strSubIndex;
- int sval;
- ec_ioctl_slave_sdo_upload_t data;
- unsigned int 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
- >> resetiosflags(ios::basefield) // guess base from prefix
- >> data.sdo_index;
- if (strIndex.fail()) {
- stringstream err;
- err << "Invalid Sdo index '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
-
- strSubIndex << commandArgs[1];
- strSubIndex
- >> resetiosflags(ios::basefield) // guess base from prefix
- >> uval;
- if (strSubIndex.fail() || uval > 0xff) {
- stringstream err;
- err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
- throw MasterException(err.str());
- }
- data.sdo_entry_subindex = uval;
-
- 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_slave_sdo_entry_t entry;
-
- open(Read);
-
- 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 = DefaultBufferSize;
- }
-
- data.target = new uint8_t[data.target_size + 1];
-
- open(Read);
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &data) < 0) {
- stringstream err;
- err << "Failed to upload Sdo: ";
- if (errno == EIO && data.abort_code) {
- err << "Abort code 0x" << hex << setfill('0')
- << setw(8) << data.abort_code;
- } else {
- err << strerror(errno);
- }
- delete [] data.target;
- close();
- throw MasterException(err.str());
- }
-
- close();
-
- if (dataType->byteSize && data.data_size != dataType->byteSize) {
- stringstream err;
- err << "Data type mismatch. Expected " << dataType->name
- << " with " << dataType->byteSize << " byte, but got "
- << data.data_size << " byte.";
- 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::showSlaves(int slavePosition)
-{
- open(Read);
-
- if (verbosity == Verbose) {
- if (slavePosition == -1) {
- unsigned int numSlaves = slaveCount(), i;
-
- for (i = 0; i < numSlaves; i++) {
- showSlave(i);
- }
- } else {
- showSlave(slavePosition);
- }
- } else {
- listSlaves(slavePosition);
- }
-}
-
-/****************************************************************************/
-
-struct CategoryName {
- uint16_t type;
- const char *name;
-};
-
-static const CategoryName categoryNames[] = {
- {0x000a, "STRINGS"},
- {0x0014, "DataTypes"},
- {0x001e, "General"},
- {0x0028, "FMMU"},
- {0x0029, "SyncM"},
- {0x0032, "TXPDO"},
- {0x0033, "RXPDO"},
- {0x003c, "DC"},
- {}
-};
-
-const char *getCategoryName(uint16_t type)
-{
- const CategoryName *cn = categoryNames;
-
- while (cn->type) {
- if (cn->type == type) {
- return cn->name;
- }
- cn++;
- }
-
- return "unknown";
-}
-
-void Master::siiRead(int slavePosition)
-{
- ec_ioctl_slave_sii_t data;
- ec_ioctl_slave_t slave;
- unsigned int i;
- const uint16_t *categoryHeader;
- uint16_t categoryType, categorySize;
- stringstream err;
-
- if (slavePosition < 0) {
- stringstream err;
- err << "'sii_read' requires a slave! Please specify --slave.";
- throw MasterException(err.str());
- }
- data.slave_position = slavePosition;
-
- open(Read);
-
- getSlave(&slave, slavePosition);
-
- if (!slave.sii_nwords)
- return;
-
- data.offset = 0;
- data.nwords = slave.sii_nwords;
- data.words = new uint16_t[data.nwords];
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {
- stringstream err;
- delete [] data.words;
- err << "Failed to read SII: " << strerror(errno);
- throw MasterException(err.str());
- }
-
- if (verbosity == Verbose) {
- cout << "SII Area:" << hex << setfill('0');
- for (i = 0; i < min(data.nwords, 0x0040U) * 2; i++) {
- if (i % BreakAfterBytes) {
- cout << " ";
- } else {
- cout << endl << " ";
- }
- cout << setw(2) << (unsigned int) *((uint8_t *) data.words + i);
- }
- cout << endl;
-
- if (data.nwords > 0x0040U) {
- // cycle through categories
- categoryHeader = data.words + 0x0040U;
- categoryType = le16tocpu(*categoryHeader);
- while (categoryType != 0xffff) {
- cout << "SII Category 0x" << hex
- << setw(4) << categoryType
- << " (" << getCategoryName(categoryType) << ")" << flush;
-
- if (categoryHeader + 1 > data.words + data.nwords) {
- err << "SII data seem to be corrupted!";
- throw MasterException(err.str());
- }
- categorySize = le16tocpu(*(categoryHeader + 1));
- cout << ", " << dec << categorySize << " words" << flush;
-
- if (categoryHeader + 2 + categorySize
- > data.words + data.nwords) {
- err << "SII data seem to be corrupted!";
- throw MasterException(err.str());
- }
-
- cout << hex;
- for (i = 0; i < categorySize * 2U; i++) {
- if (i % BreakAfterBytes) {
- cout << " ";
- } else {
- cout << endl << " ";
- }
- cout << setw(2) << (unsigned int)
- *((uint8_t *) (categoryHeader + 2) + i);
- }
- cout << endl;
-
- if (categoryHeader + 2 + categorySize + 1
- > data.words + data.nwords) {
- err << "SII data seem to be corrupted!";
- throw MasterException(err.str());
- }
- categoryHeader += 2 + categorySize;
- categoryType = le16tocpu(*categoryHeader);
- }
- }
- } else {
- for (i = 0; i < data.nwords; i++) {
- uint16_t *w = data.words + i;
- cout << *(uint8_t *) w << *((uint8_t *) w + 1);
- }
- }
-
- delete [] data.words;
-}
-
-/****************************************************************************/
-
-void Master::siiWrite(
- int slavePosition,
- bool force,
- const vector<string> &commandArgs
- )
-{
- stringstream err;
- ec_ioctl_slave_sii_t data;
- ifstream file;
- unsigned int byte_size;
- const uint16_t *categoryHeader;
- uint16_t categoryType, categorySize;
- uint8_t crc;
-
- if (slavePosition < 0) {
- err << "'sii_write' requires a slave! Please specify --slave.";
- throw MasterException(err.str());
- }
- data.slave_position = slavePosition;
-
- if (commandArgs.size() != 1) {
- err << "'ssi_write' takes exactly one argument!";
- throw MasterException(err.str());
- }
-
- file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary);
- if (file.fail()) {
- err << "Failed to open '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
-
- // get length of file
- file.seekg(0, ios::end);
- byte_size = file.tellg();
- file.seekg(0, ios::beg);
-
- if (!byte_size || byte_size % 2) {
- stringstream err;
- err << "Invalid file size! Must be non-zero and even.";
- throw MasterException(err.str());
- }
-
- data.nwords = byte_size / 2;
- if (data.nwords < 0x0041 && !force) {
- err << "SII data too short (" << data.nwords << " words)! Mimimum is"
- " 40 fixed words + 1 delimiter. Use --force to write anyway.";
- throw MasterException(err.str());
- }
-
- // allocate buffer and read file into buffer
- data.words = new uint16_t[data.nwords];
- file.read((char *) data.words, byte_size);
- file.close();
-
- if (!force) {
- // calculate checksum over words 0 to 6
- crc = calcSiiCrc((const uint8_t *) data.words, 14);
- if (crc != ((const uint8_t *) data.words)[14]) {
- err << "CRC incorrect. Must be 0x"
- << hex << setfill('0') << setw(2) << (unsigned int) crc
- << ". Use --force to write anyway.";
- throw MasterException(err.str());
- }
-
- // cycle through categories to detect corruption
- categoryHeader = data.words + 0x0040U;
- categoryType = le16tocpu(*categoryHeader);
- while (categoryType != 0xffff) {
- if (categoryHeader + 1 > data.words + data.nwords) {
- err << "SII data seem to be corrupted! "
- << "Use --force to write anyway.";
- throw MasterException(err.str());
- }
- categorySize = le16tocpu(*(categoryHeader + 1));
- if (categoryHeader + 2 + categorySize + 1
- > data.words + data.nwords) {
- err << "SII data seem to be corrupted! "
- "Use --force to write anyway.";
- throw MasterException(err.str());
- }
- categoryHeader += 2 + categorySize;
- categoryType = le16tocpu(*categoryHeader);
- }
- }
-
- // send data to master
- open(ReadWrite);
- data.offset = 0;
- if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {
- stringstream err;
- err << "Failed to write SII: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::requestStates(
- int slavePosition,
- const vector<string> &commandArgs
- )
-{
- string stateStr;
- uint8_t state;
-
- if (commandArgs.size() != 1) {
- stringstream err;
- err << "'state' takes exactly one argument!";
- throw MasterException(err.str());
- }
-
- stateStr = commandArgs[0];
- transform(stateStr.begin(), stateStr.end(),
- stateStr.begin(), (int (*) (int)) std::toupper);
-
- if (stateStr == "INIT") {
- state = 0x01;
- } else if (stateStr == "PREOP") {
- state = 0x02;
- } else if (stateStr == "SAFEOP") {
- state = 0x04;
- } else if (stateStr == "OP") {
- state = 0x08;
- } else {
- stringstream err;
- err << "Invalid state '" << commandArgs[0] << "'!";
- throw MasterException(err.str());
- }
-
- open(ReadWrite);
-
- if (slavePosition == -1) {
- unsigned int i, numSlaves = slaveCount();
- for (i = 0; i < numSlaves; i++)
- requestState(i, state);
- } else {
- requestState(slavePosition, state);
- }
-}
-
-/****************************************************************************/
-
-void Master::generateXml(int slavePosition)
-{
- open(Read);
-
- if (slavePosition == -1) {
- unsigned int numSlaves = slaveCount(), i;
-
- for (i = 0; i < numSlaves; i++) {
- generateSlaveXml(i);
- }
- } else {
- generateSlaveXml(slavePosition);
- }
-}
-
-/****************************************************************************/
-
-void Master::open(Permissions perm)
-{
- stringstream deviceName;
-
- if (fd != -1) { // already open
- return;
- }
-
- deviceName << "/dev/EtherCAT" << index;
-
- if ((fd = ::open(deviceName.str().c_str(),
- perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
- stringstream err;
- err << "Failed to open master device " << deviceName.str() << ": "
- << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::close()
-{
- if (fd == -1)
- return;
-
- ::close(fd);
- fd = -1;
-}
-
-/*****************************************************************************/
-
-/**
- * Writes the Secondary slave address (alias) to the slave's SII.
- */
-void Master::writeSlaveAlias(
- uint16_t slavePosition,
- uint16_t alias
- )
-{
- ec_ioctl_slave_sii_t data;
- ec_ioctl_slave_t slave;
- stringstream err;
- uint8_t crc;
-
- open(ReadWrite);
-
- getSlave(&slave, slavePosition);
-
- if (slave.sii_nwords < 8) {
- err << "Current SII contents are too small to set an alias "
- << "(" << slave.sii_nwords << " words)!";
- throw MasterException(err.str());
- }
-
- data.slave_position = slavePosition;
- data.offset = 0;
- data.nwords = 8;
- data.words = new uint16_t[data.nwords];
-
- // read first 8 SII words
- if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {
- delete [] data.words;
- err << "Failed to read SII: " << strerror(errno);
- throw MasterException(err.str());
- }
-
- // write new alias address in word 4
- data.words[4] = cputole16(alias);
-
- // calculate checksum over words 0 to 6
- crc = calcSiiCrc((const uint8_t *) data.words, 14);
-
- // write new checksum into first byte of word 7
- *(uint8_t *) (data.words + 7) = crc;
-
- // write first 8 words with new alias and checksum
- if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {
- delete [] data.words;
- err << "Failed to write SII: " << strerror(errno);
- throw MasterException(err.str());
- }
-
- delete [] data.words;
-}
-
-/*****************************************************************************/
-
-/** Lists the complete bus configuration.
- */
-void Master::showDetailedConfigs(const ConfigList &configList)
-{
- ConfigList::const_iterator configIter;
- unsigned int j, k, l;
- ec_ioctl_config_pdo_t pdo;
- ec_ioctl_config_pdo_entry_t entry;
- ec_ioctl_config_sdo_t sdo;
-
- for (configIter = configList.begin();
- configIter != configList.end();
- configIter++) {
-
- cout << "Alias: "
- << dec << configIter->alias << endl
- << "Position: " << configIter->position << endl
- << "Vendor Id: 0x"
- << hex << setfill('0')
- << setw(8) << configIter->vendor_id << endl
- << "Product code: 0x"
- << setw(8) << configIter->product_code << endl
- << "Attached: " << (configIter->attached ? "yes" : "no") << endl
- << "Operational: " << (configIter->operational ? "yes" : "no") << endl;
-
- for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) {
- if (configIter->syncs[j].pdo_count) {
- cout << "SM" << dec << j << " ("
- << (configIter->syncs[j].dir == EC_DIR_INPUT
- ? "Input" : "Output") << ")" << endl;
- for (k = 0; k < configIter->syncs[j].pdo_count; k++) {
- getConfigPdo(&pdo, configIter->config_index, j, k);
-
- cout << " Pdo 0x" << hex
- << setw(4) << pdo.index
- << " \"" << pdo.name << "\"" << endl;
-
- for (l = 0; l < pdo.entry_count; l++) {
- getConfigPdoEntry(&entry, configIter->config_index, j, k, l);
-
- cout << " Pdo entry 0x" << hex
- << setw(4) << entry.index << ":"
- << setw(2) << (unsigned int) entry.subindex
- << ", " << dec << (unsigned int) entry.bit_length
- << " bit, \"" << entry.name << "\"" << endl;
- }
- }
- }
- }
-
- cout << "Sdo configuration:" << endl;
- if (configIter->sdo_count) {
- for (j = 0; j < configIter->sdo_count; j++) {
- getConfigSdo(&sdo, configIter->config_index, j);
-
- cout << " 0x"
- << hex << setfill('0')
- << setw(4) << sdo.index << ":"
- << setw(2) << (unsigned int) sdo.subindex
- << ", " << dec << sdo.size << " byte: " << hex;
-
- switch (sdo.size) {
- case 1:
- cout << "0x" << setw(2)
- << (unsigned int) *(uint8_t *) &sdo.data;
- break;
- case 2:
- cout << "0x" << setw(4)
- << le16tocpu(*(uint16_t *) &sdo.data);
- break;
- case 4:
- cout << "0x" << setw(8)
- << le32tocpu(*(uint32_t *) &sdo.data);
- break;
- default:
- cout << "???";
- }
-
- cout << endl;
- }
- } else {
- cout << " None." << endl;
- }
-
- cout << endl;
- }
-}
-
-/*****************************************************************************/
-
-struct ConfigInfo {
- string alias;
- string pos;
- string ident;
- string att;
- string op;
-};
-
-/** Lists the bus configuration.
- */
-void Master::listConfigs(const ConfigList &configList)
-{
- ConfigList::const_iterator configIter;
- stringstream str;
- ConfigInfo info;
- typedef list<ConfigInfo> ConfigInfoList;
- ConfigInfoList list;
- ConfigInfoList::const_iterator iter;
- unsigned int maxAliasWidth = 0, maxPosWidth = 0,
- maxAttWidth = 0, maxOpWidth = 0;
-
- for (configIter = configList.begin();
- configIter != configList.end();
- configIter++) {
-
- str << dec << configIter->alias;
- info.alias = str.str();
- str.clear();
- str.str("");
-
- str << configIter->position;
- info.pos = str.str();
- str.clear();
- str.str("");
-
- str << hex << setfill('0')
- << "0x" << setw(8) << configIter->vendor_id
- << "/0x" << setw(8) << configIter->product_code;
- info.ident = str.str();
- str.clear();
- str.str("");
-
- str << (configIter->attached ? "attached" : "-");
- info.att = str.str();
- str.clear();
- str.str("");
-
- str << (configIter->operational ? "operational" : "-");
- info.op = str.str();
- str.clear();
- str.str("");
-
- list.push_back(info);
-
- if (info.alias.length() > maxAliasWidth)
- maxAliasWidth = info.alias.length();
- if (info.pos.length() > maxPosWidth)
- maxPosWidth = info.pos.length();
- if (info.att.length() > maxAttWidth)
- maxAttWidth = info.att.length();
- if (info.op.length() > maxOpWidth)
- maxOpWidth = info.op.length();
- }
-
- for (iter = list.begin(); iter != list.end(); iter++) {
- cout << setfill(' ') << right
- << setw(maxAliasWidth) << iter->alias
- << ":" << left
- << setw(maxPosWidth) << iter->pos
- << " "
- << iter->ident
- << " "
- << setw(maxAttWidth) << iter->att << " "
- << setw(maxOpWidth) << iter->op << " "
- << endl;
- }
-}
-
-/****************************************************************************/
-
-void Master::outputDomainData(unsigned int domainIndex)
-{
- ec_ioctl_domain_t domain;
- ec_ioctl_domain_data_t data;
- unsigned char *processData;
- unsigned int i;
-
- getDomain(&domain, domainIndex);
-
- if (!domain.data_size)
- return;
-
- processData = new unsigned char[domain.data_size];
-
- try {
- getData(&data, domainIndex, domain.data_size, processData);
- } catch (MasterException &e) {
- delete [] processData;
- throw e;
- }
-
- for (i = 0; i < data.data_size; i++)
- cout << processData[i];
- cout.flush();
-
- delete [] processData;
-}
-
-/****************************************************************************/
-
-void Master::showDomain(unsigned int domainIndex)
-{
- ec_ioctl_domain_t domain;
- unsigned char *processData;
- ec_ioctl_domain_data_t data;
- unsigned int i, j;
- ec_ioctl_domain_fmmu_t fmmu;
- unsigned int dataOffset;
-
- getDomain(&domain, domainIndex);
-
- cout << "Domain" << dec << domainIndex << ":"
- << " LogBaseAddr 0x"
- << hex << setfill('0')
- << setw(8) << domain.logical_base_address
- << ", Size " << dec << setfill(' ')
- << setw(3) << domain.data_size
- << ", WorkingCounter "
- << domain.working_counter << "/"
- << domain.expected_working_counter << endl;
-
- if (!domain.data_size || verbosity != Verbose)
- return;
-
- processData = new unsigned char[domain.data_size];
-
- try {
- getData(&data, domainIndex, domain.data_size, processData);
- } catch (MasterException &e) {
- delete [] processData;
- throw e;
- }
-
- for (i = 0; i < domain.fmmu_count; i++) {
- getFmmu(&fmmu, domainIndex, i);
-
- cout << " SlaveConfig "
- << dec << fmmu.slave_config_alias
- << ":" << fmmu.slave_config_position
- << ", SM" << (unsigned int) fmmu.sync_index << " ("
- << setfill(' ') << setw(6)
- << (fmmu.dir == EC_DIR_INPUT ? "Input" : "Output")
- << "), LogAddr 0x"
- << hex << setfill('0')
- << setw(8) << fmmu.logical_address
- << ", Size " << dec << fmmu.data_size << endl;
-
- dataOffset = fmmu.logical_address - domain.logical_base_address;
- if (dataOffset + fmmu.data_size > domain.data_size) {
- stringstream err;
- err << "Fmmu information corrupted!";
- delete [] processData;
- throw MasterException(err.str());
- }
-
- cout << " " << hex << setfill('0');
- for (j = 0; j < fmmu.data_size; j++) {
- if (j && !(j % BreakAfterBytes))
- cout << endl << " ";
- cout << setw(2)
- << (unsigned int) *(processData + dataOffset + j) << " ";
- }
- cout << endl;
- }
-
- delete [] processData;
-}
-
-/****************************************************************************/
-
-void Master::listSlavePdos(
- uint16_t slavePosition,
- bool withHeader
- )
-{
- ec_ioctl_slave_t slave;
- ec_ioctl_slave_sync_t sync;
- ec_ioctl_slave_sync_pdo_t pdo;
- ec_ioctl_slave_sync_pdo_entry_t entry;
- unsigned int i, j, k;
-
- getSlave(&slave, slavePosition);
-
- if (withHeader)
- cout << "=== Slave " << slavePosition << " ===" << endl;
-
- for (i = 0; i < slave.sync_count; i++) {
- getSync(&sync, slavePosition, i);
-
- cout << "SM" << i << ":"
- << " PhysAddr 0x"
- << hex << setfill('0')
- << setw(4) << sync.physical_start_address
- << ", DefaultSize "
- << dec << setfill(' ') << setw(4) << sync.default_size
- << ", ControlRegister 0x"
- << hex << setfill('0') << setw(2)
- << (unsigned int) sync.control_register
- << ", Enable " << dec << (unsigned int) sync.enable
- << endl;
-
- for (j = 0; j < sync.pdo_count; j++) {
- getPdo(&pdo, slavePosition, i, j);
-
- cout << " " << (sync.control_register & 0x04 ? "R" : "T")
- << "xPdo 0x"
- << hex << setfill('0')
- << setw(4) << pdo.index
- << " \"" << pdo.name << "\"" << endl;
-
- if (verbosity == Quiet)
- continue;
-
- for (k = 0; k < pdo.entry_count; k++) {
- getPdoEntry(&entry, slavePosition, i, j, k);
-
- cout << " Pdo entry 0x"
- << hex << setfill('0')
- << setw(4) << entry.index
- << ":" << setw(2) << (unsigned int) entry.subindex
- << ", " << dec << (unsigned int) entry.bit_length
- << " bit, \"" << entry.name << "\"" << endl;
- }
- }
- }
-}
-
-/****************************************************************************/
-
-void Master::listSlaveSdos(
- uint16_t slavePosition,
- bool withHeader
- )
-{
- ec_ioctl_slave_t slave;
- ec_ioctl_slave_sdo_t sdo;
- ec_ioctl_slave_sdo_entry_t entry;
- unsigned int i, j;
- const CoEDataType *d;
-
- getSlave(&slave, slavePosition);
-
- if (withHeader)
- cout << "=== Slave " << slavePosition << " ===" << endl;
-
- for (i = 0; i < slave.sdo_count; i++) {
- getSdo(&sdo, slavePosition, i);
-
- cout << "Sdo 0x"
- << hex << setfill('0')
- << setw(4) << sdo.sdo_index
- << ", \"" << sdo.name << "\"" << endl;
-
- if (verbosity == Quiet)
- continue;
-
- for (j = 0; j <= sdo.max_subindex; j++) {
- getSdoEntry(&entry, slavePosition, -i, j);
-
- cout << " 0x" << hex << setfill('0')
- << setw(4) << sdo.sdo_index << ":"
- << setw(2) << (unsigned int) entry.sdo_entry_subindex
- << ", ";
-
- if ((d = findDataType(entry.data_type))) {
- cout << d->name;
- } else {
- cout << "type " << setw(4) << entry.data_type;
- }
-
- cout << ", " << dec << entry.bit_length << " bit, \""
- << entry.description << "\"" << endl;
- }
- }
-}
-
-/****************************************************************************/
-
-struct SlaveInfo {
- string pos;
- string alias;
- string relPos;
- string state;
- string flag;
- string name;
-};
-
-void Master::listSlaves(int slavePosition)
-{
- unsigned int numSlaves, i;
- ec_ioctl_slave_t slave;
- uint16_t lastAlias, aliasIndex;
- SlaveInfo slaveInfo;
- typedef list<SlaveInfo> SlaveInfoList;
- SlaveInfoList slaveInfoList;
- SlaveInfoList::const_iterator iter;
- stringstream str;
- unsigned int maxPosWidth = 0, maxAliasWidth = 0,
- maxRelPosWidth = 0, maxStateWidth = 0;
-
- open(Read);
-
- numSlaves = slaveCount();
-
- lastAlias = 0;
- aliasIndex = 0;
- for (i = 0; i < numSlaves; i++) {
- getSlave(&slave, i);
-
- if (slave.alias) {
- lastAlias = slave.alias;
- aliasIndex = 0;
- }
-
- if (slavePosition == -1 || i == (unsigned int) slavePosition) {
- str << dec << i;
- slaveInfo.pos = str.str();
- str.clear();
- str.str("");
-
- str << lastAlias;
- slaveInfo.alias = str.str();
- str.str("");
-
- str << aliasIndex;
- slaveInfo.relPos = str.str();
- str.str("");
-
- slaveInfo.state = slaveState(slave.state);
- slaveInfo.flag = (slave.error_flag ? 'E' : '+');
-
- if (strlen(slave.name)) {
- slaveInfo.name = slave.name;
- } else {
- str << "0x" << hex << setfill('0')
- << setw(8) << slave.vendor_id << ":0x"
- << setw(8) << slave.product_code;
- slaveInfo.name = str.str();
- str.str("");
- }
-
-
- slaveInfoList.push_back(slaveInfo);
-
- if (slaveInfo.pos.length() > maxPosWidth)
- maxPosWidth = slaveInfo.pos.length();
- if (slaveInfo.alias.length() > maxAliasWidth)
- maxAliasWidth = slaveInfo.alias.length();
- if (slaveInfo.relPos.length() > maxRelPosWidth)
- maxRelPosWidth = slaveInfo.relPos.length();
- if (slaveInfo.state.length() > maxStateWidth)
- maxStateWidth = slaveInfo.state.length();
- }
-
- aliasIndex++;
- }
-
- for (iter = slaveInfoList.begin(); iter != slaveInfoList.end(); iter++) {
- cout << setfill(' ') << right
- << setw(maxPosWidth) << iter->pos << " "
- << setw(maxAliasWidth) << iter->alias
- << ":" << left
- << setw(maxRelPosWidth) << iter->relPos << " "
- << setw(maxStateWidth) << iter->state << " "
- << iter->flag << " "
- << iter->name << endl;
- }
-}
-
-/****************************************************************************/
-
-void Master::showSlave(uint16_t slavePosition)
-{
- ec_ioctl_slave_t slave;
- list<string> protoList;
- list<string>::const_iterator protoIter;
-
- getSlave(&slave, slavePosition);
-
- cout << "=== Slave " << dec << slavePosition << " ===" << endl;
-
- if (slave.alias)
- cout << "Alias: " << slave.alias << endl;
-
- cout
- << "State: " << slaveState(slave.state) << endl
- << "Flag: " << (slave.error_flag ? 'E' : '+') << endl
- << "Identity:" << endl
- << " Vendor Id: 0x"
- << hex << setfill('0')
- << setw(8) << slave.vendor_id << endl
- << " Product code: 0x"
- << setw(8) << slave.product_code << endl
- << " Revision number: 0x"
- << setw(8) << slave.revision_number << endl
- << " Serial number: 0x"
- << setw(8) << slave.serial_number << endl;
-
- if (slave.mailbox_protocols) {
- cout << "Mailboxes:" << endl
- << " RX: 0x"
- << hex << setw(4) << slave.rx_mailbox_offset << "/"
- << dec << slave.rx_mailbox_size
- << ", TX: 0x"
- << hex << setw(4) << slave.tx_mailbox_offset << "/"
- << dec << slave.tx_mailbox_size << endl
- << " Supported protocols: ";
-
- if (slave.mailbox_protocols & EC_MBOX_AOE) {
- protoList.push_back("AoE");
- }
- if (slave.mailbox_protocols & EC_MBOX_EOE) {
- protoList.push_back("EoE");
- }
- if (slave.mailbox_protocols & EC_MBOX_COE) {
- protoList.push_back("CoE");
- }
- if (slave.mailbox_protocols & EC_MBOX_FOE) {
- protoList.push_back("FoE");
- }
- if (slave.mailbox_protocols & EC_MBOX_SOE) {
- protoList.push_back("SoE");
- }
- if (slave.mailbox_protocols & EC_MBOX_VOE) {
- protoList.push_back("VoE");
- }
-
- for (protoIter = protoList.begin(); protoIter != protoList.end();
- protoIter++) {
- if (protoIter != protoList.begin())
- cout << ", ";
- cout << *protoIter;
- }
- cout << endl;
- }
-
- if (slave.has_general_category) {
- cout << "General:" << endl
- << " Group: " << slave.group << endl
- << " Image name: " << slave.image << endl
- << " Order number: " << slave.order << endl
- << " Device name: " << slave.name << endl;
-
- if (slave.mailbox_protocols & EC_MBOX_COE) {
- cout << " CoE details:" << endl
- << " Enable Sdo: "
- << (slave.coe_details.enable_sdo ? "yes" : "no") << endl
- << " Enable Sdo Info: "
- << (slave.coe_details.enable_sdo_info ? "yes" : "no") << endl
- << " Enable Pdo Assign: "
- << (slave.coe_details.enable_pdo_assign
- ? "yes" : "no") << endl
- << " Enable Pdo Configuration: "
- << (slave.coe_details.enable_pdo_configuration
- ? "yes" : "no") << endl
- << " Enable Upload at startup: "
- << (slave.coe_details.enable_upload_at_startup
- ? "yes" : "no") << endl
- << " Enable Sdo complete access: "
- << (slave.coe_details.enable_sdo_complete_access
- ? "yes" : "no") << endl;
- }
-
- cout << " Flags:" << endl
- << " Enable SafeOp: "
- << (slave.general_flags.enable_safeop ? "yes" : "no") << endl
- << " Enable notLRW: "
- << (slave.general_flags.enable_not_lrw ? "yes" : "no") << endl
- << " Current consumption: "
- << dec << slave.current_on_ebus << " mA" << endl;
- }
-}
-
-/****************************************************************************/
-
-void Master::generateSlaveXml(uint16_t slavePosition)
-{
- ec_ioctl_slave_t slave;
- ec_ioctl_slave_sync_t sync;
- ec_ioctl_slave_sync_pdo_t pdo;
- string pdoType;
- ec_ioctl_slave_sync_pdo_entry_t entry;
- unsigned int i, j, k;
-
- getSlave(&slave, slavePosition);
-
- cout
- << "<?xml version=\"1.0\" ?>" << endl
- << " <EtherCATInfo>" << endl
- << " <!-- Slave " << slave.position << " -->" << endl
- << " <Vendor>" << endl
- << " <Id>" << slave.vendor_id << "</Id>" << endl
- << " </Vendor>" << endl
- << " <Descriptions>" << endl
- << " <Devices>" << endl
- << " <Device>" << endl
- << " <Type ProductCode=\"#x"
- << hex << setfill('0') << setw(8) << slave.product_code
- << "\" RevisionNo=\"#x"
- << hex << setfill('0') << setw(8) << slave.revision_number
- << "\">" << slave.order << "</Type>" << endl;
-
- if (strlen(slave.name)) {
- cout
- << " <Name><![CDATA["
- << slave.name
- << "]]></Name>" << endl;
- }
-
- for (i = 0; i < slave.sync_count; i++) {
- getSync(&sync, slavePosition, i);
-
- cout
- << " <Sm Enable=\"" << dec << (unsigned int) sync.enable
- << "\" StartAddress=\"" << sync.physical_start_address
- << "\" ControlByte=\"" << (unsigned int) sync.control_register
- << "\" DefaultSize=\"" << sync.default_size
- << "\" />" << endl;
- }
-
- for (i = 0; i < slave.sync_count; i++) {
- getSync(&sync, slavePosition, i);
-
- for (j = 0; j < sync.pdo_count; j++) {
- getPdo(&pdo, slavePosition, i, j);
- pdoType = (sync.control_register & 0x04 ? "R" : "T");
- pdoType += "xPdo";
-
- cout
- << " <" << pdoType
- << " Sm=\"" << i << "\" Fixed=\"1\" Mandatory=\"1\">" << endl
- << " <Index>#x"
- << hex << setfill('0') << setw(4) << pdo.index
- << "</Index>" << endl
- << " <Name>" << pdo.name << "</Name>" << endl;
-
- for (k = 0; k < pdo.entry_count; k++) {
- getPdoEntry(&entry, slavePosition, i, j, k);
-
- cout
- << " <Entry>" << endl
- << " <Index>#x"
- << hex << setfill('0') << setw(4) << entry.index
- << "</Index>" << endl;
- if (entry.index)
- cout
- << " <SubIndex>"
- << dec << (unsigned int) entry.subindex
- << "</SubIndex>" << endl;
-
- cout
- << " <BitLen>"
- << dec << (unsigned int) entry.bit_length
- << "</BitLen>" << endl;
-
- if (entry.index) {
- cout
- << " <Name>" << entry.name
- << "</Name>" << endl
- << " <DataType>";
-
- if (entry.bit_length == 1) {
- cout << "BOOL";
- } else if (!(entry.bit_length % 8)) {
- if (entry.bit_length <= 64)
- cout << "UINT" << (unsigned int) entry.bit_length;
- else
- cout << "STRING("
- << (unsigned int) (entry.bit_length / 8)
- << ")";
- } else {
- cerr << "Invalid bit length "
- << (unsigned int) entry.bit_length << endl;
- }
-
- cout << "</DataType>" << endl;
- }
-
- cout << " </Entry>" << endl;
- }
-
- cout
- << " </" << pdoType << ">" << endl;
- }
- }
-
- cout
- << " </Device>" << endl
- << " </Devices>" << endl
- << " </Descriptions>" << endl
- << "</EtherCATInfo>" << endl;
-}
-
-/****************************************************************************/
-
-unsigned int Master::slaveCount()
-{
- ec_ioctl_master_t data;
-
- getMaster(&data);
- return data.slave_count;
-}
-
-/****************************************************************************/
-
-void Master::getMaster(ec_ioctl_master_t *data)
-{
- if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
- stringstream err;
- err << "Failed to get master information: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getConfig(ec_ioctl_config_t *data, unsigned int index)
-{
- data->config_index = index;
-
- if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {
- stringstream err;
- err << "Failed to get slave configuration: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getConfigPdo(
- ec_ioctl_config_pdo_t *data,
- unsigned int index,
- uint8_t sync_index,
- uint16_t pdo_pos
- )
-{
- data->config_index = index;
- data->sync_index = sync_index;
- data->pdo_pos = pdo_pos;
-
- if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {
- stringstream err;
- err << "Failed to get slave config Pdo: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getConfigPdoEntry(
- ec_ioctl_config_pdo_entry_t *data,
- unsigned int index,
- uint8_t sync_index,
- uint16_t pdo_pos,
- uint8_t entry_pos
- )
-{
- data->config_index = index;
- data->sync_index = sync_index;
- data->pdo_pos = pdo_pos;
- data->entry_pos = entry_pos;
-
- if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {
- stringstream err;
- err << "Failed to get slave config Pdo entry: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getConfigSdo(
- ec_ioctl_config_sdo_t *data,
- unsigned int index,
- unsigned int sdo_pos
- )
-{
- data->config_index = index;
- data->sdo_pos = sdo_pos;
-
- if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {
- stringstream err;
- err << "Failed to get slave config Sdo: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getDomain(ec_ioctl_domain_t *data, unsigned int index)
-{
- data->index = index;
-
- if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
- stringstream err;
- err << "Failed to get domain: ";
- if (errno == EINVAL)
- err << "Domain " << index << " does not exist!";
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getData(ec_ioctl_domain_data_t *data, unsigned int domainIndex,
- unsigned int dataSize, unsigned char *mem)
-{
- data->domain_index = domainIndex;
- data->data_size = dataSize;
- data->target = mem;
-
- if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {
- stringstream err;
- err << "Failed to get domain data: " << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
-{
- slave->position = slaveIndex;
-
- if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
- stringstream err;
- err << "Failed to get slave: ";
- if (errno == EINVAL)
- err << "Slave " << slaveIndex << " does not exist!";
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getFmmu(
- ec_ioctl_domain_fmmu_t *fmmu,
- unsigned int domainIndex,
- unsigned int fmmuIndex
- )
-{
- fmmu->domain_index = domainIndex;
- fmmu->fmmu_index = fmmuIndex;
-
- if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
- stringstream err;
- err << "Failed to get domain FMMU: ";
- if (errno == EINVAL)
- err << "Either domain " << domainIndex << " does not exist, "
- << "or it contains less than " << (unsigned int) fmmuIndex + 1
- << " FMMus!";
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getSync(
- ec_ioctl_slave_sync_t *sync,
- uint16_t slaveIndex,
- uint8_t syncIndex
- )
-{
- sync->slave_position = slaveIndex;
- sync->sync_index = syncIndex;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) {
- stringstream err;
- err << "Failed to get sync manager: ";
- if (errno == EINVAL)
- err << "Either slave " << slaveIndex << " does not exist, "
- << "or it contains less than " << (unsigned int) syncIndex + 1
- << " sync managers!";
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getPdo(
- ec_ioctl_slave_sync_pdo_t *pdo,
- uint16_t slaveIndex,
- uint8_t syncIndex,
- uint8_t pdoPos
- )
-{
- pdo->slave_position = slaveIndex;
- pdo->sync_index = syncIndex;
- pdo->pdo_pos = pdoPos;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) {
- stringstream err;
- err << "Failed to get Pdo: ";
- if (errno == EINVAL)
- err << "Either slave " << slaveIndex << " does not exist, "
- << "or it contains less than " << (unsigned int) syncIndex + 1
- << " sync managers, or sync manager "
- << (unsigned int) syncIndex << " contains less than "
- << pdoPos + 1 << " Pdos!" << endl;
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getPdoEntry(
- ec_ioctl_slave_sync_pdo_entry_t *entry,
- uint16_t slaveIndex,
- uint8_t syncIndex,
- uint8_t pdoPos,
- uint8_t entryPos
- )
-{
- entry->slave_position = slaveIndex;
- entry->sync_index = syncIndex;
- entry->pdo_pos = pdoPos;
- entry->entry_pos = entryPos;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) {
- stringstream err;
- err << "Failed to get Pdo entry: ";
- if (errno == EINVAL)
- err << "Either slave " << slaveIndex << " does not exist, "
- << "or it contains less than " << (unsigned int) syncIndex + 1
- << " sync managers, or sync manager "
- << (unsigned int) syncIndex << " contains less than "
- << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
- << " contains less than " << (unsigned int) entryPos + 1
- << " entries!" << endl;
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getSdo(
- ec_ioctl_slave_sdo_t *sdo,
- uint16_t slaveIndex,
- uint16_t sdoPosition
- )
-{
- sdo->slave_position = slaveIndex;
- sdo->sdo_position = sdoPosition;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {
- stringstream err;
- err << "Failed to get Sdo: ";
- if (errno == EINVAL)
- err << "Either slave " << slaveIndex << " does not exist, "
- << "or it contains less than " << sdoPosition + 1 << " Sdos!"
- << endl;
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::getSdoEntry(
- ec_ioctl_slave_sdo_entry_t *entry,
- uint16_t slaveIndex,
- int sdoSpec,
- uint8_t entrySubindex
- )
-{
- entry->slave_position = slaveIndex;
- entry->sdo_spec = sdoSpec;
- entry->sdo_entry_subindex = entrySubindex;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) {
- stringstream err;
- err << "Failed to get Sdo entry: ";
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-void Master::requestState(
- uint16_t slavePosition,
- uint8_t state
- )
-{
- ec_ioctl_slave_state_t data;
-
- data.slave_position = slavePosition;
- data.requested_state = state;
-
- if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
- stringstream err;
- err << "Failed to request slave state: ";
- if (errno == EINVAL)
- err << "Slave " << slavePosition << " does not exist!";
- else
- err << strerror(errno);
- throw MasterException(err.str());
- }
-}
-
-/****************************************************************************/
-
-string Master::slaveState(uint8_t state)
-{
- switch (state) {
- case 1: return "INIT";
- case 2: return "PREOP";
- case 4: return "SAFEOP";
- case 8: return "OP";
- default: return "???";
- }
-}
-
-/****************************************************************************/
-
-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;
-}
-
-/*****************************************************************************/
-
-/**
- * Calculates the SII checksum field.
- *
- * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an
- * initial value of 0xff (see IEC 61158-6-12 ch. 5.4).
- *
- * The below code was originally generated with PYCRC
- * http://www.tty1.net/pycrc
- *
- * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff
- * --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit
- *
- * \return CRC8
- */
-uint8_t Master::calcSiiCrc(
- const uint8_t *data, /**< pointer to data */
- size_t length /**< number of bytes in \a data */
- )
-{
- unsigned int i;
- uint8_t bit, byte, crc = 0x48;
-
- while (length--) {
- byte = *data++;
- for (i = 0; i < 8; i++) {
- bit = crc & 0x80;
- crc = (crc << 1) | ((byte >> (7 - i)) & 0x01);
- if (bit) crc ^= 0x07;
- }
- }
-
- for (i = 0; i < 8; i++) {
- bit = crc & 0x80;
- crc <<= 1;
- if (bit) crc ^= 0x07;
- }
-
- return crc;
-}
-
-/*****************************************************************************/
--- a/tool/Master.h Mon Jul 21 22:06:33 2008 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,121 +0,0 @@
-/*****************************************************************************
- *
- * $Id$
- *
- ****************************************************************************/
-
-#ifndef __EC_MASTER_H__
-#define __EC_MASTER_H__
-
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <list>
-using namespace std;
-
-#include "../include/ecrt.h"
-#include "../master/ioctl.h"
-
-/****************************************************************************/
-
-class MasterException:
- public runtime_error
-{
- public:
- /** Constructor with std::string parameter. */
- MasterException(
- const string &s /**< Message. */
- ): runtime_error(s) {}
-
- /** Constructor with const char pointer parameter. */
- MasterException(
- const char *s /**< Message. */
- ): runtime_error(s) {}
-};
-
-/****************************************************************************/
-
-class Master
-{
- public:
- Master();
- ~Master();
-
- void setIndex(unsigned int);
-
- enum Verbosity {
- Quiet,
- Normal,
- Verbose
- };
- void setVerbosity(Verbosity);
-
- void writeAlias(int, bool, const vector<string> &);
- void showConfigs();
- void outputData(int);
- void setDebug(const vector<string> &);
- void showDomains(int);
- void showMaster();
- void listPdos(int);
- void listSdos(int);
- void sdoDownload(int, const string &, const vector<string> &);
- void sdoUpload(int, const string &, const vector<string> &);
- void showSlaves(int);
- void siiRead(int);
- void siiWrite(int, bool, const vector<string> &);
- void requestStates(int, const vector<string> &);
- void generateXml(int);
-
- protected:
- enum Permissions {Read, ReadWrite};
- void open(Permissions);
- void close();
-
- void writeSlaveAlias(uint16_t, uint16_t);
- typedef list<ec_ioctl_config_t> ConfigList;
- void showDetailedConfigs(const ConfigList &);
- void listConfigs(const ConfigList &);
- void outputDomainData(unsigned int);
- enum {BreakAfterBytes = 16};
- void showDomain(unsigned int);
- void listSlavePdos(uint16_t, bool = false);
- void listSlaveSdos(uint16_t, bool = false);
- void listSlaves(int);
- void showSlave(uint16_t);
- void generateSlaveXml(uint16_t);
- unsigned int slaveCount();
- void getMaster(ec_ioctl_master_t *);
- void getConfig(ec_ioctl_config_t *, unsigned int);
- void getConfigPdo(ec_ioctl_config_pdo_t *, unsigned int, uint8_t,
- uint16_t);
- void getConfigPdoEntry(ec_ioctl_config_pdo_entry_t *, unsigned int,
- uint8_t, uint16_t, uint8_t);
- void getConfigSdo(ec_ioctl_config_sdo_t *, unsigned int, unsigned int);
- void getDomain(ec_ioctl_domain_t *, unsigned int);
- void getFmmu(ec_ioctl_domain_fmmu_t *, unsigned int, unsigned int);
- void getData(ec_ioctl_domain_data_t *, unsigned int, unsigned int,
- unsigned char *);
- void getSlave(ec_ioctl_slave_t *, uint16_t);
- void getSync(ec_ioctl_slave_sync_t *, uint16_t, uint8_t);
- void getPdo(ec_ioctl_slave_sync_pdo_t *, uint16_t, uint8_t, uint8_t);
- void getPdoEntry(ec_ioctl_slave_sync_pdo_entry_t *, uint16_t, uint8_t,
- uint8_t, uint8_t);
- void getSdo(ec_ioctl_slave_sdo_t *, uint16_t, uint16_t);
- void getSdoEntry(ec_ioctl_slave_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);
- static uint8_t calcSiiCrc(const uint8_t *, unsigned int);
-
- private:
- enum {DefaultBufferSize = 1024};
-
- unsigned int index;
- Verbosity verbosity;
- int fd;
-};
-
-/****************************************************************************/
-
-#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/MasterDevice.cpp Mon Jul 21 22:20:23 2008 +0000
@@ -0,0 +1,2021 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ ****************************************************************************/
+
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+
+#include <sstream>
+using namespace std;
+
+#if 0
+#include <iostream>
+#include <fstream>
+#include <cctype> // toupper()
+#include <list>
+#endif
+
+#include "MasterDevice.h"
+
+/****************************************************************************/
+
+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;
+}
+
+/****************************************************************************/
+
+MasterDevice::MasterDevice()
+{
+ index = 0;
+ fd = -1;
+}
+
+/****************************************************************************/
+
+MasterDevice::~MasterDevice()
+{
+ close();
+}
+
+/****************************************************************************/
+
+void MasterDevice::setIndex(unsigned int i)
+{
+ index = i;
+}
+
+#if 0
+/*****************************************************************************/
+
+bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)
+{
+ return a.alias < b.alias
+ || (a.alias == b.alias && a.position < b.position);
+}
+
+/** Lists the bus configuration.
+ */
+void Master::showConfigs()
+{
+ ec_ioctl_master_t master;
+ unsigned int i;
+ ec_ioctl_config_t config;
+ ConfigList configList;
+
+ open(Read);
+ getMaster(&master);
+
+ for (i = 0; i < master.config_count; i++) {
+ getConfig(&config, i);
+ configList.push_back(config);
+ }
+
+ configList.sort();
+
+ if (verbosity == Verbose) {
+ showDetailedConfigs(configList);
+ } else {
+ listConfigs(configList);
+ }
+}
+
+/****************************************************************************/
+
+void Master::outputData(int domainIndex)
+{
+ open(Read);
+
+ if (domainIndex == -1) {
+ unsigned int i;
+ ec_ioctl_master_t master;
+
+ getMaster(&master);
+
+ for (i = 0; i < master.domain_count; i++) {
+ outputDomainData(i);
+ }
+ } else {
+ outputDomainData(domainIndex);
+ }
+}
+
+/****************************************************************************/
+
+void Master::setDebug(const vector<string> &commandArgs)
+{
+ stringstream str;
+ int debugLevel;
+
+ if (commandArgs.size() != 1) {
+ stringstream err;
+ err << "'debug' takes exactly one argument!";
+ throw MasterDeviceException(err.str());
+ }
+
+ str << commandArgs[0];
+ str >> resetiosflags(ios::basefield) // guess base from prefix
+ >> debugLevel;
+
+ if (str.fail()) {
+ stringstream err;
+ err << "Invalid debug level '" << commandArgs[0] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ open(ReadWrite);
+
+ if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {
+ stringstream err;
+ err << "Failed to set debug level: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void Master::showDomains(int domainIndex)
+{
+ open(Read);
+
+ if (domainIndex == -1) {
+ unsigned int i;
+ ec_ioctl_master_t master;
+
+ getMaster(&master);
+
+ for (i = 0; i < master.domain_count; i++) {
+ showDomain(i);
+ }
+ } else {
+ showDomain(domainIndex);
+ }
+}
+
+/****************************************************************************/
+
+void Master::showMaster()
+{
+ ec_ioctl_master_t data;
+ stringstream err;
+ unsigned int i;
+
+ open(Read);
+ getMaster(&data);
+
+ cout
+ << "Master" << index << endl
+ << " Phase: ";
+
+ switch (data.phase) {
+ case 0: cout << "Waiting for device..."; break;
+ case 1: cout << "Idle"; break;
+ case 2: cout << "Operation"; break;
+ default:
+ err << "Invalid master phase " << data.phase;
+ throw MasterDeviceException(err.str());
+ }
+
+ cout << endl
+ << " Slaves: " << data.slave_count << endl;
+
+ for (i = 0; i < 2; i++) {
+ cout << " Device" << i << ": ";
+ if (data.devices[i].address[0] == 0x00
+ && data.devices[i].address[1] == 0x00
+ && data.devices[i].address[2] == 0x00
+ && data.devices[i].address[3] == 0x00
+ && data.devices[i].address[4] == 0x00
+ && data.devices[i].address[5] == 0x00) {
+ cout << "None.";
+ } else {
+ cout << hex << setfill('0')
+ << setw(2) << (unsigned int) data.devices[i].address[0] << ":"
+ << setw(2) << (unsigned int) data.devices[i].address[1] << ":"
+ << setw(2) << (unsigned int) data.devices[i].address[2] << ":"
+ << setw(2) << (unsigned int) data.devices[i].address[3] << ":"
+ << setw(2) << (unsigned int) data.devices[i].address[4] << ":"
+ << setw(2) << (unsigned int) data.devices[i].address[5] << " ("
+ << (data.devices[i].attached ? "attached" : "waiting...")
+ << ")" << endl << dec
+ << " Tx count: " << data.devices[i].tx_count << endl
+ << " Rx count: " << data.devices[i].rx_count;
+ }
+ cout << endl;
+ }
+}
+
+/****************************************************************************/
+
+void Master::listPdos(int slavePosition)
+{
+ open(Read);
+
+ if (slavePosition == -1) {
+ unsigned int numSlaves = slaveCount(), i;
+
+ for (i = 0; i < numSlaves; i++) {
+ listSlavePdos(i, true);
+ }
+ } else {
+ listSlavePdos(slavePosition, false);
+ }
+}
+
+/****************************************************************************/
+
+void Master::listSdos(int slavePosition)
+{
+ open(Read);
+
+ if (slavePosition == -1) {
+ unsigned int numSlaves = slaveCount(), i;
+
+ for (i = 0; i < numSlaves; i++) {
+ listSlaveSdos(i, true);
+ }
+ } else {
+ listSlaveSdos(slavePosition, false);
+ }
+}
+
+/****************************************************************************/
+
+void Master::sdoDownload(
+ int slavePosition,
+ const string &dataTypeStr,
+ const vector<string> &commandArgs
+ )
+{
+ stringstream strIndex, strSubIndex, strValue, err;
+ ec_ioctl_slave_sdo_download_t data;
+ unsigned int number;
+ const CoEDataType *dataType = NULL;
+
+ if (slavePosition < 0) {
+ err << "'sdo_download' requires a slave! Please specify --slave.";
+ throw MasterDeviceException(err.str());
+ }
+ data.slave_position = slavePosition;
+
+ if (commandArgs.size() != 3) {
+ err << "'sdo_download' takes 3 arguments!";
+ throw MasterDeviceException(err.str());
+ }
+
+ strIndex << commandArgs[0];
+ strIndex
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> data.sdo_index;
+ if (strIndex.fail()) {
+ err << "Invalid Sdo index '" << commandArgs[0] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ strSubIndex << commandArgs[1];
+ strSubIndex
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> number;
+ if (strSubIndex.fail() || number > 0xff) {
+ err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+ data.sdo_entry_subindex = number;
+
+ if (dataTypeStr != "") { // data type specified
+ if (!(dataType = findDataType(dataTypeStr))) {
+ err << "Invalid data type '" << dataTypeStr << "'!";
+ throw MasterDeviceException(err.str());
+ }
+ } else { // no data type specified: fetch from dictionary
+ ec_ioctl_slave_sdo_entry_t entry;
+
+ open(ReadWrite);
+
+ try {
+ getSdoEntry(&entry, slavePosition,
+ data.sdo_index, data.sdo_entry_subindex);
+ } catch (MasterDeviceException &e) {
+ err << "Failed to determine Sdo entry data type. "
+ << "Please specify --type.";
+ throw MasterDeviceException(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 MasterDeviceException(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];
+ strValue >> resetiosflags(ios::basefield); // guess base from prefix
+ strValue.exceptions(ios::failbit);
+
+ try {
+ switch (dataType->coeCode) {
+ case 0x0002: // int8
+ {
+ int16_t val; // uint8_t is interpreted as char
+ strValue >> val;
+ if (val > 127 || val < -128)
+ throw ios::failure("Value out of range");
+ *data.data = val;
+ break;
+ }
+ case 0x0003: // int16
+ {
+ int16_t val;
+ strValue >> val;
+ *(int16_t *) data.data = cputole16(val);
+ break;
+ }
+ case 0x0004: // int32
+ {
+ int32_t val;
+ strValue >> val;
+ *(int32_t *) data.data = cputole32(val);
+ break;
+ }
+ case 0x0005: // uint8
+ {
+ uint16_t val; // uint8_t is interpreted as char
+ strValue >> val;
+ if (val > 0xff)
+ throw ios::failure("Value out of range");
+ *data.data = val;
+ break;
+ }
+ case 0x0006: // uint16
+ {
+ uint16_t val;
+ strValue >> val;
+ *(uint16_t *) data.data = cputole16(val);
+ break;
+ }
+ case 0x0007: // uint32
+ {
+ uint32_t val;
+ strValue >> val;
+ *(uint32_t *) data.data = cputole32(val);
+ break;
+ }
+ case 0x0009: // string
+ if (strValue.str().size() >= data.data_size) {
+ err << "String too large";
+ throw MasterDeviceException(err.str());
+ }
+ data.data_size = strValue.str().size();
+ strValue >> (char *) data.data;
+ break;
+
+ default:
+ delete [] data.data;
+ err << "Unknown data type 0x" << hex << dataType->coeCode;
+ throw MasterDeviceException(err.str());
+ }
+ } catch (ios::failure &e) {
+ delete [] data.data;
+ err << "Invalid value argument '" << commandArgs[2]
+ << "' for type '" << dataType->name << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ open(ReadWrite);
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &data) < 0) {
+ stringstream err;
+ err << "Failed to download Sdo: ";
+ if (errno == EIO && data.abort_code) {
+ err << "Abort code 0x" << hex << setfill('0')
+ << setw(8) << data.abort_code;
+ } else {
+ err << strerror(errno);
+ }
+ delete [] data.data;
+ throw MasterDeviceException(err.str());
+ }
+
+ delete [] data.data;
+}
+
+/****************************************************************************/
+
+void Master::sdoUpload(
+ int slavePosition,
+ const string &dataTypeStr,
+ const vector<string> &commandArgs
+ )
+{
+ stringstream strIndex, strSubIndex;
+ int sval;
+ ec_ioctl_slave_sdo_upload_t data;
+ unsigned int uval;
+ const CoEDataType *dataType = NULL;
+
+ if (slavePosition < 0) {
+ stringstream err;
+ err << "'sdo_upload' requires a slave! Please specify --slave.";
+ throw MasterDeviceException(err.str());
+ }
+ data.slave_position = slavePosition;
+
+ if (commandArgs.size() != 2) {
+ stringstream err;
+ err << "'sdo_upload' takes two arguments!";
+ throw MasterDeviceException(err.str());
+ }
+
+ strIndex << commandArgs[0];
+ strIndex
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> data.sdo_index;
+ if (strIndex.fail()) {
+ stringstream err;
+ err << "Invalid Sdo index '" << commandArgs[0] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ strSubIndex << commandArgs[1];
+ strSubIndex
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> uval;
+ if (strSubIndex.fail() || uval > 0xff) {
+ stringstream err;
+ err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+ data.sdo_entry_subindex = uval;
+
+ if (dataTypeStr != "") { // data type specified
+ if (!(dataType = findDataType(dataTypeStr))) {
+ stringstream err;
+ err << "Invalid data type '" << dataTypeStr << "'!";
+ throw MasterDeviceException(err.str());
+ }
+ } else { // no data type specified: fetch from dictionary
+ ec_ioctl_slave_sdo_entry_t entry;
+
+ open(Read);
+
+ try {
+ getSdoEntry(&entry, slavePosition,
+ data.sdo_index, data.sdo_entry_subindex);
+ } catch (MasterDeviceException &e) {
+ stringstream err;
+ err << "Failed to determine Sdo entry data type. "
+ << "Please specify --type.";
+ throw MasterDeviceException(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 MasterDeviceException(err.str());
+ }
+ }
+
+ if (dataType->byteSize) {
+ data.target_size = dataType->byteSize;
+ } else {
+ data.target_size = DefaultBufferSize;
+ }
+
+ data.target = new uint8_t[data.target_size + 1];
+
+ open(Read);
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &data) < 0) {
+ stringstream err;
+ err << "Failed to upload Sdo: ";
+ if (errno == EIO && data.abort_code) {
+ err << "Abort code 0x" << hex << setfill('0')
+ << setw(8) << data.abort_code;
+ } else {
+ err << strerror(errno);
+ }
+ delete [] data.target;
+ close();
+ throw MasterDeviceException(err.str());
+ }
+
+ close();
+
+ if (dataType->byteSize && data.data_size != dataType->byteSize) {
+ stringstream err;
+ err << "Data type mismatch. Expected " << dataType->name
+ << " with " << dataType->byteSize << " byte, but got "
+ << data.data_size << " byte.";
+ throw MasterDeviceException(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::showSlaves(int slavePosition)
+{
+ open(Read);
+
+ if (verbosity == Verbose) {
+ if (slavePosition == -1) {
+ unsigned int numSlaves = slaveCount(), i;
+
+ for (i = 0; i < numSlaves; i++) {
+ showSlave(i);
+ }
+ } else {
+ showSlave(slavePosition);
+ }
+ } else {
+ listSlaves(slavePosition);
+ }
+}
+
+/****************************************************************************/
+
+struct CategoryName {
+ uint16_t type;
+ const char *name;
+};
+
+static const CategoryName categoryNames[] = {
+ {0x000a, "STRINGS"},
+ {0x0014, "DataTypes"},
+ {0x001e, "General"},
+ {0x0028, "FMMU"},
+ {0x0029, "SyncM"},
+ {0x0032, "TXPDO"},
+ {0x0033, "RXPDO"},
+ {0x003c, "DC"},
+ {}
+};
+
+const char *getCategoryName(uint16_t type)
+{
+ const CategoryName *cn = categoryNames;
+
+ while (cn->type) {
+ if (cn->type == type) {
+ return cn->name;
+ }
+ cn++;
+ }
+
+ return "unknown";
+}
+
+void Master::siiRead(int slavePosition)
+{
+ ec_ioctl_slave_sii_t data;
+ ec_ioctl_slave_t slave;
+ unsigned int i;
+ const uint16_t *categoryHeader;
+ uint16_t categoryType, categorySize;
+ stringstream err;
+
+ if (slavePosition < 0) {
+ stringstream err;
+ err << "'sii_read' requires a slave! Please specify --slave.";
+ throw MasterDeviceException(err.str());
+ }
+ data.slave_position = slavePosition;
+
+ open(Read);
+
+ getSlave(&slave, slavePosition);
+
+ if (!slave.sii_nwords)
+ return;
+
+ data.offset = 0;
+ data.nwords = slave.sii_nwords;
+ data.words = new uint16_t[data.nwords];
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {
+ stringstream err;
+ delete [] data.words;
+ err << "Failed to read SII: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+
+ if (verbosity == Verbose) {
+ cout << "SII Area:" << hex << setfill('0');
+ for (i = 0; i < min(data.nwords, 0x0040U) * 2; i++) {
+ if (i % BreakAfterBytes) {
+ cout << " ";
+ } else {
+ cout << endl << " ";
+ }
+ cout << setw(2) << (unsigned int) *((uint8_t *) data.words + i);
+ }
+ cout << endl;
+
+ if (data.nwords > 0x0040U) {
+ // cycle through categories
+ categoryHeader = data.words + 0x0040U;
+ categoryType = le16tocpu(*categoryHeader);
+ while (categoryType != 0xffff) {
+ cout << "SII Category 0x" << hex
+ << setw(4) << categoryType
+ << " (" << getCategoryName(categoryType) << ")" << flush;
+
+ if (categoryHeader + 1 > data.words + data.nwords) {
+ err << "SII data seem to be corrupted!";
+ throw MasterDeviceException(err.str());
+ }
+ categorySize = le16tocpu(*(categoryHeader + 1));
+ cout << ", " << dec << categorySize << " words" << flush;
+
+ if (categoryHeader + 2 + categorySize
+ > data.words + data.nwords) {
+ err << "SII data seem to be corrupted!";
+ throw MasterDeviceException(err.str());
+ }
+
+ cout << hex;
+ for (i = 0; i < categorySize * 2U; i++) {
+ if (i % BreakAfterBytes) {
+ cout << " ";
+ } else {
+ cout << endl << " ";
+ }
+ cout << setw(2) << (unsigned int)
+ *((uint8_t *) (categoryHeader + 2) + i);
+ }
+ cout << endl;
+
+ if (categoryHeader + 2 + categorySize + 1
+ > data.words + data.nwords) {
+ err << "SII data seem to be corrupted!";
+ throw MasterDeviceException(err.str());
+ }
+ categoryHeader += 2 + categorySize;
+ categoryType = le16tocpu(*categoryHeader);
+ }
+ }
+ } else {
+ for (i = 0; i < data.nwords; i++) {
+ uint16_t *w = data.words + i;
+ cout << *(uint8_t *) w << *((uint8_t *) w + 1);
+ }
+ }
+
+ delete [] data.words;
+}
+
+/****************************************************************************/
+
+void Master::siiWrite(
+ int slavePosition,
+ bool force,
+ const vector<string> &commandArgs
+ )
+{
+ stringstream err;
+ ec_ioctl_slave_sii_t data;
+ ifstream file;
+ unsigned int byte_size;
+ const uint16_t *categoryHeader;
+ uint16_t categoryType, categorySize;
+ uint8_t crc;
+
+ if (slavePosition < 0) {
+ err << "'sii_write' requires a slave! Please specify --slave.";
+ throw MasterDeviceException(err.str());
+ }
+ data.slave_position = slavePosition;
+
+ if (commandArgs.size() != 1) {
+ err << "'ssi_write' takes exactly one argument!";
+ throw MasterDeviceException(err.str());
+ }
+
+ file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary);
+ if (file.fail()) {
+ err << "Failed to open '" << commandArgs[0] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ // get length of file
+ file.seekg(0, ios::end);
+ byte_size = file.tellg();
+ file.seekg(0, ios::beg);
+
+ if (!byte_size || byte_size % 2) {
+ stringstream err;
+ err << "Invalid file size! Must be non-zero and even.";
+ throw MasterDeviceException(err.str());
+ }
+
+ data.nwords = byte_size / 2;
+ if (data.nwords < 0x0041 && !force) {
+ err << "SII data too short (" << data.nwords << " words)! Mimimum is"
+ " 40 fixed words + 1 delimiter. Use --force to write anyway.";
+ throw MasterDeviceException(err.str());
+ }
+
+ // allocate buffer and read file into buffer
+ data.words = new uint16_t[data.nwords];
+ file.read((char *) data.words, byte_size);
+ file.close();
+
+ if (!force) {
+ // calculate checksum over words 0 to 6
+ crc = calcSiiCrc((const uint8_t *) data.words, 14);
+ if (crc != ((const uint8_t *) data.words)[14]) {
+ err << "CRC incorrect. Must be 0x"
+ << hex << setfill('0') << setw(2) << (unsigned int) crc
+ << ". Use --force to write anyway.";
+ throw MasterDeviceException(err.str());
+ }
+
+ // cycle through categories to detect corruption
+ categoryHeader = data.words + 0x0040U;
+ categoryType = le16tocpu(*categoryHeader);
+ while (categoryType != 0xffff) {
+ if (categoryHeader + 1 > data.words + data.nwords) {
+ err << "SII data seem to be corrupted! "
+ << "Use --force to write anyway.";
+ throw MasterDeviceException(err.str());
+ }
+ categorySize = le16tocpu(*(categoryHeader + 1));
+ if (categoryHeader + 2 + categorySize + 1
+ > data.words + data.nwords) {
+ err << "SII data seem to be corrupted! "
+ "Use --force to write anyway.";
+ throw MasterDeviceException(err.str());
+ }
+ categoryHeader += 2 + categorySize;
+ categoryType = le16tocpu(*categoryHeader);
+ }
+ }
+
+ // send data to master
+ open(ReadWrite);
+ data.offset = 0;
+ if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {
+ stringstream err;
+ err << "Failed to write SII: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void Master::requestStates(
+ int slavePosition,
+ const vector<string> &commandArgs
+ )
+{
+ string stateStr;
+ uint8_t state;
+
+ if (commandArgs.size() != 1) {
+ stringstream err;
+ err << "'state' takes exactly one argument!";
+ throw MasterDeviceException(err.str());
+ }
+
+ stateStr = commandArgs[0];
+ transform(stateStr.begin(), stateStr.end(),
+ stateStr.begin(), (int (*) (int)) std::toupper);
+
+ if (stateStr == "INIT") {
+ state = 0x01;
+ } else if (stateStr == "PREOP") {
+ state = 0x02;
+ } else if (stateStr == "SAFEOP") {
+ state = 0x04;
+ } else if (stateStr == "OP") {
+ state = 0x08;
+ } else {
+ stringstream err;
+ err << "Invalid state '" << commandArgs[0] << "'!";
+ throw MasterDeviceException(err.str());
+ }
+
+ open(ReadWrite);
+
+ if (slavePosition == -1) {
+ unsigned int i, numSlaves = slaveCount();
+ for (i = 0; i < numSlaves; i++)
+ requestState(i, state);
+ } else {
+ requestState(slavePosition, state);
+ }
+}
+
+/****************************************************************************/
+
+void Master::generateXml(int slavePosition)
+{
+ open(Read);
+
+ if (slavePosition == -1) {
+ unsigned int numSlaves = slaveCount(), i;
+
+ for (i = 0; i < numSlaves; i++) {
+ generateSlaveXml(i);
+ }
+ } else {
+ generateSlaveXml(slavePosition);
+ }
+}
+
+#endif
+/****************************************************************************/
+
+void MasterDevice::open(Permissions perm)
+{
+ stringstream deviceName;
+
+ if (fd != -1) { // already open
+ return;
+ }
+
+ deviceName << "/dev/EtherCAT" << index;
+
+ if ((fd = ::open(deviceName.str().c_str(),
+ perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
+ stringstream err;
+ err << "Failed to open master device " << deviceName.str() << ": "
+ << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::close()
+{
+ if (fd == -1)
+ return;
+
+ ::close(fd);
+ fd = -1;
+}
+
+#if 0
+
+/*****************************************************************************/
+
+/** Lists the complete bus configuration.
+ */
+void Master::showDetailedConfigs(const ConfigList &configList)
+{
+ ConfigList::const_iterator configIter;
+ unsigned int j, k, l;
+ ec_ioctl_config_pdo_t pdo;
+ ec_ioctl_config_pdo_entry_t entry;
+ ec_ioctl_config_sdo_t sdo;
+
+ for (configIter = configList.begin();
+ configIter != configList.end();
+ configIter++) {
+
+ cout << "Alias: "
+ << dec << configIter->alias << endl
+ << "Position: " << configIter->position << endl
+ << "Vendor Id: 0x"
+ << hex << setfill('0')
+ << setw(8) << configIter->vendor_id << endl
+ << "Product code: 0x"
+ << setw(8) << configIter->product_code << endl
+ << "Attached: " << (configIter->attached ? "yes" : "no") << endl
+ << "Operational: " << (configIter->operational ? "yes" : "no") << endl;
+
+ for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) {
+ if (configIter->syncs[j].pdo_count) {
+ cout << "SM" << dec << j << " ("
+ << (configIter->syncs[j].dir == EC_DIR_INPUT
+ ? "Input" : "Output") << ")" << endl;
+ for (k = 0; k < configIter->syncs[j].pdo_count; k++) {
+ getConfigPdo(&pdo, configIter->config_index, j, k);
+
+ cout << " Pdo 0x" << hex
+ << setw(4) << pdo.index
+ << " \"" << pdo.name << "\"" << endl;
+
+ for (l = 0; l < pdo.entry_count; l++) {
+ getConfigPdoEntry(&entry, configIter->config_index, j, k, l);
+
+ cout << " Pdo entry 0x" << hex
+ << setw(4) << entry.index << ":"
+ << setw(2) << (unsigned int) entry.subindex
+ << ", " << dec << (unsigned int) entry.bit_length
+ << " bit, \"" << entry.name << "\"" << endl;
+ }
+ }
+ }
+ }
+
+ cout << "Sdo configuration:" << endl;
+ if (configIter->sdo_count) {
+ for (j = 0; j < configIter->sdo_count; j++) {
+ getConfigSdo(&sdo, configIter->config_index, j);
+
+ cout << " 0x"
+ << hex << setfill('0')
+ << setw(4) << sdo.index << ":"
+ << setw(2) << (unsigned int) sdo.subindex
+ << ", " << dec << sdo.size << " byte: " << hex;
+
+ switch (sdo.size) {
+ case 1:
+ cout << "0x" << setw(2)
+ << (unsigned int) *(uint8_t *) &sdo.data;
+ break;
+ case 2:
+ cout << "0x" << setw(4)
+ << le16tocpu(*(uint16_t *) &sdo.data);
+ break;
+ case 4:
+ cout << "0x" << setw(8)
+ << le32tocpu(*(uint32_t *) &sdo.data);
+ break;
+ default:
+ cout << "???";
+ }
+
+ cout << endl;
+ }
+ } else {
+ cout << " None." << endl;
+ }
+
+ cout << endl;
+ }
+}
+
+/*****************************************************************************/
+
+struct ConfigInfo {
+ string alias;
+ string pos;
+ string ident;
+ string att;
+ string op;
+};
+
+/** Lists the bus configuration.
+ */
+void Master::listConfigs(const ConfigList &configList)
+{
+ ConfigList::const_iterator configIter;
+ stringstream str;
+ ConfigInfo info;
+ typedef list<ConfigInfo> ConfigInfoList;
+ ConfigInfoList list;
+ ConfigInfoList::const_iterator iter;
+ unsigned int maxAliasWidth = 0, maxPosWidth = 0,
+ maxAttWidth = 0, maxOpWidth = 0;
+
+ for (configIter = configList.begin();
+ configIter != configList.end();
+ configIter++) {
+
+ str << dec << configIter->alias;
+ info.alias = str.str();
+ str.clear();
+ str.str("");
+
+ str << configIter->position;
+ info.pos = str.str();
+ str.clear();
+ str.str("");
+
+ str << hex << setfill('0')
+ << "0x" << setw(8) << configIter->vendor_id
+ << "/0x" << setw(8) << configIter->product_code;
+ info.ident = str.str();
+ str.clear();
+ str.str("");
+
+ str << (configIter->attached ? "attached" : "-");
+ info.att = str.str();
+ str.clear();
+ str.str("");
+
+ str << (configIter->operational ? "operational" : "-");
+ info.op = str.str();
+ str.clear();
+ str.str("");
+
+ list.push_back(info);
+
+ if (info.alias.length() > maxAliasWidth)
+ maxAliasWidth = info.alias.length();
+ if (info.pos.length() > maxPosWidth)
+ maxPosWidth = info.pos.length();
+ if (info.att.length() > maxAttWidth)
+ maxAttWidth = info.att.length();
+ if (info.op.length() > maxOpWidth)
+ maxOpWidth = info.op.length();
+ }
+
+ for (iter = list.begin(); iter != list.end(); iter++) {
+ cout << setfill(' ') << right
+ << setw(maxAliasWidth) << iter->alias
+ << ":" << left
+ << setw(maxPosWidth) << iter->pos
+ << " "
+ << iter->ident
+ << " "
+ << setw(maxAttWidth) << iter->att << " "
+ << setw(maxOpWidth) << iter->op << " "
+ << endl;
+ }
+}
+
+/****************************************************************************/
+
+void Master::outputDomainData(unsigned int domainIndex)
+{
+ ec_ioctl_domain_t domain;
+ ec_ioctl_domain_data_t data;
+ unsigned char *processData;
+ unsigned int i;
+
+ getDomain(&domain, domainIndex);
+
+ if (!domain.data_size)
+ return;
+
+ processData = new unsigned char[domain.data_size];
+
+ try {
+ getData(&data, domainIndex, domain.data_size, processData);
+ } catch (MasterDeviceException &e) {
+ delete [] processData;
+ throw e;
+ }
+
+ for (i = 0; i < data.data_size; i++)
+ cout << processData[i];
+ cout.flush();
+
+ delete [] processData;
+}
+
+/****************************************************************************/
+
+void Master::showDomain(unsigned int domainIndex)
+{
+ ec_ioctl_domain_t domain;
+ unsigned char *processData;
+ ec_ioctl_domain_data_t data;
+ unsigned int i, j;
+ ec_ioctl_domain_fmmu_t fmmu;
+ unsigned int dataOffset;
+
+ getDomain(&domain, domainIndex);
+
+ cout << "Domain" << dec << domainIndex << ":"
+ << " LogBaseAddr 0x"
+ << hex << setfill('0')
+ << setw(8) << domain.logical_base_address
+ << ", Size " << dec << setfill(' ')
+ << setw(3) << domain.data_size
+ << ", WorkingCounter "
+ << domain.working_counter << "/"
+ << domain.expected_working_counter << endl;
+
+ if (!domain.data_size || verbosity != Verbose)
+ return;
+
+ processData = new unsigned char[domain.data_size];
+
+ try {
+ getData(&data, domainIndex, domain.data_size, processData);
+ } catch (MasterDeviceException &e) {
+ delete [] processData;
+ throw e;
+ }
+
+ for (i = 0; i < domain.fmmu_count; i++) {
+ getFmmu(&fmmu, domainIndex, i);
+
+ cout << " SlaveConfig "
+ << dec << fmmu.slave_config_alias
+ << ":" << fmmu.slave_config_position
+ << ", SM" << (unsigned int) fmmu.sync_index << " ("
+ << setfill(' ') << setw(6)
+ << (fmmu.dir == EC_DIR_INPUT ? "Input" : "Output")
+ << "), LogAddr 0x"
+ << hex << setfill('0')
+ << setw(8) << fmmu.logical_address
+ << ", Size " << dec << fmmu.data_size << endl;
+
+ dataOffset = fmmu.logical_address - domain.logical_base_address;
+ if (dataOffset + fmmu.data_size > domain.data_size) {
+ stringstream err;
+ err << "Fmmu information corrupted!";
+ delete [] processData;
+ throw MasterDeviceException(err.str());
+ }
+
+ cout << " " << hex << setfill('0');
+ for (j = 0; j < fmmu.data_size; j++) {
+ if (j && !(j % BreakAfterBytes))
+ cout << endl << " ";
+ cout << setw(2)
+ << (unsigned int) *(processData + dataOffset + j) << " ";
+ }
+ cout << endl;
+ }
+
+ delete [] processData;
+}
+
+/****************************************************************************/
+
+void Master::listSlavePdos(
+ uint16_t slavePosition,
+ bool withHeader
+ )
+{
+ ec_ioctl_slave_t slave;
+ ec_ioctl_slave_sync_t sync;
+ ec_ioctl_slave_sync_pdo_t pdo;
+ ec_ioctl_slave_sync_pdo_entry_t entry;
+ unsigned int i, j, k;
+
+ getSlave(&slave, slavePosition);
+
+ if (withHeader)
+ cout << "=== Slave " << slavePosition << " ===" << endl;
+
+ for (i = 0; i < slave.sync_count; i++) {
+ getSync(&sync, slavePosition, i);
+
+ cout << "SM" << i << ":"
+ << " PhysAddr 0x"
+ << hex << setfill('0')
+ << setw(4) << sync.physical_start_address
+ << ", DefaultSize "
+ << dec << setfill(' ') << setw(4) << sync.default_size
+ << ", ControlRegister 0x"
+ << hex << setfill('0') << setw(2)
+ << (unsigned int) sync.control_register
+ << ", Enable " << dec << (unsigned int) sync.enable
+ << endl;
+
+ for (j = 0; j < sync.pdo_count; j++) {
+ getPdo(&pdo, slavePosition, i, j);
+
+ cout << " " << (sync.control_register & 0x04 ? "R" : "T")
+ << "xPdo 0x"
+ << hex << setfill('0')
+ << setw(4) << pdo.index
+ << " \"" << pdo.name << "\"" << endl;
+
+ if (verbosity == Quiet)
+ continue;
+
+ for (k = 0; k < pdo.entry_count; k++) {
+ getPdoEntry(&entry, slavePosition, i, j, k);
+
+ cout << " Pdo entry 0x"
+ << hex << setfill('0')
+ << setw(4) << entry.index
+ << ":" << setw(2) << (unsigned int) entry.subindex
+ << ", " << dec << (unsigned int) entry.bit_length
+ << " bit, \"" << entry.name << "\"" << endl;
+ }
+ }
+ }
+}
+
+/****************************************************************************/
+
+void Master::listSlaveSdos(
+ uint16_t slavePosition,
+ bool withHeader
+ )
+{
+ ec_ioctl_slave_t slave;
+ ec_ioctl_slave_sdo_t sdo;
+ ec_ioctl_slave_sdo_entry_t entry;
+ unsigned int i, j;
+ const CoEDataType *d;
+
+ getSlave(&slave, slavePosition);
+
+ if (withHeader)
+ cout << "=== Slave " << slavePosition << " ===" << endl;
+
+ for (i = 0; i < slave.sdo_count; i++) {
+ getSdo(&sdo, slavePosition, i);
+
+ cout << "Sdo 0x"
+ << hex << setfill('0')
+ << setw(4) << sdo.sdo_index
+ << ", \"" << sdo.name << "\"" << endl;
+
+ if (verbosity == Quiet)
+ continue;
+
+ for (j = 0; j <= sdo.max_subindex; j++) {
+ getSdoEntry(&entry, slavePosition, -i, j);
+
+ cout << " 0x" << hex << setfill('0')
+ << setw(4) << sdo.sdo_index << ":"
+ << setw(2) << (unsigned int) entry.sdo_entry_subindex
+ << ", ";
+
+ if ((d = findDataType(entry.data_type))) {
+ cout << d->name;
+ } else {
+ cout << "type " << setw(4) << entry.data_type;
+ }
+
+ cout << ", " << dec << entry.bit_length << " bit, \""
+ << entry.description << "\"" << endl;
+ }
+ }
+}
+
+/****************************************************************************/
+
+struct SlaveInfo {
+ string pos;
+ string alias;
+ string relPos;
+ string state;
+ string flag;
+ string name;
+};
+
+void Master::listSlaves(int slavePosition)
+{
+ unsigned int numSlaves, i;
+ ec_ioctl_slave_t slave;
+ uint16_t lastAlias, aliasIndex;
+ SlaveInfo slaveInfo;
+ typedef list<SlaveInfo> SlaveInfoList;
+ SlaveInfoList slaveInfoList;
+ SlaveInfoList::const_iterator iter;
+ stringstream str;
+ unsigned int maxPosWidth = 0, maxAliasWidth = 0,
+ maxRelPosWidth = 0, maxStateWidth = 0;
+
+ open(Read);
+
+ numSlaves = slaveCount();
+
+ lastAlias = 0;
+ aliasIndex = 0;
+ for (i = 0; i < numSlaves; i++) {
+ getSlave(&slave, i);
+
+ if (slave.alias) {
+ lastAlias = slave.alias;
+ aliasIndex = 0;
+ }
+
+ if (slavePosition == -1 || i == (unsigned int) slavePosition) {
+ str << dec << i;
+ slaveInfo.pos = str.str();
+ str.clear();
+ str.str("");
+
+ str << lastAlias;
+ slaveInfo.alias = str.str();
+ str.str("");
+
+ str << aliasIndex;
+ slaveInfo.relPos = str.str();
+ str.str("");
+
+ slaveInfo.state = slaveState(slave.state);
+ slaveInfo.flag = (slave.error_flag ? 'E' : '+');
+
+ if (strlen(slave.name)) {
+ slaveInfo.name = slave.name;
+ } else {
+ str << "0x" << hex << setfill('0')
+ << setw(8) << slave.vendor_id << ":0x"
+ << setw(8) << slave.product_code;
+ slaveInfo.name = str.str();
+ str.str("");
+ }
+
+
+ slaveInfoList.push_back(slaveInfo);
+
+ if (slaveInfo.pos.length() > maxPosWidth)
+ maxPosWidth = slaveInfo.pos.length();
+ if (slaveInfo.alias.length() > maxAliasWidth)
+ maxAliasWidth = slaveInfo.alias.length();
+ if (slaveInfo.relPos.length() > maxRelPosWidth)
+ maxRelPosWidth = slaveInfo.relPos.length();
+ if (slaveInfo.state.length() > maxStateWidth)
+ maxStateWidth = slaveInfo.state.length();
+ }
+
+ aliasIndex++;
+ }
+
+ for (iter = slaveInfoList.begin(); iter != slaveInfoList.end(); iter++) {
+ cout << setfill(' ') << right
+ << setw(maxPosWidth) << iter->pos << " "
+ << setw(maxAliasWidth) << iter->alias
+ << ":" << left
+ << setw(maxRelPosWidth) << iter->relPos << " "
+ << setw(maxStateWidth) << iter->state << " "
+ << iter->flag << " "
+ << iter->name << endl;
+ }
+}
+
+/****************************************************************************/
+
+void Master::showSlave(uint16_t slavePosition)
+{
+ ec_ioctl_slave_t slave;
+ list<string> protoList;
+ list<string>::const_iterator protoIter;
+
+ getSlave(&slave, slavePosition);
+
+ cout << "=== Slave " << dec << slavePosition << " ===" << endl;
+
+ if (slave.alias)
+ cout << "Alias: " << slave.alias << endl;
+
+ cout
+ << "State: " << slaveState(slave.state) << endl
+ << "Flag: " << (slave.error_flag ? 'E' : '+') << endl
+ << "Identity:" << endl
+ << " Vendor Id: 0x"
+ << hex << setfill('0')
+ << setw(8) << slave.vendor_id << endl
+ << " Product code: 0x"
+ << setw(8) << slave.product_code << endl
+ << " Revision number: 0x"
+ << setw(8) << slave.revision_number << endl
+ << " Serial number: 0x"
+ << setw(8) << slave.serial_number << endl;
+
+ if (slave.mailbox_protocols) {
+ cout << "Mailboxes:" << endl
+ << " RX: 0x"
+ << hex << setw(4) << slave.rx_mailbox_offset << "/"
+ << dec << slave.rx_mailbox_size
+ << ", TX: 0x"
+ << hex << setw(4) << slave.tx_mailbox_offset << "/"
+ << dec << slave.tx_mailbox_size << endl
+ << " Supported protocols: ";
+
+ if (slave.mailbox_protocols & EC_MBOX_AOE) {
+ protoList.push_back("AoE");
+ }
+ if (slave.mailbox_protocols & EC_MBOX_EOE) {
+ protoList.push_back("EoE");
+ }
+ if (slave.mailbox_protocols & EC_MBOX_COE) {
+ protoList.push_back("CoE");
+ }
+ if (slave.mailbox_protocols & EC_MBOX_FOE) {
+ protoList.push_back("FoE");
+ }
+ if (slave.mailbox_protocols & EC_MBOX_SOE) {
+ protoList.push_back("SoE");
+ }
+ if (slave.mailbox_protocols & EC_MBOX_VOE) {
+ protoList.push_back("VoE");
+ }
+
+ for (protoIter = protoList.begin(); protoIter != protoList.end();
+ protoIter++) {
+ if (protoIter != protoList.begin())
+ cout << ", ";
+ cout << *protoIter;
+ }
+ cout << endl;
+ }
+
+ if (slave.has_general_category) {
+ cout << "General:" << endl
+ << " Group: " << slave.group << endl
+ << " Image name: " << slave.image << endl
+ << " Order number: " << slave.order << endl
+ << " Device name: " << slave.name << endl;
+
+ if (slave.mailbox_protocols & EC_MBOX_COE) {
+ cout << " CoE details:" << endl
+ << " Enable Sdo: "
+ << (slave.coe_details.enable_sdo ? "yes" : "no") << endl
+ << " Enable Sdo Info: "
+ << (slave.coe_details.enable_sdo_info ? "yes" : "no") << endl
+ << " Enable Pdo Assign: "
+ << (slave.coe_details.enable_pdo_assign
+ ? "yes" : "no") << endl
+ << " Enable Pdo Configuration: "
+ << (slave.coe_details.enable_pdo_configuration
+ ? "yes" : "no") << endl
+ << " Enable Upload at startup: "
+ << (slave.coe_details.enable_upload_at_startup
+ ? "yes" : "no") << endl
+ << " Enable Sdo complete access: "
+ << (slave.coe_details.enable_sdo_complete_access
+ ? "yes" : "no") << endl;
+ }
+
+ cout << " Flags:" << endl
+ << " Enable SafeOp: "
+ << (slave.general_flags.enable_safeop ? "yes" : "no") << endl
+ << " Enable notLRW: "
+ << (slave.general_flags.enable_not_lrw ? "yes" : "no") << endl
+ << " Current consumption: "
+ << dec << slave.current_on_ebus << " mA" << endl;
+ }
+}
+
+/****************************************************************************/
+
+void Master::generateSlaveXml(uint16_t slavePosition)
+{
+ ec_ioctl_slave_t slave;
+ ec_ioctl_slave_sync_t sync;
+ ec_ioctl_slave_sync_pdo_t pdo;
+ string pdoType;
+ ec_ioctl_slave_sync_pdo_entry_t entry;
+ unsigned int i, j, k;
+
+ getSlave(&slave, slavePosition);
+
+ cout
+ << "<?xml version=\"1.0\" ?>" << endl
+ << " <EtherCATInfo>" << endl
+ << " <!-- Slave " << slave.position << " -->" << endl
+ << " <Vendor>" << endl
+ << " <Id>" << slave.vendor_id << "</Id>" << endl
+ << " </Vendor>" << endl
+ << " <Descriptions>" << endl
+ << " <Devices>" << endl
+ << " <Device>" << endl
+ << " <Type ProductCode=\"#x"
+ << hex << setfill('0') << setw(8) << slave.product_code
+ << "\" RevisionNo=\"#x"
+ << hex << setfill('0') << setw(8) << slave.revision_number
+ << "\">" << slave.order << "</Type>" << endl;
+
+ if (strlen(slave.name)) {
+ cout
+ << " <Name><![CDATA["
+ << slave.name
+ << "]]></Name>" << endl;
+ }
+
+ for (i = 0; i < slave.sync_count; i++) {
+ getSync(&sync, slavePosition, i);
+
+ cout
+ << " <Sm Enable=\"" << dec << (unsigned int) sync.enable
+ << "\" StartAddress=\"" << sync.physical_start_address
+ << "\" ControlByte=\"" << (unsigned int) sync.control_register
+ << "\" DefaultSize=\"" << sync.default_size
+ << "\" />" << endl;
+ }
+
+ for (i = 0; i < slave.sync_count; i++) {
+ getSync(&sync, slavePosition, i);
+
+ for (j = 0; j < sync.pdo_count; j++) {
+ getPdo(&pdo, slavePosition, i, j);
+ pdoType = (sync.control_register & 0x04 ? "R" : "T");
+ pdoType += "xPdo";
+
+ cout
+ << " <" << pdoType
+ << " Sm=\"" << i << "\" Fixed=\"1\" Mandatory=\"1\">" << endl
+ << " <Index>#x"
+ << hex << setfill('0') << setw(4) << pdo.index
+ << "</Index>" << endl
+ << " <Name>" << pdo.name << "</Name>" << endl;
+
+ for (k = 0; k < pdo.entry_count; k++) {
+ getPdoEntry(&entry, slavePosition, i, j, k);
+
+ cout
+ << " <Entry>" << endl
+ << " <Index>#x"
+ << hex << setfill('0') << setw(4) << entry.index
+ << "</Index>" << endl;
+ if (entry.index)
+ cout
+ << " <SubIndex>"
+ << dec << (unsigned int) entry.subindex
+ << "</SubIndex>" << endl;
+
+ cout
+ << " <BitLen>"
+ << dec << (unsigned int) entry.bit_length
+ << "</BitLen>" << endl;
+
+ if (entry.index) {
+ cout
+ << " <Name>" << entry.name
+ << "</Name>" << endl
+ << " <DataType>";
+
+ if (entry.bit_length == 1) {
+ cout << "BOOL";
+ } else if (!(entry.bit_length % 8)) {
+ if (entry.bit_length <= 64)
+ cout << "UINT" << (unsigned int) entry.bit_length;
+ else
+ cout << "STRING("
+ << (unsigned int) (entry.bit_length / 8)
+ << ")";
+ } else {
+ cerr << "Invalid bit length "
+ << (unsigned int) entry.bit_length << endl;
+ }
+
+ cout << "</DataType>" << endl;
+ }
+
+ cout << " </Entry>" << endl;
+ }
+
+ cout
+ << " </" << pdoType << ">" << endl;
+ }
+ }
+
+ cout
+ << " </Device>" << endl
+ << " </Devices>" << endl
+ << " </Descriptions>" << endl
+ << "</EtherCATInfo>" << endl;
+}
+#endif
+/****************************************************************************/
+
+unsigned int MasterDevice::slaveCount()
+{
+ ec_ioctl_master_t data;
+
+ getMaster(&data);
+ return data.slave_count;
+}
+
+/****************************************************************************/
+
+void MasterDevice::getMaster(ec_ioctl_master_t *data)
+{
+ if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
+ stringstream err;
+ err << "Failed to get master information: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getConfig(ec_ioctl_config_t *data, unsigned int index)
+{
+ data->config_index = index;
+
+ if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {
+ stringstream err;
+ err << "Failed to get slave configuration: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getConfigPdo(
+ ec_ioctl_config_pdo_t *data,
+ unsigned int index,
+ uint8_t sync_index,
+ uint16_t pdo_pos
+ )
+{
+ data->config_index = index;
+ data->sync_index = sync_index;
+ data->pdo_pos = pdo_pos;
+
+ if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {
+ stringstream err;
+ err << "Failed to get slave config Pdo: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getConfigPdoEntry(
+ ec_ioctl_config_pdo_entry_t *data,
+ unsigned int index,
+ uint8_t sync_index,
+ uint16_t pdo_pos,
+ uint8_t entry_pos
+ )
+{
+ data->config_index = index;
+ data->sync_index = sync_index;
+ data->pdo_pos = pdo_pos;
+ data->entry_pos = entry_pos;
+
+ if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {
+ stringstream err;
+ err << "Failed to get slave config Pdo entry: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getConfigSdo(
+ ec_ioctl_config_sdo_t *data,
+ unsigned int index,
+ unsigned int sdo_pos
+ )
+{
+ data->config_index = index;
+ data->sdo_pos = sdo_pos;
+
+ if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {
+ stringstream err;
+ err << "Failed to get slave config Sdo: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getDomain(ec_ioctl_domain_t *data, unsigned int index)
+{
+ data->index = index;
+
+ if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
+ stringstream err;
+ err << "Failed to get domain: ";
+ if (errno == EINVAL)
+ err << "Domain " << index << " does not exist!";
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getData(ec_ioctl_domain_data_t *data, unsigned int domainIndex,
+ unsigned int dataSize, unsigned char *mem)
+{
+ data->domain_index = domainIndex;
+ data->data_size = dataSize;
+ data->target = mem;
+
+ if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {
+ stringstream err;
+ err << "Failed to get domain data: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
+{
+ slave->position = slaveIndex;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
+ stringstream err;
+ err << "Failed to get slave: ";
+ if (errno == EINVAL)
+ err << "Slave " << slaveIndex << " does not exist!";
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getFmmu(
+ ec_ioctl_domain_fmmu_t *fmmu,
+ unsigned int domainIndex,
+ unsigned int fmmuIndex
+ )
+{
+ fmmu->domain_index = domainIndex;
+ fmmu->fmmu_index = fmmuIndex;
+
+ if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
+ stringstream err;
+ err << "Failed to get domain FMMU: ";
+ if (errno == EINVAL)
+ err << "Either domain " << domainIndex << " does not exist, "
+ << "or it contains less than " << (unsigned int) fmmuIndex + 1
+ << " FMMus!";
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getSync(
+ ec_ioctl_slave_sync_t *sync,
+ uint16_t slaveIndex,
+ uint8_t syncIndex
+ )
+{
+ sync->slave_position = slaveIndex;
+ sync->sync_index = syncIndex;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) {
+ stringstream err;
+ err << "Failed to get sync manager: ";
+ if (errno == EINVAL)
+ err << "Either slave " << slaveIndex << " does not exist, "
+ << "or it contains less than " << (unsigned int) syncIndex + 1
+ << " sync managers!";
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getPdo(
+ ec_ioctl_slave_sync_pdo_t *pdo,
+ uint16_t slaveIndex,
+ uint8_t syncIndex,
+ uint8_t pdoPos
+ )
+{
+ pdo->slave_position = slaveIndex;
+ pdo->sync_index = syncIndex;
+ pdo->pdo_pos = pdoPos;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) {
+ stringstream err;
+ err << "Failed to get Pdo: ";
+ if (errno == EINVAL)
+ err << "Either slave " << slaveIndex << " does not exist, "
+ << "or it contains less than " << (unsigned int) syncIndex + 1
+ << " sync managers, or sync manager "
+ << (unsigned int) syncIndex << " contains less than "
+ << pdoPos + 1 << " Pdos!" << endl;
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getPdoEntry(
+ ec_ioctl_slave_sync_pdo_entry_t *entry,
+ uint16_t slaveIndex,
+ uint8_t syncIndex,
+ uint8_t pdoPos,
+ uint8_t entryPos
+ )
+{
+ entry->slave_position = slaveIndex;
+ entry->sync_index = syncIndex;
+ entry->pdo_pos = pdoPos;
+ entry->entry_pos = entryPos;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) {
+ stringstream err;
+ err << "Failed to get Pdo entry: ";
+ if (errno == EINVAL)
+ err << "Either slave " << slaveIndex << " does not exist, "
+ << "or it contains less than " << (unsigned int) syncIndex + 1
+ << " sync managers, or sync manager "
+ << (unsigned int) syncIndex << " contains less than "
+ << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
+ << " contains less than " << (unsigned int) entryPos + 1
+ << " entries!" << endl;
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getSdo(
+ ec_ioctl_slave_sdo_t *sdo,
+ uint16_t slaveIndex,
+ uint16_t sdoPosition
+ )
+{
+ sdo->slave_position = slaveIndex;
+ sdo->sdo_position = sdoPosition;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {
+ stringstream err;
+ err << "Failed to get Sdo: ";
+ if (errno == EINVAL)
+ err << "Either slave " << slaveIndex << " does not exist, "
+ << "or it contains less than " << sdoPosition + 1 << " Sdos!"
+ << endl;
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::getSdoEntry(
+ ec_ioctl_slave_sdo_entry_t *entry,
+ uint16_t slaveIndex,
+ int sdoSpec,
+ uint8_t entrySubindex
+ )
+{
+ entry->slave_position = slaveIndex;
+ entry->sdo_spec = sdoSpec;
+ entry->sdo_entry_subindex = entrySubindex;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) {
+ stringstream err;
+ err << "Failed to get Sdo entry: ";
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::readSii(
+ ec_ioctl_slave_sii_t *data
+ )
+{
+ if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, data) < 0) {
+ stringstream err;
+ err << "Failed to read SII: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+void MasterDevice::writeSii(
+ ec_ioctl_slave_sii_t *data
+ )
+{
+ if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, data) < 0) {
+ stringstream err;
+ err << "Failed to write SII: " << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+#if 0
+void MasterDevice::requestState(
+ uint16_t slavePosition,
+ uint8_t state
+ )
+{
+ ec_ioctl_slave_state_t data;
+
+ data.slave_position = slavePosition;
+ data.requested_state = state;
+
+ if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
+ stringstream err;
+ err << "Failed to request slave state: ";
+ if (errno == EINVAL)
+ err << "Slave " << slavePosition << " does not exist!";
+ else
+ err << strerror(errno);
+ throw MasterDeviceException(err.str());
+ }
+}
+
+/****************************************************************************/
+
+string Master::slaveState(uint8_t state)
+{
+ switch (state) {
+ case 1: return "INIT";
+ case 2: return "PREOP";
+ case 4: return "SAFEOP";
+ case 8: return "OP";
+ default: return "???";
+ }
+}
+
+/****************************************************************************/
+
+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;
+}
+
+#endif
+/*****************************************************************************/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/MasterDevice.h Mon Jul 21 22:20:23 2008 +0000
@@ -0,0 +1,94 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ ****************************************************************************/
+
+#ifndef __EC_MASTER_H__
+#define __EC_MASTER_H__
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <list>
+using namespace std;
+
+#include "../include/ecrt.h"
+#include "../master/ioctl.h"
+
+/****************************************************************************/
+
+class MasterDeviceException:
+ public runtime_error
+{
+ public:
+ /** Constructor with std::string parameter. */
+ MasterDeviceException(
+ const string &s /**< Message. */
+ ): runtime_error(s) {}
+};
+
+/****************************************************************************/
+
+class MasterDevice
+{
+ public:
+ MasterDevice();
+ ~MasterDevice();
+
+ void setIndex(unsigned int);
+
+ enum Permissions {Read, ReadWrite};
+ void open(Permissions);
+ void close();
+
+ unsigned int slaveCount();
+
+ void getMaster(ec_ioctl_master_t *);
+ void getConfig(ec_ioctl_config_t *, unsigned int);
+ void getConfigPdo(ec_ioctl_config_pdo_t *, unsigned int, uint8_t,
+ uint16_t);
+ void getConfigPdoEntry(ec_ioctl_config_pdo_entry_t *, unsigned int,
+ uint8_t, uint16_t, uint8_t);
+ void getConfigSdo(ec_ioctl_config_sdo_t *, unsigned int, unsigned int);
+ void getDomain(ec_ioctl_domain_t *, unsigned int);
+ void getFmmu(ec_ioctl_domain_fmmu_t *, unsigned int, unsigned int);
+ void getData(ec_ioctl_domain_data_t *, unsigned int, unsigned int,
+ unsigned char *);
+ void getSlave(ec_ioctl_slave_t *, uint16_t);
+ void getSync(ec_ioctl_slave_sync_t *, uint16_t, uint8_t);
+ void getPdo(ec_ioctl_slave_sync_pdo_t *, uint16_t, uint8_t, uint8_t);
+ void getPdoEntry(ec_ioctl_slave_sync_pdo_entry_t *, uint16_t, uint8_t,
+ uint8_t, uint8_t);
+ void getSdo(ec_ioctl_slave_sdo_t *, uint16_t, uint16_t);
+ void getSdoEntry(ec_ioctl_slave_sdo_entry_t *, uint16_t, int, uint8_t);
+ void readSii(ec_ioctl_slave_sii_t *);
+ void writeSii(ec_ioctl_slave_sii_t *);
+
+ protected:
+#if 0
+ void outputDomainData(unsigned int);
+ enum {BreakAfterBytes = 16};
+ void showDomain(unsigned int);
+ void listSlavePdos(uint16_t, bool = false);
+ void listSlaveSdos(uint16_t, bool = false);
+ void listSlaves(int);
+ void showSlave(uint16_t);
+ void generateSlaveXml(uint16_t);
+ void requestState(uint16_t, uint8_t);
+
+ static string slaveState(uint8_t);
+ static void printRawData(const uint8_t *, unsigned int);
+ static uint8_t calcSiiCrc(const uint8_t *, unsigned int);
+#endif
+
+ private:
+ //enum {DefaultBufferSize = 1024};
+
+ unsigned int index;
+ int fd;
+};
+
+/****************************************************************************/
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/cmd_alias.cpp Mon Jul 21 22:20:23 2008 +0000
@@ -0,0 +1,181 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ ****************************************************************************/
+
+#include <iostream>
+#include <iomanip>
+#include <sstream>
+using namespace std;
+
+#include "globals.h"
+
+/*****************************************************************************/
+
+const char *help_alias =
+ "[OPTIONS] <ALIAS>\n"
+ "\n"
+ "Write the secondary slave address (alias) for either\n"
+ "one or for multiple slaves.\n"
+ "\n"
+ "Arguments:\n"
+ " ALIAS must be a 16 bit unsigned integer, specified\n"
+ " either in decimal (no prefix), octal (prefix '0')\n"
+ " or hexadecimal (prefix '0x').\n"
+ "\n"
+ "Command-specific options:\n"
+ " -s <SLAVE> Write the alias of the slave with the given\n"
+ " ring position. If this option is not\n"
+ " specified, the alias of all slaves is set.\n"
+ " The --force option is required in this\n"
+ " case.\n";
+
+/*****************************************************************************/
+
+void writeSlaveAlias(uint16_t, uint16_t);
+
+/*****************************************************************************/
+
+/** Writes the Secondary slave address (alias) to the slave's SII.
+ */
+void command_alias(void)
+{
+ uint16_t alias;
+ stringstream err, strAlias;
+ int number;
+ unsigned int numSlaves, i;
+
+ if (commandArgs.size() != 1) {
+ err << "'" << command << "' takes exactly one argument!";
+ throw InvalidUsageException(err);
+ }
+
+ strAlias << commandArgs[0];
+ strAlias
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> number;
+ if (strAlias.fail() || number < 0x0000 || number > 0xffff) {
+ err << "Invalid alias '" << commandArgs[0] << "'!";
+ throw InvalidUsageException(err);
+ }
+ alias = number;
+
+ if (slavePosition == -1) {
+ if (!force) {
+ err << "This will write the alias addresses of all slaves to "
+ << alias << "! Please specify --force to proceed.";
+ throw ExecutionFailureException(err);
+ }
+
+ masterDev.open(MasterDevice::ReadWrite);
+ numSlaves = masterDev.slaveCount();
+
+ for (i = 0; i < numSlaves; i++) {
+ writeSlaveAlias(i, alias);
+ }
+ } else {
+ masterDev.open(MasterDevice::ReadWrite);
+ writeSlaveAlias(slavePosition, alias);
+ }
+}
+
+/*****************************************************************************/
+
+/** Calculates the SII checksum field.
+ *
+ * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an
+ * initial value of 0xff (see IEC 61158-6-12 ch. 5.4).
+ *
+ * The below code was originally generated with PYCRC
+ * http://www.tty1.net/pycrc
+ *
+ * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff
+ * --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit
+ *
+ * \return CRC8
+ */
+uint8_t calcSiiCrc(
+ const uint8_t *data, /**< pointer to data */
+ size_t length /**< number of bytes in \a data */
+ )
+{
+ unsigned int i;
+ uint8_t bit, byte, crc = 0x48;
+
+ while (length--) {
+ byte = *data++;
+ for (i = 0; i < 8; i++) {
+ bit = crc & 0x80;
+ crc = (crc << 1) | ((byte >> (7 - i)) & 0x01);
+ if (bit) crc ^= 0x07;
+ }
+ }
+
+ for (i = 0; i < 8; i++) {
+ bit = crc & 0x80;
+ crc <<= 1;
+ if (bit) crc ^= 0x07;
+ }
+
+ return crc;
+}
+
+/*****************************************************************************/
+
+/** Writes the Secondary slave address (alias) to the slave's SII.
+ */
+void writeSlaveAlias(
+ uint16_t slavePosition,
+ uint16_t alias
+ )
+{
+ ec_ioctl_slave_sii_t data;
+ ec_ioctl_slave_t slave;
+ stringstream err;
+ uint8_t crc;
+
+ masterDev.getSlave(&slave, slavePosition);
+
+ if (slave.sii_nwords < 8) {
+ err << "Current SII contents are too small to set an alias "
+ << "(" << slave.sii_nwords << " words)!";
+ throw ExecutionFailureException(err);
+ }
+
+ // read first 8 SII words
+ data.slave_position = slavePosition;
+ data.offset = 0;
+ data.nwords = 8;
+ data.words = new uint16_t[data.nwords];
+
+ try {
+ masterDev.readSii(&data);
+ } catch (MasterDeviceException &e) {
+ delete [] data.words;
+ err << "Failed to read SII: " << e.what();
+ throw ExecutionFailureException(err);
+ }
+
+ // write new alias address in word 4
+ data.words[4] = cputole16(alias);
+
+ // calculate checksum over words 0 to 6
+ crc = calcSiiCrc((const uint8_t *) data.words, 14);
+
+ // write new checksum into first byte of word 7
+ *(uint8_t *) (data.words + 7) = crc;
+
+ // write first 8 words with new alias and checksum
+ try {
+ masterDev.writeSii(&data);
+ } catch (MasterDeviceException &e) {
+ delete [] data.words;
+ err << "Failed to read SII: " << e.what();
+ throw ExecutionFailureException(err);
+ }
+
+ delete [] data.words;
+}
+
+/*****************************************************************************/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/cmd_config.cpp Mon Jul 21 22:20:23 2008 +0000
@@ -0,0 +1,237 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ ****************************************************************************/
+
+#include <list>
+#include <iostream>
+#include <iomanip>
+#include <sstream>
+using namespace std;
+
+#include "globals.h"
+
+/*****************************************************************************/
+
+const char *help_config =
+ "[OPTIONS]\n"
+ "\n"
+ "\n"
+ "Command-specific options:\n";
+
+/*****************************************************************************/
+
+struct ConfigInfo {
+ string alias;
+ string pos;
+ string ident;
+ string att;
+ string op;
+};
+
+typedef list<ec_ioctl_config_t> ConfigList;
+
+void showDetailedConfigs(const ConfigList &configList);
+void listConfigs(const ConfigList &configList);
+
+/*****************************************************************************/
+
+bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)
+{
+ return a.alias < b.alias
+ || (a.alias == b.alias && a.position < b.position);
+}
+
+/*****************************************************************************/
+
+/** Lists the bus configuration.
+ */
+void command_config(void)
+{
+ ec_ioctl_master_t master;
+ unsigned int i;
+ ec_ioctl_config_t config;
+ ConfigList configList;
+
+ masterDev.open(MasterDevice::Read);
+ masterDev.getMaster(&master);
+
+ for (i = 0; i < master.config_count; i++) {
+ masterDev.getConfig(&config, i);
+ configList.push_back(config);
+ }
+
+ configList.sort();
+
+ if (verbosity == Verbose) {
+ showDetailedConfigs(configList);
+ } else {
+ listConfigs(configList);
+ }
+}
+
+/*****************************************************************************/
+
+/** Lists the complete bus configuration.
+ */
+void showDetailedConfigs(const ConfigList &configList)
+{
+ ConfigList::const_iterator configIter;
+ unsigned int j, k, l;
+ ec_ioctl_config_pdo_t pdo;
+ ec_ioctl_config_pdo_entry_t entry;
+ ec_ioctl_config_sdo_t sdo;
+
+ for (configIter = configList.begin();
+ configIter != configList.end();
+ configIter++) {
+
+ cout << "Alias: "
+ << dec << configIter->alias << endl
+ << "Position: " << configIter->position << endl
+ << "Vendor Id: 0x"
+ << hex << setfill('0')
+ << setw(8) << configIter->vendor_id << endl
+ << "Product code: 0x"
+ << setw(8) << configIter->product_code << endl
+ << "Attached: " << (configIter->attached ? "yes" : "no") << endl
+ << "Operational: " << (configIter->operational ? "yes" : "no") << endl;
+
+ for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) {
+ if (configIter->syncs[j].pdo_count) {
+ cout << "SM" << dec << j << " ("
+ << (configIter->syncs[j].dir == EC_DIR_INPUT
+ ? "Input" : "Output") << ")" << endl;
+ for (k = 0; k < configIter->syncs[j].pdo_count; k++) {
+ masterDev.getConfigPdo(&pdo, configIter->config_index, j, k);
+
+ cout << " Pdo 0x" << hex
+ << setw(4) << pdo.index
+ << " \"" << pdo.name << "\"" << endl;
+
+ for (l = 0; l < pdo.entry_count; l++) {
+ masterDev.getConfigPdoEntry(&entry,
+ configIter->config_index, j, k, l);
+
+ cout << " Pdo entry 0x" << hex
+ << setw(4) << entry.index << ":"
+ << setw(2) << (unsigned int) entry.subindex
+ << ", " << dec << (unsigned int) entry.bit_length
+ << " bit, \"" << entry.name << "\"" << endl;
+ }
+ }
+ }
+ }
+
+ cout << "Sdo configuration:" << endl;
+ if (configIter->sdo_count) {
+ for (j = 0; j < configIter->sdo_count; j++) {
+ masterDev.getConfigSdo(&sdo, configIter->config_index, j);
+
+ cout << " 0x"
+ << hex << setfill('0')
+ << setw(4) << sdo.index << ":"
+ << setw(2) << (unsigned int) sdo.subindex
+ << ", " << dec << sdo.size << " byte: " << hex;
+
+ switch (sdo.size) {
+ case 1:
+ cout << "0x" << setw(2)
+ << (unsigned int) *(uint8_t *) &sdo.data;
+ break;
+ case 2:
+ cout << "0x" << setw(4)
+ << le16tocpu(*(uint16_t *) &sdo.data);
+ break;
+ case 4:
+ cout << "0x" << setw(8)
+ << le32tocpu(*(uint32_t *) &sdo.data);
+ break;
+ default:
+ cout << "???";
+ }
+
+ cout << endl;
+ }
+ } else {
+ cout << " None." << endl;
+ }
+
+ cout << endl;
+ }
+}
+
+/*****************************************************************************/
+
+/** Lists the bus configuration.
+ */
+void listConfigs(const ConfigList &configList)
+{
+ ConfigList::const_iterator configIter;
+ stringstream str;
+ ConfigInfo info;
+ typedef list<ConfigInfo> ConfigInfoList;
+ ConfigInfoList list;
+ ConfigInfoList::const_iterator iter;
+ unsigned int maxAliasWidth = 0, maxPosWidth = 0,
+ maxAttWidth = 0, maxOpWidth = 0;
+
+ for (configIter = configList.begin();
+ configIter != configList.end();
+ configIter++) {
+
+ str << dec << configIter->alias;
+ info.alias = str.str();
+ str.clear();
+ str.str("");
+
+ str << configIter->position;
+ info.pos = str.str();
+ str.clear();
+ str.str("");
+
+ str << hex << setfill('0')
+ << "0x" << setw(8) << configIter->vendor_id
+ << "/0x" << setw(8) << configIter->product_code;
+ info.ident = str.str();
+ str.clear();
+ str.str("");
+
+ str << (configIter->attached ? "attached" : "-");
+ info.att = str.str();
+ str.clear();
+ str.str("");
+
+ str << (configIter->operational ? "operational" : "-");
+ info.op = str.str();
+ str.clear();
+ str.str("");
+
+ list.push_back(info);
+
+ if (info.alias.length() > maxAliasWidth)
+ maxAliasWidth = info.alias.length();
+ if (info.pos.length() > maxPosWidth)
+ maxPosWidth = info.pos.length();
+ if (info.att.length() > maxAttWidth)
+ maxAttWidth = info.att.length();
+ if (info.op.length() > maxOpWidth)
+ maxOpWidth = info.op.length();
+ }
+
+ for (iter = list.begin(); iter != list.end(); iter++) {
+ cout << setfill(' ') << right
+ << setw(maxAliasWidth) << iter->alias
+ << ":" << left
+ << setw(maxPosWidth) << iter->pos
+ << " "
+ << iter->ident
+ << " "
+ << setw(maxAttWidth) << iter->att << " "
+ << setw(maxOpWidth) << iter->op << " "
+ << endl;
+ }
+}
+
+/*****************************************************************************/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/globals.h Mon Jul 21 22:20:23 2008 +0000
@@ -0,0 +1,89 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ ****************************************************************************/
+
+#include <sys/types.h>
+
+#include <string>
+#include <sstream>
+#include <vector>
+using namespace std;
+
+#include "MasterDevice.h"
+
+/*****************************************************************************/
+
+enum Verbosity {
+ Quiet,
+ Normal,
+ Verbose
+};
+
+extern string command;
+extern int slavePosition;
+extern int domainIndex;
+extern vector<string> commandArgs;
+extern Verbosity verbosity;
+extern string dataTypeStr;
+extern bool force;
+
+extern MasterDevice masterDev;
+
+/****************************************************************************/
+
+class InvalidUsageException:
+ public runtime_error
+{
+ public:
+ /** Constructor with std::string parameter. */
+ InvalidUsageException(
+ const stringstream &s /**< Message. */
+ ): runtime_error(s.str()) {}
+};
+
+/****************************************************************************/
+
+class ExecutionFailureException:
+ public runtime_error
+{
+ public:
+ /** Constructor with std::string parameter. */
+ ExecutionFailureException(
+ const stringstream &s /**< Message. */
+ ): runtime_error(s.str()) {}
+};
+
+/*****************************************************************************/
+
+#define swap16(x) \
+ ((uint16_t)( \
+ (((uint16_t)(x) & 0x00ffU) << 8) | \
+ (((uint16_t)(x) & 0xff00U) >> 8) ))
+#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
+
+/****************************************************************************/
--- a/tool/main.cpp Mon Jul 21 22:06:33 2008 +0000
+++ b/tool/main.cpp Mon Jul 21 22:20:23 2008 +0000
@@ -12,194 +12,58 @@
#include <vector>
using namespace std;
-#include "Master.h"
-
-/*****************************************************************************/
-
-#define DEFAULT_MASTER 0
-
-static string cmdName;
-static unsigned int masterIndex = DEFAULT_MASTER;
-static int slavePosition = -1;
-static int domainIndex = -1;
-static string command;
+#include "globals.h"
+
+/*****************************************************************************/
+
+string binaryBaseName;
+unsigned int masterIndex = 0;
+int slavePosition = -1;
+int domainIndex = -1;
+string command;
vector<string> commandArgs;
-static Master::Verbosity verbosity = Master::Normal;
+Verbosity verbosity = Normal;
string dataTypeStr;
bool force = false;
+
bool helpRequested = false;
-/*****************************************************************************/
-
-void printUsage()
-{
- cerr
- << "Usage: " << cmdName << " <COMMAND> [OPTIONS]" << endl
- << "Commands:" << endl
- << " alias Write alias addresses." << endl
- << " config Show bus configuration." << endl
- << " data Output binary domain process data." << endl
- << " debug Set the master's debug level." << endl
- << " domain Show domain information." << endl
- << " master Show master information." << endl
- << " pdos List Pdo assignment/mapping." << endl
- << " sdo_download Write an Sdo entry." << endl
- << " sdos List Sdo dictionaries." << endl
- << " sdo_upload Read an Sdo entry." << endl
- << " sii_read Output a slave's SII contents." << endl
- << " sii_write Write slave's SII contents." << endl
- << " slaves Show slaves." << endl
- << " state Request slave states." << endl
- << " xml Generate slave information xmls." << endl
- << "Global options:" << endl
- << " --master -m <master> Index of the master to use. Default: "
- << DEFAULT_MASTER << endl
- << " --slave -s <index> Positive numerical ring position,"
- << endl
- << " or 'all' for all slaves (default)."
- << endl
- << " --domain -d <index> Positive numerical index,"
- << endl
- << " or 'all' for all domains (default)."
- << endl
- << " --type -t <type> Forced Sdo data type." << endl
- << " --force -f Force action." << endl
- << " --quiet -q Output less information." << endl
- << " --verbose -v Output more information." << endl
- << " --help -h Show this help." << endl
- << "Call '" << cmdName << " <COMMAND> --help' for command-specific "
- << "help." << endl
- << "Send bug reports to " << PACKAGE_BUGREPORT << "." << endl;
-}
-
-/*****************************************************************************/
-
-void getOptions(int argc, char **argv)
-{
- int c, argCount, optionIndex, number;
- char *remainder;
-
- static struct option longOptions[] = {
- //name, has_arg, flag, val
- {"master", required_argument, NULL, 'm'},
- {"slave", required_argument, NULL, 's'},
- {"domain", required_argument, NULL, 'd'},
- {"type", required_argument, NULL, 't'},
- {"force", no_argument, NULL, 'f'},
- {"quiet", no_argument, NULL, 'q'},
- {"verbose", no_argument, NULL, 'v'},
- {"help", no_argument, NULL, 'h'},
- {}
- };
-
- do {
- c = getopt_long(argc, argv, "m:s:d:t:fqvh", longOptions, &optionIndex);
-
- switch (c) {
- case 'm':
- number = strtoul(optarg, &remainder, 0);
- if (remainder == optarg || *remainder || number < 0) {
- cerr << "Invalid master number " << optarg << "!" << endl;
- printUsage();
- exit(1);
- }
- masterIndex = number;
- break;
-
- case 's':
- if (!strcmp(optarg, "all")) {
- slavePosition = -1;
- } else {
- number = strtoul(optarg, &remainder, 0);
- if (remainder == optarg || *remainder
- || number < 0 || number > 0xFFFF) {
- cerr << "Invalid slave position "
- << optarg << "!" << endl;
- printUsage();
- exit(1);
- }
- slavePosition = number;
- }
- break;
-
- case 'd':
- if (!strcmp(optarg, "all")) {
- domainIndex = -1;
- } else {
- number = strtoul(optarg, &remainder, 0);
- if (remainder == optarg || *remainder || number < 0) {
- cerr << "Invalid domain index "
- << optarg << "!" << endl;
- printUsage();
- exit(1);
- }
- domainIndex = number;
- }
- break;
-
- case 't':
- dataTypeStr = optarg;
- break;
-
- case 'f':
- force = true;
- break;
-
- case 'q':
- verbosity = Master::Quiet;
- break;
-
- case 'v':
- verbosity = Master::Verbose;
- break;
-
- case 'h':
- helpRequested = true;
- break;
-
- case '?':
- printUsage();
- exit(1);
-
- default:
- break;
- }
- }
- while (c != -1);
-
- argCount = argc - optind;
-
- if (!argCount) {
- if (!helpRequested) {
- cerr << "Please specify a command!" << endl;
- }
- printUsage();
- exit(!helpRequested);
- }
-
- command = argv[optind];
- while (++optind < argc)
- commandArgs.push_back(string(argv[optind]));
-}
-
-/****************************************************************************/
-
-int main(int argc, char **argv)
-{
- Master master;
-
- cmdName = basename(argv[0]);
-
- getOptions(argc, argv);
-
- master.setIndex(masterIndex);
- master.setVerbosity(verbosity);
-
- try {
- if (command == "alias") {
- master.writeAlias(slavePosition, force, commandArgs);
- } else if (command == "config") {
- master.showConfigs();
+MasterDevice masterDev;
+
+/*****************************************************************************/
+
+struct Command {
+ void (*func)(void);
+ const char *helpString;
+
+ int execute(void) const;
+ void displayHelp(void) const;
+};
+
+struct CommandAlias {
+ const char *name;
+ const Command *command;
+};
+
+/*****************************************************************************/
+
+#define COMMAND(name) \
+ void command_##name(void); \
+ extern const char *help_##name; \
+ const Command cmd_##name = {command_##name, help_##name};
+
+COMMAND(alias);
+COMMAND(config);
+
+const CommandAlias commandAliases[] = {
+ {"alias", &cmd_alias},
+
+ {"config", &cmd_config},
+ {"conf", &cmd_config},
+ {"cf", &cmd_config},
+};
+
+#if 0
} else if (command == "data") {
master.outputData(domainIndex);
} else if (command == "debug") {
@@ -227,17 +91,221 @@
master.requestStates(slavePosition, commandArgs);
} else if (command == "xml") {
master.generateXml(slavePosition);
+#endif
+
+/*****************************************************************************/
+
+void printUsage()
+{
+ cerr
+ << "Usage: " << binaryBaseName << " <COMMAND> [OPTIONS]" << endl
+ << "Commands:" << endl
+ << " alias Write alias addresses." << endl
+ << " config Show bus configuration." << endl
+ << " data Output binary domain process data." << endl
+ << " debug Set the master's debug level." << endl
+ << " domain Show domain information." << endl
+ << " master Show master information." << endl
+ << " pdos List Pdo assignment/mapping." << endl
+ << " sdo_download Write an Sdo entry." << endl
+ << " sdos List Sdo dictionaries." << endl
+ << " sdo_upload Read an Sdo entry." << endl
+ << " sii_read Output a slave's SII contents." << endl
+ << " sii_write Write slave's SII contents." << endl
+ << " slaves Show slaves." << endl
+ << " state Request slave states." << endl
+ << " xml Generate slave information xmls." << endl
+ << "Global options:" << endl
+ << " --master -m <master> Index of the master to use. Default: 0"
+ << endl
+ << " --slave -s <index> Positive numerical ring position,"
+ << endl
+ << " or 'all' for all slaves (default)."
+ << endl
+ << " --domain -d <index> Positive numerical index,"
+ << endl
+ << " or 'all' for all domains (default)."
+ << endl
+ << " --type -t <type> Forced Sdo data type." << endl
+ << " --force -f Force action." << endl
+ << " --quiet -q Output less information." << endl
+ << " --verbose -v Output more information." << endl
+ << " --help -h Show this help." << endl
+ << "Call '" << binaryBaseName
+ << " <COMMAND> --help' for command-specific help." << endl
+ << "Send bug reports to " << PACKAGE_BUGREPORT << "." << endl;
+}
+
+/*****************************************************************************/
+
+void getOptions(int argc, char **argv)
+{
+ int c, argCount, optionIndex, number;
+ char *remainder;
+
+ static struct option longOptions[] = {
+ //name, has_arg, flag, val
+ {"master", required_argument, NULL, 'm'},
+ {"slave", required_argument, NULL, 's'},
+ {"domain", required_argument, NULL, 'd'},
+ {"type", required_argument, NULL, 't'},
+ {"force", no_argument, NULL, 'f'},
+ {"quiet", no_argument, NULL, 'q'},
+ {"verbose", no_argument, NULL, 'v'},
+ {"help", no_argument, NULL, 'h'},
+ {}
+ };
+
+ do {
+ c = getopt_long(argc, argv, "m:s:d:t:fqvh", longOptions, &optionIndex);
+
+ switch (c) {
+ case 'm':
+ number = strtoul(optarg, &remainder, 0);
+ if (remainder == optarg || *remainder || number < 0) {
+ cerr << "Invalid master number " << optarg << "!" << endl;
+ printUsage();
+ exit(1);
+ }
+ masterIndex = number;
+ break;
+
+ case 's':
+ if (!strcmp(optarg, "all")) {
+ slavePosition = -1;
+ } else {
+ number = strtoul(optarg, &remainder, 0);
+ if (remainder == optarg || *remainder
+ || number < 0 || number > 0xFFFF) {
+ cerr << "Invalid slave position "
+ << optarg << "!" << endl;
+ printUsage();
+ exit(1);
+ }
+ slavePosition = number;
+ }
+ break;
+
+ case 'd':
+ if (!strcmp(optarg, "all")) {
+ domainIndex = -1;
+ } else {
+ number = strtoul(optarg, &remainder, 0);
+ if (remainder == optarg || *remainder || number < 0) {
+ cerr << "Invalid domain index "
+ << optarg << "!" << endl;
+ printUsage();
+ exit(1);
+ }
+ domainIndex = number;
+ }
+ break;
+
+ case 't':
+ dataTypeStr = optarg;
+ break;
+
+ case 'f':
+ force = true;
+ break;
+
+ case 'q':
+ verbosity = Quiet;
+ break;
+
+ case 'v':
+ verbosity = Verbose;
+ break;
+
+ case 'h':
+ helpRequested = true;
+ break;
+
+ case '?':
+ printUsage();
+ exit(1);
+
+ default:
+ break;
+ }
+ }
+ while (c != -1);
+
+ argCount = argc - optind;
+
+ if (!argCount) {
+ if (!helpRequested) {
+ cerr << "Please specify a command!" << endl;
+ }
+ printUsage();
+ exit(!helpRequested);
+ }
+
+ command = argv[optind];
+ while (++optind < argc)
+ commandArgs.push_back(string(argv[optind]));
+}
+
+/****************************************************************************/
+
+int Command::execute() const
+{
+ try {
+ func();
+ } catch (InvalidUsageException &e) {
+ cerr << e.what() << endl << endl;
+ displayHelp();
+ return 1;
+ } catch (ExecutionFailureException &e) {
+ cerr << e.what() << endl;
+ return 1;
+ } catch (MasterDeviceException &e) {
+ cerr << e.what() << endl;
+ return 1;
+ }
+
+ return 0;
+}
+
+/****************************************************************************/
+
+void Command::displayHelp() const
+{
+ cerr << binaryBaseName << " " << command << " " << helpString;
+}
+
+/****************************************************************************/
+
+int main(int argc, char **argv)
+{
+ int retval = 0;
+ const CommandAlias *alias;
+ const CommandAlias *endAlias =
+ commandAliases + sizeof(commandAliases) / sizeof(CommandAlias);
+
+ binaryBaseName = basename(argv[0]);
+ getOptions(argc, argv);
+
+ // search command alias in alias map
+ for (alias = commandAliases; alias < endAlias; alias++) {
+ if (command == alias->name)
+ break;
+ }
+
+ if (alias < endAlias) { // command alias found
+ if (!helpRequested) {
+ masterDev.setIndex(masterIndex);
+ retval = alias->command->execute();
} else {
- cerr << "Unknown command " << command << "!" << endl;
- printUsage();
- exit(1);
+ alias->command->displayHelp();
}
- } catch (MasterException &e) {
- cerr << e.what() << endl;
- exit(1);
+ } else { // command not found
+ cerr << "Unknown command " << command << "!" << endl << endl;
+ printUsage();
+ retval = 1;
}
- return 0;
+ return retval;
}
/****************************************************************************/