fp@1122: /*****************************************************************************
fp@1122:  *
fp@1122:  * $Id$
fp@1122:  *
fp@1122:  ****************************************************************************/
fp@1122: 
fp@1122: #include <sys/stat.h>
fp@1122: #include <fcntl.h>
fp@1122: #include <errno.h>
fp@1122: #include <sys/ioctl.h>
fp@1122: 
fp@1122: #include <sstream>
fp@1122: using namespace std;
fp@1122: 
fp@1122: #if 0
fp@1122: #include <iostream>
fp@1122: #include <fstream>
fp@1122: #include <cctype> // toupper()
fp@1122: #include <list>
fp@1122: #endif
fp@1122: 
fp@1122: #include "MasterDevice.h"
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: struct CoEDataType {
fp@1122:     const char *name;
fp@1122:     uint16_t coeCode;
fp@1122:     unsigned int byteSize;
fp@1122: };
fp@1122: 
fp@1122: static const CoEDataType dataTypes[] = {
fp@1122:     {"int8",   0x0002, 1},
fp@1122:     {"int16",  0x0003, 2},
fp@1122:     {"int32",  0x0004, 4},
fp@1122:     {"uint8",  0x0005, 1},
fp@1122:     {"uint16", 0x0006, 2},
fp@1122:     {"uint32", 0x0007, 4},
fp@1122:     {"string", 0x0009, 0},
fp@1122:     {"raw",    0xffff, 0},
fp@1122:     {}
fp@1122: };
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: const CoEDataType *findDataType(const string &str)
fp@1122: {
fp@1122:     const CoEDataType *d;
fp@1122:     
fp@1122:     for (d = dataTypes; d->name; d++)
fp@1122:         if (str == d->name)
fp@1122:             return d;
fp@1122: 
fp@1122:     return NULL;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: const CoEDataType *findDataType(uint16_t code)
fp@1122: {
fp@1122:     const CoEDataType *d;
fp@1122:     
fp@1122:     for (d = dataTypes; d->name; d++)
fp@1122:         if (code == d->coeCode)
fp@1122:             return d;
fp@1122: 
fp@1122:     return NULL;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: MasterDevice::MasterDevice()
fp@1122: {
fp@1122:     index = 0;
fp@1122:     fd = -1;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: MasterDevice::~MasterDevice()
fp@1122: {
fp@1122:     close();
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::setIndex(unsigned int i)
fp@1122: {
fp@1122:     index = i;
fp@1122: }
fp@1122: 
fp@1122: #if 0
fp@1122: /*****************************************************************************/
fp@1122: 
fp@1122: bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)
fp@1122: {
fp@1122:     return a.alias < b.alias
fp@1122:         || (a.alias == b.alias && a.position < b.position);
fp@1122: }
fp@1122: 
fp@1122: /** Lists the bus configuration.
fp@1122:  */
fp@1122: void Master::showConfigs()
fp@1122: {
fp@1122:     ec_ioctl_master_t master;
fp@1122:     unsigned int i;
fp@1122:     ec_ioctl_config_t config;
fp@1122:     ConfigList configList;
fp@1122: 
fp@1122:     open(Read);
fp@1122:     getMaster(&master);
fp@1122: 
fp@1122:     for (i = 0; i < master.config_count; i++) {
fp@1122:         getConfig(&config, i);
fp@1122:         configList.push_back(config);
fp@1122:     }
fp@1122: 
fp@1122:     configList.sort();
fp@1122: 
fp@1122:     if (verbosity == Verbose) {
fp@1122:         showDetailedConfigs(configList);
fp@1122:     } else {
fp@1122:         listConfigs(configList);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::outputData(int domainIndex)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (domainIndex == -1) {
fp@1122:         unsigned int i;
fp@1122:         ec_ioctl_master_t master;
fp@1122: 
fp@1122:         getMaster(&master);
fp@1122: 
fp@1122:         for (i = 0; i < master.domain_count; i++) {
fp@1122:             outputDomainData(i);
fp@1122:         }
fp@1122:     } else {
fp@1122:         outputDomainData(domainIndex);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::setDebug(const vector<string> &commandArgs)
fp@1122: {
fp@1122:     stringstream str;
fp@1122:     int debugLevel;
fp@1122:     
fp@1122:     if (commandArgs.size() != 1) {
fp@1122:         stringstream err;
fp@1122:         err << "'debug' takes exactly one argument!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     str << commandArgs[0];
fp@1122:     str >> resetiosflags(ios::basefield) // guess base from prefix
fp@1122:         >> debugLevel;
fp@1122: 
fp@1122:     if (str.fail()) {
fp@1122:         stringstream err;
fp@1122:         err << "Invalid debug level '" << commandArgs[0] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     open(ReadWrite);
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to set debug level: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::showDomains(int domainIndex)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (domainIndex == -1) {
fp@1122:         unsigned int i;
fp@1122:         ec_ioctl_master_t master;
fp@1122: 
fp@1122:         getMaster(&master);
fp@1122: 
fp@1122:         for (i = 0; i < master.domain_count; i++) {
fp@1122:             showDomain(i);
fp@1122:         }
fp@1122:     } else {
fp@1122:         showDomain(domainIndex);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::showMaster()
fp@1122: {
fp@1122:     ec_ioctl_master_t data;
fp@1122:     stringstream err;
fp@1122:     unsigned int i;
fp@1122:     
fp@1122:     open(Read);
fp@1122:     getMaster(&data);
fp@1122: 
fp@1122:     cout
fp@1122:         << "Master" << index << endl
fp@1122:         << "  Phase: ";
fp@1122: 
fp@1122:     switch (data.phase) {
fp@1122:         case 0: cout << "Waiting for device..."; break;
fp@1122:         case 1: cout << "Idle"; break;
fp@1122:         case 2: cout << "Operation"; break;
fp@1122:         default:
fp@1122:                 err << "Invalid master phase " << data.phase;
fp@1122:                 throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     cout << endl
fp@1122:         << "  Slaves: " << data.slave_count << endl;
fp@1122: 
fp@1122:     for (i = 0; i < 2; i++) {
fp@1122:         cout << "  Device" << i << ": ";
fp@1122:         if (data.devices[i].address[0] == 0x00
fp@1122:                 && data.devices[i].address[1] == 0x00
fp@1122:                 && data.devices[i].address[2] == 0x00
fp@1122:                 && data.devices[i].address[3] == 0x00
fp@1122:                 && data.devices[i].address[4] == 0x00
fp@1122:                 && data.devices[i].address[5] == 0x00) {
fp@1122:             cout << "None.";
fp@1122:         } else {
fp@1122:             cout << hex << setfill('0')
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[0] << ":"
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[1] << ":"
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[2] << ":"
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[3] << ":"
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[4] << ":"
fp@1122:                 << setw(2) << (unsigned int) data.devices[i].address[5] << " ("
fp@1122:                 << (data.devices[i].attached ? "attached" : "waiting...")
fp@1122:                 << ")" << endl << dec
fp@1122:                 << "    Tx count: " << data.devices[i].tx_count << endl
fp@1122:                 << "    Rx count: " << data.devices[i].rx_count;
fp@1122:         }
fp@1122:         cout << endl;
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::listPdos(int slavePosition)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (slavePosition == -1) {
fp@1122:         unsigned int numSlaves = slaveCount(), i;
fp@1122: 
fp@1122:         for (i = 0; i < numSlaves; i++) {
fp@1122:             listSlavePdos(i, true);
fp@1122:         }
fp@1122:     } else {
fp@1122:         listSlavePdos(slavePosition, false);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::listSdos(int slavePosition)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (slavePosition == -1) {
fp@1122:         unsigned int numSlaves = slaveCount(), i;
fp@1122: 
fp@1122:         for (i = 0; i < numSlaves; i++) {
fp@1122:             listSlaveSdos(i, true);
fp@1122:         }
fp@1122:     } else {
fp@1122:         listSlaveSdos(slavePosition, false);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::sdoDownload(
fp@1122:         int slavePosition,
fp@1122:         const string &dataTypeStr,
fp@1122:         const vector<string> &commandArgs
fp@1122:         )
fp@1122: {
fp@1122:     stringstream strIndex, strSubIndex, strValue, err;
fp@1122:     ec_ioctl_slave_sdo_download_t data;
fp@1122:     unsigned int number;
fp@1122:     const CoEDataType *dataType = NULL;
fp@1122: 
fp@1122:     if (slavePosition < 0) {
fp@1122:         err << "'sdo_download' requires a slave! Please specify --slave.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.slave_position = slavePosition;
fp@1122: 
fp@1122:     if (commandArgs.size() != 3) {
fp@1122:         err << "'sdo_download' takes 3 arguments!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     strIndex << commandArgs[0];
fp@1122:     strIndex
fp@1122:         >> resetiosflags(ios::basefield) // guess base from prefix
fp@1122:         >> data.sdo_index;
fp@1122:     if (strIndex.fail()) {
fp@1122:         err << "Invalid Sdo index '" << commandArgs[0] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     strSubIndex << commandArgs[1];
fp@1122:     strSubIndex
fp@1122:         >> resetiosflags(ios::basefield) // guess base from prefix
fp@1122:         >> number;
fp@1122:     if (strSubIndex.fail() || number > 0xff) {
fp@1122:         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.sdo_entry_subindex = number;
fp@1122: 
fp@1122:     if (dataTypeStr != "") { // data type specified
fp@1122:         if (!(dataType = findDataType(dataTypeStr))) {
fp@1122:             err << "Invalid data type '" << dataTypeStr << "'!";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:     } else { // no data type specified: fetch from dictionary
fp@1122:         ec_ioctl_slave_sdo_entry_t entry;
fp@1122: 
fp@1122:         open(ReadWrite);
fp@1122: 
fp@1122:         try {
fp@1122:             getSdoEntry(&entry, slavePosition,
fp@1122:                     data.sdo_index, data.sdo_entry_subindex);
fp@1122:         } catch (MasterDeviceException &e) {
fp@1122:             err << "Failed to determine Sdo entry data type. "
fp@1122:                 << "Please specify --type.";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:         if (!(dataType = findDataType(entry.data_type))) {
fp@1122:             err << "Pdo entry has unknown data type 0x"
fp@1122:                 << hex << setfill('0') << setw(4) << entry.data_type << "!"
fp@1122:                 << " Please specify --type.";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:     }
fp@1122: 
fp@1122:     if (dataType->byteSize) {
fp@1122:         data.data_size = dataType->byteSize;
fp@1122:     } else {
fp@1122:         data.data_size = DefaultBufferSize;
fp@1122:     }
fp@1122: 
fp@1122:     data.data = new uint8_t[data.data_size + 1];
fp@1122: 
fp@1122:     strValue << commandArgs[2];
fp@1122:     strValue >> resetiosflags(ios::basefield); // guess base from prefix
fp@1122:     strValue.exceptions(ios::failbit);
fp@1122: 
fp@1122:     try {
fp@1122:         switch (dataType->coeCode) {
fp@1122:             case 0x0002: // int8
fp@1122:                 {
fp@1122:                     int16_t val; // uint8_t is interpreted as char
fp@1122:                     strValue >> val;
fp@1122:                     if (val > 127 || val < -128)
fp@1122:                         throw ios::failure("Value out of range");
fp@1122:                     *data.data = val;
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0003: // int16
fp@1122:                 {
fp@1122:                     int16_t val;
fp@1122:                     strValue >> val;
fp@1122:                     *(int16_t *) data.data = cputole16(val);
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0004: // int32
fp@1122:                 {
fp@1122:                     int32_t val;
fp@1122:                     strValue >> val;
fp@1122:                     *(int32_t *) data.data = cputole32(val);
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0005: // uint8
fp@1122:                 {
fp@1122:                     uint16_t val; // uint8_t is interpreted as char
fp@1122:                     strValue >> val;
fp@1122:                     if (val > 0xff)
fp@1122:                         throw ios::failure("Value out of range");
fp@1122:                     *data.data = val;
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0006: // uint16
fp@1122:                 {
fp@1122:                     uint16_t val;
fp@1122:                     strValue >> val;
fp@1122:                     *(uint16_t *) data.data = cputole16(val);
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0007: // uint32
fp@1122:                 {
fp@1122:                     uint32_t val;
fp@1122:                     strValue >> val;
fp@1122:                     *(uint32_t *) data.data = cputole32(val);
fp@1122:                     break;
fp@1122:                 }
fp@1122:             case 0x0009: // string
fp@1122:                 if (strValue.str().size() >= data.data_size) {
fp@1122:                     err << "String too large";
fp@1122:                     throw MasterDeviceException(err.str());
fp@1122:                 }
fp@1122:                 data.data_size = strValue.str().size();
fp@1122:                 strValue >> (char *) data.data;
fp@1122:                 break;
fp@1122: 
fp@1122:             default:
fp@1122:                 delete [] data.data;
fp@1122:                 err << "Unknown data type 0x" << hex << dataType->coeCode;
fp@1122:                 throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:     } catch (ios::failure &e) {
fp@1122:         delete [] data.data;
fp@1122:         err << "Invalid value argument '" << commandArgs[2]
fp@1122:             << "' for type '" << dataType->name << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     open(ReadWrite);
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to download Sdo: ";
fp@1122:         if (errno == EIO && data.abort_code) {
fp@1122:             err << "Abort code 0x" << hex << setfill('0')
fp@1122:                 << setw(8) << data.abort_code;
fp@1122:         } else {
fp@1122:             err << strerror(errno);
fp@1122:         }
fp@1122:         delete [] data.data;
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     delete [] data.data;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::sdoUpload(
fp@1122:         int slavePosition,
fp@1122:         const string &dataTypeStr,
fp@1122:         const vector<string> &commandArgs
fp@1122:         )
fp@1122: {
fp@1122:     stringstream strIndex, strSubIndex;
fp@1122:     int sval;
fp@1122:     ec_ioctl_slave_sdo_upload_t data;
fp@1122:     unsigned int uval;
fp@1122:     const CoEDataType *dataType = NULL;
fp@1122: 
fp@1122:     if (slavePosition < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "'sdo_upload' requires a slave! Please specify --slave.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.slave_position = slavePosition;
fp@1122: 
fp@1122:     if (commandArgs.size() != 2) {
fp@1122:         stringstream err;
fp@1122:         err << "'sdo_upload' takes two arguments!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     strIndex << commandArgs[0];
fp@1122:     strIndex
fp@1122:         >> resetiosflags(ios::basefield) // guess base from prefix
fp@1122:         >> data.sdo_index;
fp@1122:     if (strIndex.fail()) {
fp@1122:         stringstream err;
fp@1122:         err << "Invalid Sdo index '" << commandArgs[0] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     strSubIndex << commandArgs[1];
fp@1122:     strSubIndex
fp@1122:         >> resetiosflags(ios::basefield) // guess base from prefix
fp@1122:         >> uval;
fp@1122:     if (strSubIndex.fail() || uval > 0xff) {
fp@1122:         stringstream err;
fp@1122:         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.sdo_entry_subindex = uval;
fp@1122: 
fp@1122:     if (dataTypeStr != "") { // data type specified
fp@1122:         if (!(dataType = findDataType(dataTypeStr))) {
fp@1122:             stringstream err;
fp@1122:             err << "Invalid data type '" << dataTypeStr << "'!";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:     } else { // no data type specified: fetch from dictionary
fp@1122:         ec_ioctl_slave_sdo_entry_t entry;
fp@1122: 
fp@1122:         open(Read);
fp@1122: 
fp@1122:         try {
fp@1122:             getSdoEntry(&entry, slavePosition,
fp@1122:                     data.sdo_index, data.sdo_entry_subindex);
fp@1122:         } catch (MasterDeviceException &e) {
fp@1122:             stringstream err;
fp@1122:             err << "Failed to determine Sdo entry data type. "
fp@1122:                 << "Please specify --type.";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:         if (!(dataType = findDataType(entry.data_type))) {
fp@1122:             stringstream err;
fp@1122:             err << "Pdo entry has unknown data type 0x"
fp@1122:                 << hex << setfill('0') << setw(4) << entry.data_type << "!"
fp@1122:                 << " Please specify --type.";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122:     }
fp@1122: 
fp@1122:     if (dataType->byteSize) {
fp@1122:         data.target_size = dataType->byteSize;
fp@1122:     } else {
fp@1122:         data.target_size = DefaultBufferSize;
fp@1122:     }
fp@1122: 
fp@1122:     data.target = new uint8_t[data.target_size + 1];
fp@1122: 
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to upload Sdo: ";
fp@1122:         if (errno == EIO && data.abort_code) {
fp@1122:             err << "Abort code 0x" << hex << setfill('0')
fp@1122:                 << setw(8) << data.abort_code;
fp@1122:         } else {
fp@1122:             err << strerror(errno);
fp@1122:         }
fp@1122:         delete [] data.target;
fp@1122:         close();
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     close();
fp@1122: 
fp@1122:     if (dataType->byteSize && data.data_size != dataType->byteSize) {
fp@1122:         stringstream err;
fp@1122:         err << "Data type mismatch. Expected " << dataType->name
fp@1122:             << " with " << dataType->byteSize << " byte, but got "
fp@1122:             << data.data_size << " byte.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     cout << setfill('0');
fp@1122:     switch (dataType->coeCode) {
fp@1122:         case 0x0002: // int8
fp@1122:             sval = *(int8_t *) data.target;
fp@1122:             cout << sval << " 0x" << hex << setw(2) << sval << endl;
fp@1122:             break;
fp@1122:         case 0x0003: // int16
fp@1122:             sval = le16tocpu(*(int16_t *) data.target);
fp@1122:             cout << sval << " 0x" << hex << setw(4) << sval << endl;
fp@1122:             break;
fp@1122:         case 0x0004: // int32
fp@1122:             sval = le32tocpu(*(int32_t *) data.target);
fp@1122:             cout << sval << " 0x" << hex << setw(8) << sval << endl;
fp@1122:             break;
fp@1122:         case 0x0005: // uint8
fp@1122:             uval = (unsigned int) *(uint8_t *) data.target;
fp@1122:             cout << uval << " 0x" << hex << setw(2) << uval << endl;
fp@1122:             break;
fp@1122:         case 0x0006: // uint16
fp@1122:             uval = le16tocpu(*(uint16_t *) data.target);
fp@1122:             cout << uval << " 0x" << hex << setw(4) << uval << endl;
fp@1122:             break;
fp@1122:         case 0x0007: // uint32
fp@1122:             uval = le32tocpu(*(uint32_t *) data.target);
fp@1122:             cout << uval << " 0x" << hex << setw(8) << uval << endl;
fp@1122:             break;
fp@1122:         case 0x0009: // string
fp@1122:             cout << string((const char *) data.target, data.data_size)
fp@1122:                 << endl;
fp@1122:             break;
fp@1122:         default:
fp@1122:             printRawData(data.target, data.data_size);
fp@1122:             break;
fp@1122:     }
fp@1122: 
fp@1122:     delete [] data.target;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::showSlaves(int slavePosition)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (verbosity == Verbose) {
fp@1122:         if (slavePosition == -1) {
fp@1122:             unsigned int numSlaves = slaveCount(), i;
fp@1122: 
fp@1122:             for (i = 0; i < numSlaves; i++) {
fp@1122:                 showSlave(i);
fp@1122:             }
fp@1122:         } else {
fp@1122:             showSlave(slavePosition);
fp@1122:         }
fp@1122:     } else {
fp@1122:         listSlaves(slavePosition);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: struct CategoryName {
fp@1122:     uint16_t type;
fp@1122:     const char *name;
fp@1122: };
fp@1122: 
fp@1122: static const CategoryName categoryNames[] = {
fp@1122:     {0x000a, "STRINGS"},
fp@1122:     {0x0014, "DataTypes"},
fp@1122:     {0x001e, "General"},
fp@1122:     {0x0028, "FMMU"},
fp@1122:     {0x0029, "SyncM"},
fp@1122:     {0x0032, "TXPDO"},
fp@1122:     {0x0033, "RXPDO"},
fp@1122:     {0x003c, "DC"},
fp@1122:     {}
fp@1122: };
fp@1122: 
fp@1122: const char *getCategoryName(uint16_t type)
fp@1122: {
fp@1122:     const CategoryName *cn = categoryNames;
fp@1122: 
fp@1122:     while (cn->type) {
fp@1122:         if (cn->type == type) {
fp@1122:             return cn->name;
fp@1122:         }
fp@1122:         cn++;
fp@1122:     }
fp@1122: 
fp@1122:     return "unknown";
fp@1122: }
fp@1122: 
fp@1122: void Master::siiRead(int slavePosition)
fp@1122: {
fp@1122:     ec_ioctl_slave_sii_t data;
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     unsigned int i;
fp@1122:     const uint16_t *categoryHeader;
fp@1122:     uint16_t categoryType, categorySize;
fp@1122:     stringstream err;
fp@1122: 
fp@1122:     if (slavePosition < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "'sii_read' requires a slave! Please specify --slave.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.slave_position = slavePosition;
fp@1122: 
fp@1122:     open(Read);
fp@1122: 
fp@1122:     getSlave(&slave, slavePosition);
fp@1122: 
fp@1122:     if (!slave.sii_nwords)
fp@1122:         return;
fp@1122: 
fp@1122:     data.offset = 0;
fp@1122:     data.nwords = slave.sii_nwords;
fp@1122:     data.words = new uint16_t[data.nwords];
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {
fp@1122:         stringstream err;
fp@1122:         delete [] data.words;
fp@1122:         err << "Failed to read SII: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     if (verbosity == Verbose) {
fp@1122:         cout << "SII Area:" << hex << setfill('0');
fp@1122:         for (i = 0; i < min(data.nwords, 0x0040U) * 2; i++) {
fp@1122:             if (i % BreakAfterBytes) {
fp@1122:                 cout << " ";
fp@1122:             } else {
fp@1122:                 cout << endl << "  ";
fp@1122:             }
fp@1122:             cout << setw(2) << (unsigned int) *((uint8_t *) data.words + i);
fp@1122:         }
fp@1122:         cout << endl;
fp@1122: 
fp@1122:         if (data.nwords > 0x0040U) {
fp@1122:             // cycle through categories
fp@1122:             categoryHeader = data.words + 0x0040U;
fp@1122:             categoryType = le16tocpu(*categoryHeader);
fp@1122:             while (categoryType != 0xffff) {
fp@1122:                 cout << "SII Category 0x" << hex
fp@1122:                     << setw(4) << categoryType
fp@1122:                     << " (" << getCategoryName(categoryType) << ")" << flush;
fp@1122: 
fp@1122:                 if (categoryHeader + 1 > data.words + data.nwords) {
fp@1122:                     err << "SII data seem to be corrupted!";
fp@1122:                     throw MasterDeviceException(err.str());
fp@1122:                 }
fp@1122:                 categorySize = le16tocpu(*(categoryHeader + 1));
fp@1122:                 cout << ", " << dec << categorySize << " words" << flush;
fp@1122: 
fp@1122:                 if (categoryHeader + 2 + categorySize
fp@1122:                         > data.words + data.nwords) {
fp@1122:                     err << "SII data seem to be corrupted!";
fp@1122:                     throw MasterDeviceException(err.str());
fp@1122:                 }
fp@1122: 
fp@1122:                 cout << hex;
fp@1122:                 for (i = 0; i < categorySize * 2U; i++) {
fp@1122:                     if (i % BreakAfterBytes) {
fp@1122:                         cout << " ";
fp@1122:                     } else {
fp@1122:                         cout << endl << "  ";
fp@1122:                     }
fp@1122:                     cout << setw(2) << (unsigned int)
fp@1122:                         *((uint8_t *) (categoryHeader + 2) + i);
fp@1122:                 }
fp@1122:                 cout << endl;
fp@1122: 
fp@1122:                 if (categoryHeader + 2 + categorySize + 1
fp@1122:                         > data.words + data.nwords) {
fp@1122:                     err << "SII data seem to be corrupted!"; 
fp@1122:                     throw MasterDeviceException(err.str());
fp@1122:                 }
fp@1122:                 categoryHeader += 2 + categorySize;
fp@1122:                 categoryType = le16tocpu(*categoryHeader);
fp@1122:             }
fp@1122:         }
fp@1122:     } else {
fp@1122:         for (i = 0; i < data.nwords; i++) {
fp@1122:             uint16_t *w = data.words + i;
fp@1122:             cout << *(uint8_t *) w << *((uint8_t *) w + 1);
fp@1122:         }
fp@1122:     }
fp@1122: 
fp@1122:     delete [] data.words;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::siiWrite(
fp@1122:         int slavePosition,
fp@1122:         bool force,
fp@1122:         const vector<string> &commandArgs
fp@1122:         )
fp@1122: {
fp@1122:     stringstream err;
fp@1122:     ec_ioctl_slave_sii_t data;
fp@1122:     ifstream file;
fp@1122:     unsigned int byte_size;
fp@1122:     const uint16_t *categoryHeader;
fp@1122:     uint16_t categoryType, categorySize;
fp@1122:     uint8_t crc;
fp@1122: 
fp@1122:     if (slavePosition < 0) {
fp@1122:         err << "'sii_write' requires a slave! Please specify --slave.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122:     data.slave_position = slavePosition;
fp@1122: 
fp@1122:     if (commandArgs.size() != 1) {
fp@1122:         err << "'ssi_write' takes exactly one argument!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary);
fp@1122:     if (file.fail()) {
fp@1122:         err << "Failed to open '" << commandArgs[0] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     // get length of file
fp@1122:     file.seekg(0, ios::end);
fp@1122:     byte_size = file.tellg();
fp@1122:     file.seekg(0, ios::beg);
fp@1122: 
fp@1122:     if (!byte_size || byte_size % 2) {
fp@1122:         stringstream err;
fp@1122:         err << "Invalid file size! Must be non-zero and even.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     data.nwords = byte_size / 2;
fp@1122:     if (data.nwords < 0x0041 && !force) {
fp@1122:         err << "SII data too short (" << data.nwords << " words)! Mimimum is"
fp@1122:                 " 40 fixed words + 1 delimiter. Use --force to write anyway.";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     // allocate buffer and read file into buffer
fp@1122:     data.words = new uint16_t[data.nwords];
fp@1122:     file.read((char *) data.words, byte_size);
fp@1122:     file.close();
fp@1122: 
fp@1122:     if (!force) {
fp@1122:         // calculate checksum over words 0 to 6
fp@1122:         crc = calcSiiCrc((const uint8_t *) data.words, 14);
fp@1122:         if (crc != ((const uint8_t *) data.words)[14]) {
fp@1122:             err << "CRC incorrect. Must be 0x"
fp@1122:                 << hex << setfill('0') << setw(2) << (unsigned int) crc
fp@1122:                 << ". Use --force to write anyway.";
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122: 
fp@1122:         // cycle through categories to detect corruption
fp@1122:         categoryHeader = data.words + 0x0040U;
fp@1122:         categoryType = le16tocpu(*categoryHeader);
fp@1122:         while (categoryType != 0xffff) {
fp@1122:             if (categoryHeader + 1 > data.words + data.nwords) {
fp@1122:                 err << "SII data seem to be corrupted! "
fp@1122:                     << "Use --force to write anyway.";
fp@1122:                 throw MasterDeviceException(err.str());
fp@1122:             }
fp@1122:             categorySize = le16tocpu(*(categoryHeader + 1));
fp@1122:             if (categoryHeader + 2 + categorySize + 1
fp@1122:                     > data.words + data.nwords) {
fp@1122:                 err << "SII data seem to be corrupted! "
fp@1122:                     "Use --force to write anyway.";
fp@1122:                 throw MasterDeviceException(err.str());
fp@1122:             }
fp@1122:             categoryHeader += 2 + categorySize;
fp@1122:             categoryType = le16tocpu(*categoryHeader);
fp@1122:         }
fp@1122:     }
fp@1122: 
fp@1122:     // send data to master
fp@1122:     open(ReadWrite);
fp@1122:     data.offset = 0;
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to write SII: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::requestStates(
fp@1122:         int slavePosition,
fp@1122:         const vector<string> &commandArgs
fp@1122:         )
fp@1122: {
fp@1122:     string stateStr;
fp@1122:     uint8_t state;
fp@1122:     
fp@1122:     if (commandArgs.size() != 1) {
fp@1122:         stringstream err;
fp@1122:         err << "'state' takes exactly one argument!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     stateStr = commandArgs[0];
fp@1122:     transform(stateStr.begin(), stateStr.end(),
fp@1122:             stateStr.begin(), (int (*) (int)) std::toupper);
fp@1122: 
fp@1122:     if (stateStr == "INIT") {
fp@1122:         state = 0x01;
fp@1122:     } else if (stateStr == "PREOP") {
fp@1122:         state = 0x02;
fp@1122:     } else if (stateStr == "SAFEOP") {
fp@1122:         state = 0x04;
fp@1122:     } else if (stateStr == "OP") {
fp@1122:         state = 0x08;
fp@1122:     } else {
fp@1122:         stringstream err;
fp@1122:         err << "Invalid state '" << commandArgs[0] << "'!";
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: 
fp@1122:     open(ReadWrite);
fp@1122: 
fp@1122:     if (slavePosition == -1) {
fp@1122:         unsigned int i, numSlaves = slaveCount();
fp@1122:         for (i = 0; i < numSlaves; i++)
fp@1122:             requestState(i, state);
fp@1122:     } else {
fp@1122:         requestState(slavePosition, state);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::generateXml(int slavePosition)
fp@1122: {
fp@1122:     open(Read);
fp@1122: 
fp@1122:     if (slavePosition == -1) {
fp@1122:         unsigned int numSlaves = slaveCount(), i;
fp@1122: 
fp@1122:         for (i = 0; i < numSlaves; i++) {
fp@1122:             generateSlaveXml(i);
fp@1122:         }
fp@1122:     } else {
fp@1122:         generateSlaveXml(slavePosition);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: #endif
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::open(Permissions perm)
fp@1122: {
fp@1122:     stringstream deviceName;
fp@1122: 
fp@1122:     if (fd != -1) { // already open
fp@1122:         return;
fp@1122:     }
fp@1122:     
fp@1122:     deviceName << "/dev/EtherCAT" << index;
fp@1122:     
fp@1122:     if ((fd = ::open(deviceName.str().c_str(),
fp@1122:                     perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to open master device " << deviceName.str() << ": "
fp@1122:             << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::close()
fp@1122: {
fp@1122:     if (fd == -1)
fp@1122:         return;
fp@1122: 
fp@1122:     ::close(fd);
fp@1122:     fd = -1;
fp@1122: }
fp@1122: 
fp@1122: #if 0
fp@1122: 
fp@1122: /*****************************************************************************/
fp@1122: 
fp@1122: /** Lists the complete bus configuration.
fp@1122:  */
fp@1122: void Master::showDetailedConfigs(const ConfigList &configList)
fp@1122: {
fp@1122:     ConfigList::const_iterator configIter;
fp@1122:     unsigned int j, k, l;
fp@1122:     ec_ioctl_config_pdo_t pdo;
fp@1122:     ec_ioctl_config_pdo_entry_t entry;
fp@1122:     ec_ioctl_config_sdo_t sdo;
fp@1122: 
fp@1122:     for (configIter = configList.begin();
fp@1122:             configIter != configList.end();
fp@1122:             configIter++) {
fp@1122: 
fp@1122:         cout << "Alias: "
fp@1122:             << dec << configIter->alias << endl
fp@1122:             << "Position: " << configIter->position << endl
fp@1122:             << "Vendor Id: 0x"
fp@1122:             << hex << setfill('0')
fp@1122:             << setw(8) << configIter->vendor_id << endl
fp@1122:             << "Product code: 0x"
fp@1122:             << setw(8) << configIter->product_code << endl
fp@1122:             << "Attached: " << (configIter->attached ? "yes" : "no") << endl
fp@1122:             << "Operational: " << (configIter->operational ? "yes" : "no") << endl;
fp@1122: 
fp@1122:         for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) {
fp@1122:             if (configIter->syncs[j].pdo_count) {
fp@1122:                 cout << "SM" << dec << j << " ("
fp@1122:                     << (configIter->syncs[j].dir == EC_DIR_INPUT
fp@1122:                             ? "Input" : "Output") << ")" << endl;
fp@1122:                 for (k = 0; k < configIter->syncs[j].pdo_count; k++) {
fp@1122:                     getConfigPdo(&pdo, configIter->config_index, j, k);
fp@1122: 
fp@1122:                     cout << "  Pdo 0x" << hex
fp@1122:                         << setw(4) << pdo.index
fp@1122:                         << " \"" << pdo.name << "\"" << endl;
fp@1122: 
fp@1122:                     for (l = 0; l < pdo.entry_count; l++) {
fp@1122:                         getConfigPdoEntry(&entry, configIter->config_index, j, k, l);
fp@1122: 
fp@1122:                         cout << "    Pdo entry 0x" << hex
fp@1122:                             << setw(4) << entry.index << ":"
fp@1122:                             << setw(2) << (unsigned int) entry.subindex
fp@1122:                             << ", " << dec << (unsigned int) entry.bit_length
fp@1122:                             << " bit, \"" << entry.name << "\"" << endl;
fp@1122:                     }
fp@1122:                 }
fp@1122:             }
fp@1122:         }
fp@1122: 
fp@1122:         cout << "Sdo configuration:" << endl;
fp@1122:         if (configIter->sdo_count) {
fp@1122:             for (j = 0; j < configIter->sdo_count; j++) {
fp@1122:                 getConfigSdo(&sdo, configIter->config_index, j);
fp@1122: 
fp@1122:                 cout << "  0x"
fp@1122:                     << hex << setfill('0')
fp@1122:                     << setw(4) << sdo.index << ":"
fp@1122:                     << setw(2) << (unsigned int) sdo.subindex
fp@1122:                     << ", " << dec << sdo.size << " byte: " << hex;
fp@1122: 
fp@1122:                 switch (sdo.size) {
fp@1122:                     case 1:
fp@1122:                         cout << "0x" << setw(2)
fp@1122:                             << (unsigned int) *(uint8_t *) &sdo.data;
fp@1122:                         break;
fp@1122:                     case 2:
fp@1122:                         cout << "0x" << setw(4)
fp@1122:                             << le16tocpu(*(uint16_t *) &sdo.data);
fp@1122:                         break;
fp@1122:                     case 4:
fp@1122:                         cout << "0x" << setw(8)
fp@1122:                             << le32tocpu(*(uint32_t *) &sdo.data);
fp@1122:                         break;
fp@1122:                     default:
fp@1122:                         cout << "???";
fp@1122:                 }
fp@1122: 
fp@1122:                 cout << endl;
fp@1122:             }
fp@1122:         } else {
fp@1122:             cout << "  None." << endl;
fp@1122:         }
fp@1122: 
fp@1122:         cout << endl;
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /*****************************************************************************/
fp@1122: 
fp@1122: struct ConfigInfo {
fp@1122:     string alias;
fp@1122:     string pos;
fp@1122:     string ident;
fp@1122:     string att;
fp@1122:     string op;
fp@1122: };
fp@1122: 
fp@1122: /** Lists the bus configuration.
fp@1122:  */
fp@1122: void Master::listConfigs(const ConfigList &configList)
fp@1122: {
fp@1122:     ConfigList::const_iterator configIter;
fp@1122:     stringstream str;
fp@1122:     ConfigInfo info;
fp@1122:     typedef list<ConfigInfo> ConfigInfoList;
fp@1122:     ConfigInfoList list;
fp@1122:     ConfigInfoList::const_iterator iter;
fp@1122:     unsigned int maxAliasWidth = 0, maxPosWidth = 0,
fp@1122:                  maxAttWidth = 0, maxOpWidth = 0;
fp@1122: 
fp@1122:     for (configIter = configList.begin();
fp@1122:             configIter != configList.end();
fp@1122:             configIter++) {
fp@1122: 
fp@1122:         str << dec << configIter->alias;
fp@1122:         info.alias = str.str();
fp@1122:         str.clear();
fp@1122:         str.str("");
fp@1122: 
fp@1122:         str << configIter->position;
fp@1122:         info.pos = str.str();
fp@1122:         str.clear();
fp@1122:         str.str("");
fp@1122: 
fp@1122:         str << hex << setfill('0')
fp@1122:             << "0x" << setw(8) << configIter->vendor_id
fp@1122:             << "/0x" << setw(8) << configIter->product_code;
fp@1122:         info.ident = str.str();
fp@1122:         str.clear();
fp@1122:         str.str("");
fp@1122: 
fp@1122:         str << (configIter->attached ? "attached" : "-");
fp@1122:         info.att = str.str();
fp@1122:         str.clear();
fp@1122:         str.str("");
fp@1122: 
fp@1122:         str << (configIter->operational ? "operational" : "-");
fp@1122:         info.op = str.str();
fp@1122:         str.clear();
fp@1122:         str.str("");
fp@1122: 
fp@1122:         list.push_back(info);
fp@1122: 
fp@1122:         if (info.alias.length() > maxAliasWidth)
fp@1122:             maxAliasWidth = info.alias.length();
fp@1122:         if (info.pos.length() > maxPosWidth)
fp@1122:             maxPosWidth = info.pos.length();
fp@1122:         if (info.att.length() > maxAttWidth)
fp@1122:             maxAttWidth = info.att.length();
fp@1122:         if (info.op.length() > maxOpWidth)
fp@1122:             maxOpWidth = info.op.length();
fp@1122:     }
fp@1122: 
fp@1122:     for (iter = list.begin(); iter != list.end(); iter++) {
fp@1122:         cout << setfill(' ') << right
fp@1122:             << setw(maxAliasWidth) << iter->alias
fp@1122:             << ":" << left
fp@1122:             << setw(maxPosWidth) << iter->pos
fp@1122:             << "  "
fp@1122:             << iter->ident
fp@1122:             << "  "
fp@1122:             << setw(maxAttWidth) << iter->att << "  "
fp@1122:             << setw(maxOpWidth) << iter->op << "  "
fp@1122:             << endl;
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::outputDomainData(unsigned int domainIndex)
fp@1122: {
fp@1122:     ec_ioctl_domain_t domain;
fp@1122:     ec_ioctl_domain_data_t data;
fp@1122:     unsigned char *processData;
fp@1122:     unsigned int i;
fp@1122:     
fp@1122:     getDomain(&domain, domainIndex);
fp@1122: 
fp@1122:     if (!domain.data_size)
fp@1122:         return;
fp@1122: 
fp@1122:     processData = new unsigned char[domain.data_size];
fp@1122: 
fp@1122:     try {
fp@1122:         getData(&data, domainIndex, domain.data_size, processData);
fp@1122:     } catch (MasterDeviceException &e) {
fp@1122:         delete [] processData;
fp@1122:         throw e;
fp@1122:     }
fp@1122: 
fp@1122:     for (i = 0; i < data.data_size; i++)
fp@1122:         cout << processData[i];
fp@1122:     cout.flush();
fp@1122: 
fp@1122:     delete [] processData;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::showDomain(unsigned int domainIndex)
fp@1122: {
fp@1122:     ec_ioctl_domain_t domain;
fp@1122:     unsigned char *processData;
fp@1122:     ec_ioctl_domain_data_t data;
fp@1122:     unsigned int i, j;
fp@1122:     ec_ioctl_domain_fmmu_t fmmu;
fp@1122:     unsigned int dataOffset;
fp@1122:     
fp@1122:     getDomain(&domain, domainIndex);
fp@1122: 
fp@1122: 	cout << "Domain" << dec << domainIndex << ":"
fp@1122: 		<< " LogBaseAddr 0x"
fp@1122: 		<< hex << setfill('0')
fp@1122:         << setw(8) << domain.logical_base_address
fp@1122: 		<< ", Size " << dec << setfill(' ')
fp@1122:         << setw(3) << domain.data_size
fp@1122: 		<< ", WorkingCounter "
fp@1122: 		<< domain.working_counter << "/"
fp@1122:         << domain.expected_working_counter << endl;
fp@1122: 
fp@1122:     if (!domain.data_size || verbosity != Verbose)
fp@1122:         return;
fp@1122: 
fp@1122:     processData = new unsigned char[domain.data_size];
fp@1122: 
fp@1122:     try {
fp@1122:         getData(&data, domainIndex, domain.data_size, processData);
fp@1122:     } catch (MasterDeviceException &e) {
fp@1122:         delete [] processData;
fp@1122:         throw e;
fp@1122:     }
fp@1122: 
fp@1122:     for (i = 0; i < domain.fmmu_count; i++) {
fp@1122:         getFmmu(&fmmu, domainIndex, i);
fp@1122: 
fp@1122:         cout << "  SlaveConfig "
fp@1122:             << dec << fmmu.slave_config_alias
fp@1122:             << ":" << fmmu.slave_config_position
fp@1122:             << ", SM" << (unsigned int) fmmu.sync_index << " ("
fp@1122:             << setfill(' ') << setw(6)
fp@1122:             << (fmmu.dir == EC_DIR_INPUT ? "Input" : "Output")
fp@1122:             << "), LogAddr 0x"
fp@1122:             << hex << setfill('0')
fp@1122:             << setw(8) << fmmu.logical_address
fp@1122:             << ", Size " << dec << fmmu.data_size << endl;
fp@1122: 
fp@1122:         dataOffset = fmmu.logical_address - domain.logical_base_address;
fp@1122:         if (dataOffset + fmmu.data_size > domain.data_size) {
fp@1122:             stringstream err;
fp@1122:             err << "Fmmu information corrupted!";
fp@1122:             delete [] processData;
fp@1122:             throw MasterDeviceException(err.str());
fp@1122:         }
fp@1122: 
fp@1122:         cout << "    " << hex << setfill('0');
fp@1122:         for (j = 0; j < fmmu.data_size; j++) {
fp@1122:             if (j && !(j % BreakAfterBytes))
fp@1122:                 cout << endl << "    ";
fp@1122:             cout << setw(2)
fp@1122:                 << (unsigned int) *(processData + dataOffset + j) << " ";
fp@1122:         }
fp@1122:         cout << endl;
fp@1122:     }
fp@1122: 
fp@1122:     delete [] processData;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::listSlavePdos(
fp@1122:         uint16_t slavePosition,
fp@1122:         bool withHeader
fp@1122:         )
fp@1122: {
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     ec_ioctl_slave_sync_t sync;
fp@1122:     ec_ioctl_slave_sync_pdo_t pdo;
fp@1122:     ec_ioctl_slave_sync_pdo_entry_t entry;
fp@1122:     unsigned int i, j, k;
fp@1122:     
fp@1122:     getSlave(&slave, slavePosition);
fp@1122: 
fp@1122:     if (withHeader)
fp@1122:         cout << "=== Slave " << slavePosition << " ===" << endl;
fp@1122: 
fp@1122:     for (i = 0; i < slave.sync_count; i++) {
fp@1122:         getSync(&sync, slavePosition, i);
fp@1122: 
fp@1122:         cout << "SM" << i << ":"
fp@1122:             << " PhysAddr 0x"
fp@1122:             << hex << setfill('0')
fp@1122:             << setw(4) << sync.physical_start_address
fp@1122:             << ", DefaultSize "
fp@1122:             << dec << setfill(' ') << setw(4) << sync.default_size
fp@1122:             << ", ControlRegister 0x"
fp@1122:             << hex << setfill('0') << setw(2)
fp@1122:             << (unsigned int) sync.control_register
fp@1122:             << ", Enable " << dec << (unsigned int) sync.enable
fp@1122:             << endl;
fp@1122: 
fp@1122:         for (j = 0; j < sync.pdo_count; j++) {
fp@1122:             getPdo(&pdo, slavePosition, i, j);
fp@1122: 
fp@1122:             cout << "  " << (sync.control_register & 0x04 ? "R" : "T")
fp@1122:                 << "xPdo 0x"
fp@1122:                 << hex << setfill('0')
fp@1122:                 << setw(4) << pdo.index
fp@1122:                 << " \"" << pdo.name << "\"" << endl;
fp@1122: 
fp@1122:             if (verbosity == Quiet)
fp@1122:                 continue;
fp@1122: 
fp@1122:             for (k = 0; k < pdo.entry_count; k++) {
fp@1122:                 getPdoEntry(&entry, slavePosition, i, j, k);
fp@1122: 
fp@1122:                 cout << "    Pdo entry 0x"
fp@1122:                     << hex << setfill('0')
fp@1122:                     << setw(4) << entry.index
fp@1122:                     << ":" << setw(2) << (unsigned int) entry.subindex
fp@1122:                     << ", " << dec << (unsigned int) entry.bit_length
fp@1122:                     << " bit, \"" << entry.name << "\"" << endl;
fp@1122:             }
fp@1122:         }
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::listSlaveSdos(
fp@1122:         uint16_t slavePosition,
fp@1122:         bool withHeader
fp@1122:         )
fp@1122: {
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     ec_ioctl_slave_sdo_t sdo;
fp@1122:     ec_ioctl_slave_sdo_entry_t entry;
fp@1122:     unsigned int i, j;
fp@1122:     const CoEDataType *d;
fp@1122:     
fp@1122:     getSlave(&slave, slavePosition);
fp@1122: 
fp@1122:     if (withHeader)
fp@1122:         cout << "=== Slave " << slavePosition << " ===" << endl;
fp@1122: 
fp@1122:     for (i = 0; i < slave.sdo_count; i++) {
fp@1122:         getSdo(&sdo, slavePosition, i);
fp@1122: 
fp@1122:         cout << "Sdo 0x"
fp@1122:             << hex << setfill('0')
fp@1122:             << setw(4) << sdo.sdo_index
fp@1122:             << ", \"" << sdo.name << "\"" << endl;
fp@1122: 
fp@1122:         if (verbosity == Quiet)
fp@1122:             continue;
fp@1122: 
fp@1122:         for (j = 0; j <= sdo.max_subindex; j++) {
fp@1122:             getSdoEntry(&entry, slavePosition, -i, j);
fp@1122: 
fp@1122:             cout << "  0x" << hex << setfill('0')
fp@1122:                 << setw(4) << sdo.sdo_index << ":"
fp@1122:                 << setw(2) << (unsigned int) entry.sdo_entry_subindex
fp@1122:                 << ", ";
fp@1122: 
fp@1122:             if ((d = findDataType(entry.data_type))) {
fp@1122:                 cout << d->name;
fp@1122:             } else {
fp@1122:                 cout << "type " << setw(4) << entry.data_type;
fp@1122:             }
fp@1122: 
fp@1122:             cout << ", " << dec << entry.bit_length << " bit, \""
fp@1122:                 << entry.description << "\"" << endl;
fp@1122:         }
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: struct SlaveInfo {
fp@1122:     string pos;
fp@1122:     string alias;
fp@1122:     string relPos;
fp@1122:     string state;
fp@1122:     string flag;
fp@1122:     string name;
fp@1122: };
fp@1122: 
fp@1122: void Master::listSlaves(int slavePosition)
fp@1122: {
fp@1122:     unsigned int numSlaves, i;
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     uint16_t lastAlias, aliasIndex;
fp@1122:     SlaveInfo slaveInfo;
fp@1122:     typedef list<SlaveInfo> SlaveInfoList;
fp@1122:     SlaveInfoList slaveInfoList;
fp@1122:     SlaveInfoList::const_iterator iter;
fp@1122:     stringstream str;
fp@1122:     unsigned int maxPosWidth = 0, maxAliasWidth = 0,
fp@1122:                  maxRelPosWidth = 0, maxStateWidth = 0;
fp@1122:     
fp@1122:     open(Read);
fp@1122: 
fp@1122:     numSlaves = slaveCount();
fp@1122: 
fp@1122:     lastAlias = 0;
fp@1122:     aliasIndex = 0;
fp@1122:     for (i = 0; i < numSlaves; i++) {
fp@1122:         getSlave(&slave, i);
fp@1122:         
fp@1122:         if (slave.alias) {
fp@1122:             lastAlias = slave.alias;
fp@1122:             aliasIndex = 0;
fp@1122:         }
fp@1122: 
fp@1122:         if (slavePosition == -1 || i == (unsigned int) slavePosition) {
fp@1122:             str << dec << i;
fp@1122:             slaveInfo.pos = str.str();
fp@1122:             str.clear();
fp@1122:             str.str("");
fp@1122: 
fp@1122:             str << lastAlias;
fp@1122:             slaveInfo.alias = str.str();
fp@1122:             str.str("");
fp@1122: 
fp@1122:             str << aliasIndex;
fp@1122:             slaveInfo.relPos = str.str();
fp@1122:             str.str("");
fp@1122: 
fp@1122:             slaveInfo.state = slaveState(slave.state);
fp@1122:             slaveInfo.flag = (slave.error_flag ? 'E' : '+');
fp@1122: 
fp@1122:             if (strlen(slave.name)) {
fp@1122:                 slaveInfo.name = slave.name;
fp@1122:             } else {
fp@1122:                 str << "0x" << hex << setfill('0')
fp@1122:                     << setw(8) << slave.vendor_id << ":0x"
fp@1122:                     << setw(8) << slave.product_code;
fp@1122:                 slaveInfo.name = str.str();
fp@1122:                 str.str("");
fp@1122:             }
fp@1122: 
fp@1122: 
fp@1122:             slaveInfoList.push_back(slaveInfo);
fp@1122: 
fp@1122:             if (slaveInfo.pos.length() > maxPosWidth)
fp@1122:                 maxPosWidth = slaveInfo.pos.length();
fp@1122:             if (slaveInfo.alias.length() > maxAliasWidth)
fp@1122:                 maxAliasWidth = slaveInfo.alias.length();
fp@1122:             if (slaveInfo.relPos.length() > maxRelPosWidth)
fp@1122:                 maxRelPosWidth = slaveInfo.relPos.length();
fp@1122:             if (slaveInfo.state.length() > maxStateWidth)
fp@1122:                 maxStateWidth = slaveInfo.state.length();
fp@1122:         }
fp@1122: 
fp@1122:         aliasIndex++;
fp@1122:     }
fp@1122: 
fp@1122:     for (iter = slaveInfoList.begin(); iter != slaveInfoList.end(); iter++) {
fp@1122:         cout << setfill(' ') << right
fp@1122:             << setw(maxPosWidth) << iter->pos << "  "
fp@1122:             << setw(maxAliasWidth) << iter->alias
fp@1122:             << ":" << left
fp@1122:             << setw(maxRelPosWidth) << iter->relPos << "  "
fp@1122:             << setw(maxStateWidth) << iter->state << "  "
fp@1122:             << iter->flag << "  "
fp@1122:             << iter->name << endl;
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::showSlave(uint16_t slavePosition)
fp@1122: {
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     list<string> protoList;
fp@1122:     list<string>::const_iterator protoIter;
fp@1122:     
fp@1122:     getSlave(&slave, slavePosition);
fp@1122:         
fp@1122:     cout << "=== Slave " << dec << slavePosition << " ===" << endl;
fp@1122:     
fp@1122:     if (slave.alias)
fp@1122:         cout << "Alias: " << slave.alias << endl;
fp@1122: 
fp@1122:     cout
fp@1122:         << "State: " << slaveState(slave.state) << endl
fp@1122:         << "Flag: " << (slave.error_flag ? 'E' : '+') << endl
fp@1122:         << "Identity:" << endl
fp@1122:         << "  Vendor Id:       0x"
fp@1122:         << hex << setfill('0')
fp@1122:         << setw(8) << slave.vendor_id << endl
fp@1122:         << "  Product code:    0x"
fp@1122:         << setw(8) << slave.product_code << endl
fp@1122:         << "  Revision number: 0x"
fp@1122:         << setw(8) << slave.revision_number << endl
fp@1122:         << "  Serial number:   0x"
fp@1122:         << setw(8) << slave.serial_number << endl;
fp@1122: 
fp@1122:     if (slave.mailbox_protocols) {
fp@1122:         cout << "Mailboxes:" << endl
fp@1122:         << "  RX: 0x"
fp@1122:         << hex << setw(4) << slave.rx_mailbox_offset << "/"
fp@1122:         << dec << slave.rx_mailbox_size
fp@1122:         << ", TX: 0x"
fp@1122:         << hex << setw(4) << slave.tx_mailbox_offset << "/"
fp@1122:         << dec << slave.tx_mailbox_size << endl
fp@1122:         << "  Supported protocols: ";
fp@1122: 
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_AOE) {
fp@1122:             protoList.push_back("AoE");
fp@1122:         }
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_EOE) {
fp@1122:             protoList.push_back("EoE");
fp@1122:         }
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_COE) {
fp@1122:             protoList.push_back("CoE");
fp@1122:         }
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_FOE) {
fp@1122:             protoList.push_back("FoE");
fp@1122:         }
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_SOE) {
fp@1122:             protoList.push_back("SoE");
fp@1122:         }
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_VOE) {
fp@1122:             protoList.push_back("VoE");
fp@1122:         }
fp@1122: 
fp@1122:         for (protoIter = protoList.begin(); protoIter != protoList.end();
fp@1122:                 protoIter++) {
fp@1122:             if (protoIter != protoList.begin())
fp@1122:                 cout << ", ";
fp@1122:             cout << *protoIter;
fp@1122:         }
fp@1122:         cout << endl;
fp@1122:     }
fp@1122: 
fp@1122:     if (slave.has_general_category) {
fp@1122:         cout << "General:" << endl
fp@1122:             << "  Group: " << slave.group << endl
fp@1122:             << "  Image name: " << slave.image << endl
fp@1122:             << "  Order number: " << slave.order << endl
fp@1122:             << "  Device name: " << slave.name << endl;
fp@1122: 
fp@1122:         if (slave.mailbox_protocols & EC_MBOX_COE) {
fp@1122:             cout << "  CoE details:" << endl
fp@1122:                 << "    Enable Sdo: "
fp@1122:                 << (slave.coe_details.enable_sdo ? "yes" : "no") << endl
fp@1122:                 << "    Enable Sdo Info: "
fp@1122:                 << (slave.coe_details.enable_sdo_info ? "yes" : "no") << endl
fp@1122:                 << "    Enable Pdo Assign: "
fp@1122:                 << (slave.coe_details.enable_pdo_assign
fp@1122:                         ? "yes" : "no") << endl
fp@1122:                 << "    Enable Pdo Configuration: "
fp@1122:                 << (slave.coe_details.enable_pdo_configuration
fp@1122:                         ? "yes" : "no") << endl
fp@1122:                 << "    Enable Upload at startup: "
fp@1122:                 << (slave.coe_details.enable_upload_at_startup
fp@1122:                         ? "yes" : "no") << endl
fp@1122:                 << "    Enable Sdo complete access: "
fp@1122:                 << (slave.coe_details.enable_sdo_complete_access
fp@1122:                         ? "yes" : "no") << endl;
fp@1122:         }
fp@1122: 
fp@1122:         cout << "  Flags:" << endl
fp@1122:             << "    Enable SafeOp: "
fp@1122:             << (slave.general_flags.enable_safeop ? "yes" : "no") << endl
fp@1122:             << "    Enable notLRW: "
fp@1122:             << (slave.general_flags.enable_not_lrw ? "yes" : "no") << endl
fp@1122:             << "  Current consumption: "
fp@1122:             << dec << slave.current_on_ebus << " mA" << endl;
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::generateSlaveXml(uint16_t slavePosition)
fp@1122: {
fp@1122:     ec_ioctl_slave_t slave;
fp@1122:     ec_ioctl_slave_sync_t sync;
fp@1122:     ec_ioctl_slave_sync_pdo_t pdo;
fp@1122:     string pdoType;
fp@1122:     ec_ioctl_slave_sync_pdo_entry_t entry;
fp@1122:     unsigned int i, j, k;
fp@1122:     
fp@1122:     getSlave(&slave, slavePosition);
fp@1122: 
fp@1122:     cout
fp@1122:         << "<?xml version=\"1.0\" ?>" << endl
fp@1122:         << "  <EtherCATInfo>" << endl
fp@1122:         << "    <!-- Slave " << slave.position << " -->" << endl
fp@1122:         << "    <Vendor>" << endl
fp@1122:         << "      <Id>" << slave.vendor_id << "</Id>" << endl
fp@1122:         << "    </Vendor>" << endl
fp@1122:         << "    <Descriptions>" << endl
fp@1122:         << "      <Devices>" << endl
fp@1122:         << "        <Device>" << endl
fp@1122:         << "          <Type ProductCode=\"#x"
fp@1122:         << hex << setfill('0') << setw(8) << slave.product_code
fp@1122:         << "\" RevisionNo=\"#x"
fp@1122:         << hex << setfill('0') << setw(8) << slave.revision_number
fp@1122:         << "\">" << slave.order << "</Type>" << endl;
fp@1122: 
fp@1122:     if (strlen(slave.name)) {
fp@1122:         cout
fp@1122:             << "          <Name><![CDATA["
fp@1122:             << slave.name
fp@1122:             << "]]></Name>" << endl;
fp@1122:     }
fp@1122: 
fp@1122:     for (i = 0; i < slave.sync_count; i++) {
fp@1122:         getSync(&sync, slavePosition, i);
fp@1122: 
fp@1122:         cout
fp@1122:             << "          <Sm Enable=\"" << dec << (unsigned int) sync.enable
fp@1122:             << "\" StartAddress=\"" << sync.physical_start_address
fp@1122:             << "\" ControlByte=\"" << (unsigned int) sync.control_register
fp@1122:             << "\" DefaultSize=\"" << sync.default_size
fp@1122:             << "\" />" << endl;
fp@1122:     }
fp@1122: 
fp@1122:     for (i = 0; i < slave.sync_count; i++) {
fp@1122:         getSync(&sync, slavePosition, i);
fp@1122: 
fp@1122:         for (j = 0; j < sync.pdo_count; j++) {
fp@1122:             getPdo(&pdo, slavePosition, i, j);
fp@1122:             pdoType = (sync.control_register & 0x04 ? "R" : "T");
fp@1122:             pdoType += "xPdo";
fp@1122: 
fp@1122:             cout
fp@1122:                 << "          <" << pdoType
fp@1122:                 << " Sm=\"" << i << "\" Fixed=\"1\" Mandatory=\"1\">" << endl
fp@1122:                 << "            <Index>#x"
fp@1122:                 << hex << setfill('0') << setw(4) << pdo.index
fp@1122:                 << "</Index>" << endl
fp@1122:                 << "            <Name>" << pdo.name << "</Name>" << endl;
fp@1122: 
fp@1122:             for (k = 0; k < pdo.entry_count; k++) {
fp@1122:                 getPdoEntry(&entry, slavePosition, i, j, k);
fp@1122: 
fp@1122:                 cout
fp@1122:                     << "            <Entry>" << endl
fp@1122:                     << "              <Index>#x"
fp@1122:                     << hex << setfill('0') << setw(4) << entry.index
fp@1122:                     << "</Index>" << endl;
fp@1122:                 if (entry.index)
fp@1122:                     cout
fp@1122:                         << "              <SubIndex>"
fp@1122:                         << dec << (unsigned int) entry.subindex
fp@1122:                         << "</SubIndex>" << endl;
fp@1122:                 
fp@1122:                 cout
fp@1122:                     << "              <BitLen>"
fp@1122:                     << dec << (unsigned int) entry.bit_length
fp@1122:                     << "</BitLen>" << endl;
fp@1122: 
fp@1122:                 if (entry.index) {
fp@1122:                     cout
fp@1122:                         << "              <Name>" << entry.name
fp@1122:                         << "</Name>" << endl
fp@1122:                         << "              <DataType>";
fp@1122: 
fp@1122:                     if (entry.bit_length == 1) {
fp@1122:                         cout << "BOOL";
fp@1122:                     } else if (!(entry.bit_length % 8)) {
fp@1122:                         if (entry.bit_length <= 64)
fp@1122:                             cout << "UINT" << (unsigned int) entry.bit_length;
fp@1122:                         else
fp@1122:                             cout << "STRING("
fp@1122:                                 << (unsigned int) (entry.bit_length / 8)
fp@1122:                                 << ")";
fp@1122:                     } else {
fp@1122:                         cerr << "Invalid bit length "
fp@1122:                             << (unsigned int) entry.bit_length << endl;
fp@1122:                     }
fp@1122: 
fp@1122:                         cout << "</DataType>" << endl;
fp@1122:                 }
fp@1122: 
fp@1122:                 cout << "            </Entry>" << endl;
fp@1122:             }
fp@1122: 
fp@1122:             cout
fp@1122:                 << "          </" << pdoType << ">" << endl;
fp@1122:         }
fp@1122:     }
fp@1122: 
fp@1122:     cout
fp@1122:         << "        </Device>" << endl
fp@1122:         << "     </Devices>" << endl
fp@1122:         << "  </Descriptions>" << endl
fp@1122:         << "</EtherCATInfo>" << endl;
fp@1122: }
fp@1122: #endif
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: unsigned int MasterDevice::slaveCount()
fp@1122: {
fp@1122:     ec_ioctl_master_t data;
fp@1122: 
fp@1122:     getMaster(&data);
fp@1122:     return data.slave_count;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getMaster(ec_ioctl_master_t *data)
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get master information: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfig(ec_ioctl_config_t *data, unsigned int index)
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave configuration: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigPdo(
fp@1122:         ec_ioctl_config_pdo_t *data,
fp@1122:         unsigned int index,
fp@1122:         uint8_t sync_index,
fp@1122:         uint16_t pdo_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sync_index = sync_index;
fp@1122:     data->pdo_pos = pdo_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave config Pdo: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigPdoEntry(
fp@1122:         ec_ioctl_config_pdo_entry_t *data,
fp@1122:         unsigned int index,
fp@1122:         uint8_t sync_index,
fp@1122:         uint16_t pdo_pos,
fp@1122:         uint8_t entry_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sync_index = sync_index;
fp@1122:     data->pdo_pos = pdo_pos;
fp@1122:     data->entry_pos = entry_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave config Pdo entry: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigSdo(
fp@1122:         ec_ioctl_config_sdo_t *data,
fp@1122:         unsigned int index,
fp@1122:         unsigned int sdo_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sdo_pos = sdo_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave config Sdo: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getDomain(ec_ioctl_domain_t *data, unsigned int index)
fp@1122: {
fp@1122:     data->index = index;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get domain: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Domain " << index << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getData(ec_ioctl_domain_data_t *data, unsigned int domainIndex,
fp@1122:         unsigned int dataSize, unsigned char *mem)
fp@1122: {
fp@1122:     data->domain_index = domainIndex;
fp@1122:     data->data_size = dataSize;
fp@1122:     data->target = mem;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get domain data: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
fp@1122: {
fp@1122:     slave->position = slaveIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Slave " << slaveIndex << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getFmmu(
fp@1122:         ec_ioctl_domain_fmmu_t *fmmu,
fp@1122:         unsigned int domainIndex,
fp@1122:         unsigned int fmmuIndex
fp@1122:         )
fp@1122: {
fp@1122:     fmmu->domain_index = domainIndex;
fp@1122:     fmmu->fmmu_index = fmmuIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get domain FMMU: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Either domain " << domainIndex << " does not exist, "
fp@1122:                 << "or it contains less than " << (unsigned int) fmmuIndex + 1
fp@1122:                 << " FMMus!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSync(
fp@1122:         ec_ioctl_slave_sync_t *sync,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex
fp@1122:         )
fp@1122: {
fp@1122:     sync->slave_position = slaveIndex;
fp@1122:     sync->sync_index = syncIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get sync manager: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Either slave " << slaveIndex << " does not exist, "
fp@1122:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@1122:                 << " sync managers!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getPdo(
fp@1122:         ec_ioctl_slave_sync_pdo_t *pdo,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex,
fp@1122:         uint8_t pdoPos
fp@1122:         )
fp@1122: {
fp@1122:     pdo->slave_position = slaveIndex;
fp@1122:     pdo->sync_index = syncIndex;
fp@1122:     pdo->pdo_pos = pdoPos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get Pdo: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Either slave " << slaveIndex << " does not exist, "
fp@1122:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@1122:                 << " sync managers, or sync manager "
fp@1122:                 << (unsigned int) syncIndex << " contains less than "
fp@1122:                 << pdoPos + 1 << " Pdos!" << endl;
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getPdoEntry(
fp@1122:         ec_ioctl_slave_sync_pdo_entry_t *entry,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex,
fp@1122:         uint8_t pdoPos,
fp@1122:         uint8_t entryPos
fp@1122:         )
fp@1122: {
fp@1122:     entry->slave_position = slaveIndex;
fp@1122:     entry->sync_index = syncIndex;
fp@1122:     entry->pdo_pos = pdoPos;
fp@1122:     entry->entry_pos = entryPos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get Pdo entry: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Either slave " << slaveIndex << " does not exist, "
fp@1122:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@1122:                 << " sync managers, or sync manager "
fp@1122:                 << (unsigned int) syncIndex << " contains less than "
fp@1122:                 << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
fp@1122:                 << " contains less than " << (unsigned int) entryPos + 1
fp@1122:                 << " entries!" << endl;
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSdo(
fp@1122:         ec_ioctl_slave_sdo_t *sdo,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint16_t sdoPosition
fp@1122:         )
fp@1122: {
fp@1122:     sdo->slave_position = slaveIndex;
fp@1122:     sdo->sdo_position = sdoPosition;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get Sdo: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Either slave " << slaveIndex << " does not exist, "
fp@1122:                 << "or it contains less than " << sdoPosition + 1 << " Sdos!"
fp@1122:                 << endl;
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSdoEntry(
fp@1122:         ec_ioctl_slave_sdo_entry_t *entry,
fp@1122:         uint16_t slaveIndex,
fp@1122:         int sdoSpec,
fp@1122:         uint8_t entrySubindex
fp@1122:         )
fp@1122: {
fp@1122:     entry->slave_position = slaveIndex;
fp@1122:     entry->sdo_spec = sdoSpec;
fp@1122:     entry->sdo_entry_subindex = entrySubindex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get Sdo entry: ";
fp@1122:         err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::readSii(
fp@1122:         ec_ioctl_slave_sii_t *data
fp@1122:         )
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to read SII: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::writeSii(
fp@1122:         ec_ioctl_slave_sii_t *data
fp@1122:         )
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to write SII: " << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: #if 0
fp@1122: void MasterDevice::requestState(
fp@1122:         uint16_t slavePosition,
fp@1122:         uint8_t state
fp@1122:         )
fp@1122: {
fp@1122:     ec_ioctl_slave_state_t data;
fp@1122: 
fp@1122:     data.slave_position = slavePosition;
fp@1122:     data.requested_state = state;
fp@1122:     
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to request slave state: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Slave " << slavePosition << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1122:         throw MasterDeviceException(err.str());
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: string Master::slaveState(uint8_t state)
fp@1122: {
fp@1122:     switch (state) {
fp@1122:         case 1: return "INIT";
fp@1122:         case 2: return "PREOP";
fp@1122:         case 4: return "SAFEOP";
fp@1122:         case 8: return "OP";
fp@1122:         default: return "???";
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void Master::printRawData(
fp@1122:         const uint8_t *data,
fp@1122:         unsigned int size)
fp@1122: {
fp@1122:     cout << hex << setfill('0');
fp@1122:     while (size--) {
fp@1122:         cout << "0x" << setw(2) << (unsigned int) *data++;
fp@1122:         if (size)
fp@1122:             cout << " ";
fp@1122:     }
fp@1122:     cout << endl;
fp@1122: }
fp@1122: 
fp@1122: #endif
fp@1122: /*****************************************************************************/