fp@922: /*****************************************************************************
fp@922:  *
fp@922:  * $Id$
fp@922:  *
fp@922:  ****************************************************************************/
fp@922: 
fp@922: #include <sys/types.h>
fp@922: #include <sys/stat.h>
fp@922: #include <fcntl.h>
fp@922: #include <errno.h>
fp@922: #include <sys/ioctl.h>
fp@922: 
fp@922: #include <iostream>
fp@922: #include <iomanip>
fp@922: #include <sstream>
fp@980: #include <fstream>
fp@960: #include <cctype> // toupper()
fp@922: using namespace std;
fp@922: 
fp@922: #include "Master.h"
fp@922: 
fp@974: #define swap16(x) \
fp@968:         ((uint16_t)( \
fp@968:         (((uint16_t)(x) & 0x00ffU) << 8) | \
fp@968:         (((uint16_t)(x) & 0xff00U) >> 8) ))
fp@974: #define swap32(x) \
fp@968:         ((uint32_t)( \
fp@968:         (((uint32_t)(x) & 0x000000ffUL) << 24) | \
fp@968:         (((uint32_t)(x) & 0x0000ff00UL) <<  8) | \
fp@968:         (((uint32_t)(x) & 0x00ff0000UL) >>  8) | \
fp@968:         (((uint32_t)(x) & 0xff000000UL) >> 24) ))
fp@968: 
fp@974: #if __BYTE_ORDER == __LITTLE_ENDIAN
fp@974: 
fp@974: #define le16tocpu(x) x
fp@974: #define le32tocpu(x) x
fp@974: 
fp@974: #define cputole16(x) x
fp@974: #define cputole32(x) x
fp@974: 
fp@974: #elif __BYTE_ORDER == __BIG_ENDIAN
fp@974: 
fp@974: #define le16tocpu(x) swap16(x)
fp@974: #define le32tocpu(x) swap32(x)
fp@974: 
fp@974: #define cputole16(x) swap16(x)
fp@974: #define cputole32(x) swap32(x)
fp@974: 
fp@968: #endif
fp@968: 
fp@968: /****************************************************************************/
fp@968: 
fp@968: struct CoEDataType {
fp@968:     const char *name;
fp@968:     uint16_t coeCode;
fp@968:     unsigned int byteSize;
fp@968: };
fp@968: 
fp@968: static const CoEDataType dataTypes[] = {
fp@968:     {"int8",   0x0002, 1},
fp@968:     {"int16",  0x0003, 2},
fp@968:     {"int32",  0x0004, 4},
fp@968:     {"uint8",  0x0005, 1},
fp@968:     {"uint16", 0x0006, 2},
fp@968:     {"uint32", 0x0007, 4},
fp@968:     {"string", 0x0009, 0},
fp@968:     {"raw",    0xffff, 0},
fp@968:     {}
fp@968: };
fp@968: 
fp@968: /****************************************************************************/
fp@968: 
fp@968: const CoEDataType *findDataType(const string &str)
fp@968: {
fp@968:     const CoEDataType *d;
fp@968:     
fp@968:     for (d = dataTypes; d->name; d++)
fp@968:         if (str == d->name)
fp@968:             return d;
fp@968: 
fp@968:     return NULL;
fp@968: }
fp@968: 
fp@968: /****************************************************************************/
fp@968: 
fp@968: const CoEDataType *findDataType(uint16_t code)
fp@968: {
fp@968:     const CoEDataType *d;
fp@968:     
fp@968:     for (d = dataTypes; d->name; d++)
fp@968:         if (code == d->coeCode)
fp@968:             return d;
fp@968: 
fp@968:     return NULL;
fp@968: }
fp@968: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: Master::Master()
fp@922: {
fp@922:     index = 0;
fp@922:     fd = -1;
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: Master::~Master()
fp@922: {
fp@922:     close();
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@972: void Master::setIndex(unsigned int i)
fp@972: {
fp@972:     index = i;
fp@922: }
fp@922: 
fp@980: /*****************************************************************************/
fp@980: 
fp@980: /**
fp@980:  * Writes the Secondary slave address (alias) to the slave's SII.
fp@980:  */
fp@980: void Master::writeAlias(
fp@980:         int slavePosition,
fp@980:         bool force,
fp@980:         const vector<string> &commandArgs
fp@980:         )
fp@980: {
fp@980:     ec_ioctl_sii_t data;
fp@980:     ec_ioctl_slave_t slave;
fp@980:     unsigned int i;
fp@980:     uint16_t alias;
fp@980:     stringstream err, strAlias;
fp@980:     int number;
fp@980: 
fp@980:     if (commandArgs.size() != 1) {
fp@980:         stringstream err;
fp@980:         err << "'alias' takes exactly one argument!";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     strAlias << commandArgs[0];
fp@980:     strAlias >> hex >> number;
fp@980:     if (strAlias.fail() || number < 0x0000 || number > 0xffff) {
fp@980:         err << "Invalid alias '" << commandArgs[0] << "'!";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980:     alias = number;
fp@980: 
fp@980:     if (slavePosition == -1) {
fp@980:         unsigned int numSlaves, i;
fp@980: 
fp@980:         if (!force) {
fp@980:             err << "This will write the alias addresses of all slaves to 0x"
fp@980:                 << hex << setfill('0') << setw(4) << alias << "! "
fp@980:                 << "Please specify --force to proceed.";
fp@980:             throw MasterException(err.str());
fp@980:         }
fp@980: 
fp@980:         open(ReadWrite);
fp@980:         numSlaves = slaveCount();
fp@980: 
fp@980:         for (i = 0; i < numSlaves; i++) {
fp@980:             writeSlaveAlias(i, alias);
fp@980:         }
fp@980:     } else {
fp@980:         open(ReadWrite);
fp@980:         writeSlaveAlias(slavePosition, alias);
fp@980:     }
fp@980: }
fp@980: 
fp@922: /****************************************************************************/
fp@922: 
fp@949: void Master::outputData(int domainIndex)
fp@949: {
fp@972:     open(Read);
fp@972: 
fp@949:     if (domainIndex == -1) {
fp@949:         unsigned int numDomains = domainCount(), i;
fp@949: 
fp@949:         for (i = 0; i < numDomains; i++) {
fp@949:             outputDomainData(i);
fp@949:         }
fp@949:     } else {
fp@949:         outputDomainData(domainIndex);
fp@949:     }
fp@949: }
fp@949: 
fp@949: /****************************************************************************/
fp@949: 
fp@956: void Master::setDebug(const vector<string> &commandArgs)
fp@956: {
fp@956:     stringstream str;
fp@956:     int debugLevel;
fp@956:     
fp@956:     if (commandArgs.size() != 1) {
fp@956:         stringstream err;
fp@956:         err << "'debug' takes exactly one argument!";
fp@956:         throw MasterException(err.str());
fp@956:     }
fp@956: 
fp@956:     str << commandArgs[0];
fp@956:     str >> debugLevel;
fp@956: 
fp@956:     if (str.fail()) {
fp@956:         stringstream err;
fp@956:         err << "Invalid debug level '" << commandArgs[0] << "'!";
fp@956:         throw MasterException(err.str());
fp@956:     }
fp@956: 
fp@972:     open(ReadWrite);
fp@972: 
fp@957:     if (ioctl(fd, EC_IOCTL_SET_DEBUG, debugLevel) < 0) {
fp@956:         stringstream err;
fp@956:         err << "Failed to set debug level: " << strerror(errno);
fp@956:         throw MasterException(err.str());
fp@956:     }
fp@956: }
fp@956: 
fp@956: /****************************************************************************/
fp@956: 
fp@948: void Master::showDomains(int domainIndex)
fp@948: {
fp@972:     open(Read);
fp@972: 
fp@948:     if (domainIndex == -1) {
fp@948:         unsigned int numDomains = domainCount(), i;
fp@948: 
fp@948:         for (i = 0; i < numDomains; i++) {
fp@948:             showDomain(i);
fp@948:         }
fp@948:     } else {
fp@948:         showDomain(domainIndex);
fp@948:     }
fp@948: }
fp@948: 
fp@948: /****************************************************************************/
fp@948: 
fp@922: void Master::listSlaves()
fp@922: {
fp@972:     unsigned int numSlaves, i;
fp@935:     ec_ioctl_slave_t slave;
fp@922:     uint16_t lastAlias, aliasIndex;
fp@922:     
fp@972:     open(Read);
fp@972: 
fp@972:     numSlaves = slaveCount();
fp@972: 
fp@922:     lastAlias = 0;
fp@922:     aliasIndex = 0;
fp@922:     for (i = 0; i < numSlaves; i++) {
fp@935:         getSlave(&slave, i);
fp@980:         cout << setfill(' ') << setw(2) << i << "  ";
fp@935: 
fp@935:         if (slave.alias) {
fp@935:             lastAlias = slave.alias;
fp@922:             aliasIndex = 0;
fp@922:         }
fp@922:         if (lastAlias) {
fp@980:             cout << "#"
fp@980:                 << hex << setfill('0') << setw(4) << lastAlias
fp@980:                 << ":" << dec << aliasIndex;
fp@980:             aliasIndex++;
fp@922:         }
fp@922: 
fp@935:         cout << "  " << slaveState(slave.state) << "  ";
fp@935: 
fp@935:         if (strlen(slave.name)) {
fp@935:             cout << slave.name;
fp@922:         } else {
fp@980:             cout << hex << setfill('0')
fp@980:                 << setw(8) << slave.vendor_id << ":"
fp@980:                 << setw(8) << slave.product_code << dec;
fp@922:         }
fp@922: 
fp@922:         cout << endl;
fp@922:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@957: void Master::showMaster()
fp@957: {
fp@957:     ec_ioctl_master_t data;
fp@957:     stringstream err;
fp@957:     unsigned int i;
fp@957:     
fp@972:     open(Read);
fp@957:     getMaster(&data);
fp@957: 
fp@957:     cout
fp@957:         << "Master" << index << endl
fp@957:         << "  State: ";
fp@957: 
fp@957:     switch (data.mode) {
fp@957:         case 0: cout << "Waiting for device..."; break;
fp@957:         case 1: cout << "Idle"; break;
fp@957:         case 2: cout << "Operation"; break;
fp@957:         default:
fp@957:                 err << "Invalid master state " << data.mode;
fp@957:                 throw MasterException(err.str());
fp@957:     }
fp@957: 
fp@957:     cout << endl
fp@957:         << "  Slaves: " << data.slave_count << endl;
fp@957: 
fp@957:     for (i = 0; i < 2; i++) {
fp@957:         cout << "  Device" << i << ": ";
fp@957:         if (data.devices[i].address[0] == 0x00
fp@957:                 && data.devices[i].address[1] == 0x00
fp@957:                 && data.devices[i].address[2] == 0x00
fp@957:                 && data.devices[i].address[3] == 0x00
fp@957:                 && data.devices[i].address[4] == 0x00
fp@957:                 && data.devices[i].address[5] == 0x00) {
fp@957:             cout << "None.";
fp@957:         } else {
fp@957:             cout << hex << setfill('0')
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[0] << ":"
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[1] << ":"
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[2] << ":"
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[3] << ":"
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[4] << ":"
fp@957:                 << setw(2) << (unsigned int) data.devices[i].address[5] << " ("
fp@957:                 << (data.devices[i].attached ? "attached" : "waiting...")
fp@957:                 << ")" << endl << dec
fp@957:                 << "    Tx count: " << data.devices[i].tx_count << endl
fp@957:                 << "    Rx count: " << data.devices[i].rx_count;
fp@957:         }
fp@957:         cout << endl;
fp@957:     }
fp@957: }
fp@957: 
fp@957: /****************************************************************************/
fp@957: 
fp@966: void Master::listPdos(int slavePosition, bool quiet)
fp@935: {
fp@972:     open(Read);
fp@972: 
fp@936:     if (slavePosition == -1) {
fp@936:         unsigned int numSlaves = slaveCount(), i;
fp@936: 
fp@936:         for (i = 0; i < numSlaves; i++) {
fp@966:             listSlavePdos(i, quiet, true);
fp@936:         }
fp@936:     } else {
fp@966:         listSlavePdos(slavePosition, quiet, false);
fp@966:     }
fp@966: }
fp@966: 
fp@966: /****************************************************************************/
fp@966: 
fp@966: void Master::listSdos(int slavePosition, bool quiet)
fp@965: {
fp@972:     open(Read);
fp@972: 
fp@965:     if (slavePosition == -1) {
fp@965:         unsigned int numSlaves = slaveCount(), i;
fp@965: 
fp@965:         for (i = 0; i < numSlaves; i++) {
fp@966:             listSlaveSdos(i, quiet, true);
fp@965:         }
fp@965:     } else {
fp@966:         listSlaveSdos(slavePosition, quiet, false);
fp@965:     }
fp@965: }
fp@965: 
fp@965: /****************************************************************************/
fp@965: 
fp@974: void Master::sdoDownload(
fp@974:         int slavePosition,
fp@974:         const string &dataTypeStr,
fp@974:         const vector<string> &commandArgs
fp@974:         )
fp@974: {
fp@974:     stringstream strIndex, strSubIndex, strValue, err;
fp@974:     int number, sval;
fp@974:     ec_ioctl_sdo_download_t data;
fp@974:     unsigned int i, uval;
fp@974:     const CoEDataType *dataType = NULL;
fp@974: 
fp@974:     if (slavePosition < 0) {
fp@974:         err << "'sdo_download' requires a slave! Please specify --slave.";
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974:     data.slave_position = slavePosition;
fp@974: 
fp@974:     if (commandArgs.size() != 3) {
fp@974:         err << "'sdo_download' takes 3 arguments!";
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974: 
fp@974:     strIndex << commandArgs[0];
fp@974:     strIndex >> hex >> number;
fp@974:     if (strIndex.fail() || number < 0x0000 || number > 0xffff) {
fp@974:         err << "Invalid Sdo index '" << commandArgs[0] << "'!";
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974:     data.sdo_index = number;
fp@974: 
fp@974:     strSubIndex << commandArgs[1];
fp@974:     strSubIndex >> hex >> number;
fp@974:     if (strSubIndex.fail() || number < 0x00 || number > 0xff) {
fp@974:         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974:     data.sdo_entry_subindex = number;
fp@974: 
fp@974:     if (dataTypeStr != "") { // data type specified
fp@974:         if (!(dataType = findDataType(dataTypeStr))) {
fp@974:             err << "Invalid data type '" << dataTypeStr << "'!";
fp@974:             throw MasterException(err.str());
fp@974:         }
fp@974:     } else { // no data type specified: fetch from dictionary
fp@974:         ec_ioctl_sdo_entry_t entry;
fp@974:         unsigned int entryByteSize;
fp@974: 
fp@982:         open(ReadWrite);
fp@974: 
fp@974:         try {
fp@974:             getSdoEntry(&entry, slavePosition,
fp@974:                     data.sdo_index, data.sdo_entry_subindex);
fp@974:         } catch (MasterException &e) {
fp@974:             err << "Failed to determine Sdo entry data type. "
fp@974:                 << "Please specify --type.";
fp@974:             throw MasterException(err.str());
fp@974:         }
fp@974:         if (!(dataType = findDataType(entry.data_type))) {
fp@974:             err << "Pdo entry has unknown data type 0x"
fp@974:                 << hex << setfill('0') << setw(4) << entry.data_type << "!"
fp@974:                 << " Please specify --type.";
fp@974:             throw MasterException(err.str());
fp@974:         }
fp@974:     }
fp@974: 
fp@974:     if (dataType->byteSize) {
fp@974:         data.data_size = dataType->byteSize;
fp@974:     } else {
fp@974:         data.data_size = DefaultBufferSize;
fp@974:     }
fp@974: 
fp@974:     data.data = new uint8_t[data.data_size + 1];
fp@974: 
fp@974:     strValue << commandArgs[2];
fp@974: 
fp@974:     switch (dataType->coeCode) {
fp@974:         case 0x0002: // int8
fp@974:             strValue >> sval;
fp@974:             if ((uint32_t) sval > 0xff) {
fp@974:                 delete [] data.data;
fp@974:                 err << "Invalid value for type '"
fp@974:                     << dataType->name << "'!";
fp@974:                 throw MasterException(err.str());
fp@974:             }
fp@974:             *data.data = (int8_t) sval;
fp@974:             break;
fp@974:         case 0x0003: // int16
fp@974:             strValue >> sval;
fp@974:             if ((uint32_t) sval > 0xffff) {
fp@974:                 delete [] data.data;
fp@974:                 err << "Invalid value for type '"
fp@974:                     << dataType->name << "'!";
fp@974:                 throw MasterException(err.str());
fp@974:             }
fp@974:             *(int16_t *) data.data = cputole16(sval);
fp@974:             break;
fp@974:         case 0x0004: // int32
fp@974:             strValue >> sval;
fp@974:             *(int32_t *) data.data = cputole32(sval);
fp@974:             break;
fp@974:         case 0x0005: // uint8
fp@974:             strValue >> uval;
fp@974:             if ((uint32_t) uval > 0xff) {
fp@974:                 delete [] data.data;
fp@974:                 err << "Invalid value for type '"
fp@974:                     << dataType->name << "'!";
fp@974:                 throw MasterException(err.str());
fp@974:             }
fp@974:             *data.data = (uint8_t) uval;
fp@974:             break;
fp@974:         case 0x0006: // uint16
fp@974:             strValue >> uval;
fp@974:             if ((uint32_t) uval > 0xffff) {
fp@974:                 delete [] data.data;
fp@974:                 err << "Invalid value for type '"
fp@974:                     << dataType->name << "'!";
fp@974:                 throw MasterException(err.str());
fp@974:             }
fp@974:             *(uint16_t *) data.data = cputole16(uval);
fp@974:             break;
fp@974:         case 0x0007: // uint32
fp@974:             strValue >> uval;
fp@974:             *(uint32_t *) data.data = cputole32(uval);
fp@974:             break;
fp@974:         case 0x0009: // string
fp@974:             if (strValue.str().size() >= data.data_size) {
fp@974:                 err << "String too big";
fp@974:                 throw MasterException(err.str());
fp@974:             }
fp@974:             data.data_size = strValue.str().size();
fp@974:             strValue >> (char *) data.data;
fp@974:             break;
fp@974:         default:
fp@974:             break;
fp@974:     }
fp@974: 
fp@974:     if (strValue.fail()) {
fp@974:         err << "Invalid value argument '" << commandArgs[2] << "'!";
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974: 
fp@974:     open(ReadWrite);
fp@974: 
fp@974:     if (ioctl(fd, EC_IOCTL_SDO_DOWNLOAD, &data) < 0) {
fp@974:         stringstream err;
fp@974:         err << "Failed to download Sdo: " << strerror(errno);
fp@974:         delete [] data.data;
fp@974:         throw MasterException(err.str());
fp@974:     }
fp@974: 
fp@974:     delete [] data.data;
fp@974: }
fp@974: 
fp@974: /****************************************************************************/
fp@974: 
fp@968: void Master::sdoUpload(
fp@968:         int slavePosition,
fp@968:         const string &dataTypeStr,
fp@968:         const vector<string> &commandArgs
fp@968:         )
fp@968: {
fp@968:     stringstream strIndex, strSubIndex;
fp@968:     int number, sval;
fp@968:     ec_ioctl_sdo_upload_t data;
fp@968:     unsigned int i, uval;
fp@968:     const CoEDataType *dataType = NULL;
fp@968: 
fp@968:     if (slavePosition < 0) {
fp@968:         stringstream err;
fp@968:         err << "'sdo_upload' requires a slave! Please specify --slave.";
fp@968:         throw MasterException(err.str());
fp@968:     }
fp@968:     data.slave_position = slavePosition;
fp@968: 
fp@968:     if (commandArgs.size() != 2) {
fp@968:         stringstream err;
fp@968:         err << "'sdo_upload' takes two arguments!";
fp@968:         throw MasterException(err.str());
fp@968:     }
fp@968: 
fp@968:     strIndex << commandArgs[0];
fp@968:     strIndex >> hex >> number;
fp@968:     if (strIndex.fail() || number < 0x0000 || number > 0xffff) {
fp@968:         stringstream err;
fp@968:         err << "Invalid Sdo index '" << commandArgs[0] << "'!";
fp@968:         throw MasterException(err.str());
fp@968:     }
fp@968:     data.sdo_index = number;
fp@968: 
fp@968:     strSubIndex << commandArgs[1];
fp@968:     strSubIndex >> hex >> number;
fp@968:     if (strSubIndex.fail() || number < 0x00 || number > 0xff) {
fp@968:         stringstream err;
fp@968:         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
fp@968:         throw MasterException(err.str());
fp@968:     }
fp@968:     data.sdo_entry_subindex = number;
fp@968: 
fp@968:     if (dataTypeStr != "") { // data type specified
fp@968:         if (!(dataType = findDataType(dataTypeStr))) {
fp@968:             stringstream err;
fp@968:             err << "Invalid data type '" << dataTypeStr << "'!";
fp@968:             throw MasterException(err.str());
fp@968:         }
fp@968:     } else { // no data type specified: fetch from dictionary
fp@968:         ec_ioctl_sdo_entry_t entry;
fp@968:         unsigned int entryByteSize;
fp@972: 
fp@972:         open(Read);
fp@972: 
fp@968:         try {
fp@968:             getSdoEntry(&entry, slavePosition,
fp@968:                     data.sdo_index, data.sdo_entry_subindex);
fp@968:         } catch (MasterException &e) {
fp@968:             stringstream err;
fp@968:             err << "Failed to determine Sdo entry data type. "
fp@968:                 << "Please specify --type.";
fp@968:             throw MasterException(err.str());
fp@968:         }
fp@968:         if (!(dataType = findDataType(entry.data_type))) {
fp@968:             stringstream err;
fp@968:             err << "Pdo entry has unknown data type 0x"
fp@968:                 << hex << setfill('0') << setw(4) << entry.data_type << "!"
fp@968:                 << " Please specify --type.";
fp@968:             throw MasterException(err.str());
fp@968:         }
fp@968:     }
fp@968: 
fp@968:     if (dataType->byteSize) {
fp@968:         data.target_size = dataType->byteSize;
fp@968:     } else {
fp@974:         data.target_size = DefaultBufferSize;
fp@968:     }
fp@968: 
fp@968:     data.target = new uint8_t[data.target_size + 1];
fp@968: 
fp@972:     open(Read);
fp@972: 
fp@968:     if (ioctl(fd, EC_IOCTL_SDO_UPLOAD, &data) < 0) {
fp@968:         stringstream err;
fp@968:         err << "Failed to upload Sdo: " << strerror(errno);
fp@968:         delete [] data.target;
fp@972:         close();
fp@972:         throw MasterException(err.str());
fp@972:     }
fp@972: 
fp@972:     close();
fp@968: 
fp@968:     if (dataType->byteSize && data.data_size != dataType->byteSize) {
fp@968:         stringstream err;
fp@968:         err << "Data type mismatch. Expected " << dataType->name
fp@970:             << " with " << dataType->byteSize << " byte, but got "
fp@970:             << data.data_size << " byte.";
fp@968:         throw MasterException(err.str());
fp@968:     }
fp@968: 
fp@968:     cout << setfill('0');
fp@968:     switch (dataType->coeCode) {
fp@968:         case 0x0002: // int8
fp@968:             sval = *(int8_t *) data.target;
fp@968:             cout << sval << " 0x" << hex << setw(2) << sval << endl;
fp@968:             break;
fp@968:         case 0x0003: // int16
fp@968:             sval = le16tocpu(*(int16_t *) data.target);
fp@968:             cout << sval << " 0x" << hex << setw(4) << sval << endl;
fp@968:             break;
fp@968:         case 0x0004: // int32
fp@968:             sval = le32tocpu(*(int32_t *) data.target);
fp@968:             cout << sval << " 0x" << hex << setw(8) << sval << endl;
fp@968:             break;
fp@968:         case 0x0005: // uint8
fp@968:             uval = (unsigned int) *(uint8_t *) data.target;
fp@968:             cout << uval << " 0x" << hex << setw(2) << uval << endl;
fp@968:             break;
fp@968:         case 0x0006: // uint16
fp@968:             uval = le16tocpu(*(uint16_t *) data.target);
fp@968:             cout << uval << " 0x" << hex << setw(4) << uval << endl;
fp@968:             break;
fp@968:         case 0x0007: // uint32
fp@968:             uval = le32tocpu(*(uint32_t *) data.target);
fp@968:             cout << uval << " 0x" << hex << setw(8) << uval << endl;
fp@968:             break;
fp@968:         case 0x0009: // string
fp@968:             cout << string((const char *) data.target, data.data_size)
fp@968:                 << endl;
fp@968:             break;
fp@968:         default:
fp@968:             printRawData(data.target, data.data_size);
fp@968:             break;
fp@968:     }
fp@968: 
fp@968:     delete [] data.target;
fp@968: }
fp@968: 
fp@968: /****************************************************************************/
fp@968: 
fp@978: void Master::siiRead(int slavePosition)
fp@978: {
fp@980:     ec_ioctl_sii_t data;
fp@978:     ec_ioctl_slave_t slave;
fp@978:     unsigned int i;
fp@978: 
fp@978:     if (slavePosition < 0) {
fp@978:         stringstream err;
fp@978:         err << "'sii_read' requires a slave! Please specify --slave.";
fp@978:         throw MasterException(err.str());
fp@978:     }
fp@978:     data.slave_position = slavePosition;
fp@978: 
fp@978:     open(Read);
fp@978: 
fp@978:     getSlave(&slave, slavePosition);
fp@978: 
fp@978:     if (!slave.sii_nwords)
fp@978:         return;
fp@978: 
fp@978:     data.offset = 0;
fp@978:     data.nwords = slave.sii_nwords;
fp@978:     data.words = new uint16_t[data.nwords];
fp@978: 
fp@978:     if (ioctl(fd, EC_IOCTL_SII_READ, &data) < 0) {
fp@978:         stringstream err;
fp@978:         delete [] data.words;
fp@978:         err << "Failed to read SII: " << strerror(errno);
fp@978:         throw MasterException(err.str());
fp@978:     }
fp@978: 
fp@978:     for (i = 0; i < data.nwords; i++) {
fp@978:         uint16_t *w = data.words + i;
fp@978:         cout << *(uint8_t *) w << *((uint8_t *) w + 1);
fp@978:     }
fp@978: 
fp@978:     delete [] data.words;
fp@978: }
fp@978: 
fp@978: /****************************************************************************/
fp@978: 
fp@980: void Master::siiWrite(
fp@980:         int slavePosition,
fp@980:         bool force,
fp@980:         const vector<string> &commandArgs
fp@980:         )
fp@980: {
fp@980:     stringstream err;
fp@980:     ec_ioctl_sii_t data;
fp@980:     ifstream file;
fp@980:     unsigned int byte_size;
fp@980:     const uint16_t *categoryHeader;
fp@980:     uint16_t categoryType, categorySize;
fp@980:     uint8_t crc;
fp@980: 
fp@980:     if (slavePosition < 0) {
fp@980:         err << "'sii_write' requires a slave! Please specify --slave.";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980:     data.slave_position = slavePosition;
fp@980: 
fp@980:     if (commandArgs.size() != 1) {
fp@980:         err << "'ssi_write' takes exactly one argument!";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary);
fp@980:     if (file.fail()) {
fp@980:         err << "Failed to open '" << commandArgs[0] << "'!";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     // get length of file
fp@980:     file.seekg(0, ios::end);
fp@980:     byte_size = file.tellg();
fp@980:     file.seekg(0, ios::beg);
fp@980: 
fp@980:     if (!byte_size || byte_size % 2) {
fp@980:         stringstream err;
fp@980:         err << "Invalid file size! Must be non-zero and even.";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     data.nwords = byte_size / 2;
fp@980:     if (data.nwords < 0x0041 && !force) {
fp@980:         err << "SII data too short (" << data.nwords << " words)! Mimimum is"
fp@980:                 " 40 fixed words + 1 delimiter. Use --force to write anyway.";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     // allocate buffer and read file into buffer
fp@980:     data.words = new uint16_t[data.nwords];
fp@980:     file.read((char *) data.words, byte_size);
fp@980:     file.close();
fp@980: 
fp@980:     if (!force) {
fp@980:         // calculate checksum over words 0 to 6
fp@980:         crc = calcSiiCrc((const uint8_t *) data.words, 14);
fp@980:         if (crc != ((const uint8_t *) data.words)[14]) {
fp@980:             err << "CRC incorrect. Must be 0x"
fp@980:                 << hex << setfill('0') << setw(2) << (unsigned int) crc
fp@980:                 << ". Use --force to write anyway.";
fp@980:             throw MasterException(err.str());
fp@980:         }
fp@980: 
fp@980:         // cycle through categories to detect corruption
fp@980:         categoryHeader = data.words + 0x0040;
fp@980:         categoryType = le16tocpu(*categoryHeader);
fp@980:         while (categoryType != 0xffff) {
fp@980:             if (categoryHeader + 1 > data.words + data.nwords) {
fp@980:                 err << "SII data seem to be corrupted! "
fp@980:                     << "Use --force to write anyway.";
fp@980:                 throw MasterException(err.str());
fp@980:             }
fp@980:             categorySize = le16tocpu(*(categoryHeader + 1));
fp@980:             if (categoryHeader + categorySize + 2 > data.words + data.nwords) {
fp@980:                 err << "SII data seem to be corrupted! "
fp@980:                     "Use --force to write anyway.";
fp@980:                 throw MasterException(err.str());
fp@980:             }
fp@980:             categoryHeader += categorySize + 2;
fp@980:             categoryType = le16tocpu(*categoryHeader);
fp@980:         }
fp@980:     }
fp@980: 
fp@980:     // send data to master
fp@980:     open(ReadWrite);
fp@980:     data.offset = 0;
fp@980:     if (ioctl(fd, EC_IOCTL_SII_WRITE, &data) < 0) {
fp@980:         stringstream err;
fp@980:         err << "Failed to write SII: " << strerror(errno);
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: }
fp@980: 
fp@980: /****************************************************************************/
fp@980: 
fp@960: void Master::requestStates(
fp@960:         int slavePosition,
fp@960:         const vector<string> &commandArgs
fp@960:         )
fp@960: {
fp@960:     string stateStr;
fp@960:     uint8_t state;
fp@960:     
fp@960:     if (commandArgs.size() != 1) {
fp@960:         stringstream err;
fp@960:         err << "'state' takes exactly one argument!";
fp@960:         throw MasterException(err.str());
fp@960:     }
fp@960: 
fp@960:     stateStr = commandArgs[0];
fp@960:     transform(stateStr.begin(), stateStr.end(),
fp@960:             stateStr.begin(), (int (*) (int)) std::toupper);
fp@960: 
fp@960:     if (stateStr == "INIT") {
fp@960:         state = 0x01;
fp@960:     } else if (stateStr == "PREOP") {
fp@960:         state = 0x02;
fp@960:     } else if (stateStr == "SAFEOP") {
fp@960:         state = 0x04;
fp@960:     } else if (stateStr == "OP") {
fp@960:         state = 0x08;
fp@960:     } else {
fp@960:         stringstream err;
fp@960:         err << "Invalid state '" << commandArgs[0] << "'!";
fp@960:         throw MasterException(err.str());
fp@960:     }
fp@960: 
fp@972:     open(ReadWrite);
fp@972: 
fp@960:     if (slavePosition == -1) {
fp@960:         unsigned int i, numSlaves = slaveCount();
fp@960:         for (i = 0; i < numSlaves; i++)
fp@960:             requestState(i, state);
fp@960:     } else {
fp@960:         requestState(slavePosition, state);
fp@960:     }
fp@960: }
fp@960: 
fp@960: /****************************************************************************/
fp@960: 
fp@938: void Master::generateXml(int slavePosition)
fp@938: {
fp@972:     open(Read);
fp@972: 
fp@938:     if (slavePosition == -1) {
fp@938:         unsigned int numSlaves = slaveCount(), i;
fp@938: 
fp@938:         for (i = 0; i < numSlaves; i++) {
fp@938:             generateSlaveXml(i);
fp@938:         }
fp@938:     } else {
fp@938:         generateSlaveXml(slavePosition);
fp@938:     }
fp@938: }
fp@938: 
fp@938: /****************************************************************************/
fp@938: 
fp@972: void Master::open(Permissions perm)
fp@972: {
fp@972:     stringstream deviceName;
fp@972: 
fp@972:     if (fd != -1) { // already open
fp@982:         return;
fp@972:     }
fp@972:     
fp@972:     deviceName << "/dev/EtherCAT" << index;
fp@972:     
fp@972:     if ((fd = ::open(deviceName.str().c_str(),
fp@972:                     perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
fp@972:         stringstream err;
fp@972:         err << "Failed to open master device " << deviceName.str() << ": "
fp@972:             << strerror(errno);
fp@972:         throw MasterException(err.str());
fp@972:     }
fp@972: }
fp@972: 
fp@972: /****************************************************************************/
fp@972: 
fp@972: void Master::close()
fp@972: {
fp@972:     if (fd == -1)
fp@972:         return;
fp@972: 
fp@972:     ::close(fd);
fp@982:     fd = -1;
fp@972: }
fp@972: 
fp@980: /*****************************************************************************/
fp@980: 
fp@980: /**
fp@980:  * Writes the Secondary slave address (alias) to the slave's SII.
fp@980:  */
fp@980: void Master::writeSlaveAlias(
fp@980:         uint16_t slavePosition,
fp@980:         uint16_t alias
fp@980:         )
fp@980: {
fp@980:     ec_ioctl_sii_t data;
fp@980:     ec_ioctl_slave_t slave;
fp@980:     stringstream err;
fp@980:     uint8_t crc;
fp@980: 
fp@980:     open(ReadWrite);
fp@980: 
fp@980:     getSlave(&slave, slavePosition);
fp@980: 
fp@980:     if (slave.sii_nwords < 8) {
fp@980:         err << "Current SII contents are too small to set an alias "
fp@980:             << "(" << slave.sii_nwords << " words)!";
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     data.slave_position = slavePosition;
fp@980:     data.offset = 0;
fp@980:     data.nwords = 8;
fp@980:     data.words = new uint16_t[data.nwords];
fp@980: 
fp@980:     // read first 8 SII words
fp@980:     if (ioctl(fd, EC_IOCTL_SII_READ, &data) < 0) {
fp@980:         delete [] data.words;
fp@980:         err << "Failed to read SII: " << strerror(errno);
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     // write new alias address in word 4
fp@980:     data.words[4] = cputole16(alias);
fp@980: 
fp@980:     // calculate checksum over words 0 to 6
fp@980:     crc = calcSiiCrc((const uint8_t *) data.words, 14);
fp@980: 
fp@980:     // write new checksum into first byte of word 7
fp@980:     *(uint8_t *) (data.words + 7) = crc;
fp@980: 
fp@980:     // write first 8 words with new alias and checksum
fp@980:     if (ioctl(fd, EC_IOCTL_SII_WRITE, &data) < 0) {
fp@980:         delete [] data.words;
fp@980:         err << "Failed to write SII: " << strerror(errno);
fp@980:         throw MasterException(err.str());
fp@980:     }
fp@980: 
fp@980:     delete [] data.words;
fp@980: }
fp@980: 
fp@972: /****************************************************************************/
fp@972: 
fp@949: void Master::outputDomainData(unsigned int domainIndex)
fp@949: {
fp@949:     ec_ioctl_domain_t domain;
fp@949:     ec_ioctl_data_t data;
fp@949:     unsigned char *processData;
fp@949:     unsigned int i;
fp@949:     
fp@949:     getDomain(&domain, domainIndex);
fp@949: 
fp@950:     if (!domain.data_size)
fp@950:         return;
fp@950: 
fp@949:     processData = new unsigned char[domain.data_size];
fp@949: 
fp@950:     try {
fp@950:         getData(&data, domainIndex, domain.data_size, processData);
fp@950:     } catch (MasterException &e) {
fp@949:         delete [] processData;
fp@950:         throw e;
fp@949:     }
fp@949: 
fp@949:     for (i = 0; i < data.data_size; i++)
fp@949:         cout << processData[i];
fp@949:     cout.flush();
fp@949: 
fp@949:     delete [] processData;
fp@949: }
fp@949: 
fp@949: /****************************************************************************/
fp@949: 
fp@948: void Master::showDomain(unsigned int domainIndex)
fp@948: {
fp@950:     ec_ioctl_domain_t domain;
fp@950:     unsigned char *processData;
fp@950:     ec_ioctl_data_t data;
fp@950:     unsigned int i, j;
fp@950:     ec_ioctl_domain_fmmu_t fmmu;
fp@950:     unsigned int dataOffset;
fp@948:     
fp@950:     getDomain(&domain, domainIndex);
fp@948: 
fp@948: 	cout << "Domain" << domainIndex << ":"
fp@948: 		<< " LogBaseAddr 0x"
fp@950: 		<< hex << setfill('0') << setw(8) << domain.logical_base_address
fp@950: 		<< ", Size " << dec << setfill(' ') << setw(3) << domain.data_size
fp@948: 		<< ", WorkingCounter "
fp@951: 		<< dec << domain.working_counter << "/"
fp@951:         << domain.expected_working_counter << endl;
fp@950: 
fp@950:     if (!domain.data_size)
fp@950:         return;
fp@950: 
fp@950:     processData = new unsigned char[domain.data_size];
fp@950: 
fp@950:     try {
fp@950:         getData(&data, domainIndex, domain.data_size, processData);
fp@950:     } catch (MasterException &e) {
fp@950:         delete [] processData;
fp@950:         throw e;
fp@950:     }
fp@950: 
fp@950:     for (i = 0; i < domain.fmmu_count; i++) {
fp@950:         getFmmu(&fmmu, domainIndex, i);
fp@950: 
fp@951:         cout << "  SlaveConfig "
fp@951:             << fmmu.slave_config_alias << ":" << fmmu.slave_config_position
fp@951:             << ", Dir "
fp@951:             << setfill(' ') << setw(3) << (fmmu.fmmu_dir ? "In" : "Out")
fp@950:             << ", LogAddr 0x" 
fp@950:             << hex << setfill('0') << setw(8) << fmmu.logical_address
fp@951:             << ", Size " << dec << fmmu.data_size << endl;
fp@950: 
fp@950:         dataOffset = fmmu.logical_address - domain.logical_base_address;
fp@950:         if (dataOffset + fmmu.data_size > domain.data_size) {
fp@950:             stringstream err;
fp@950:             err << "Fmmu information corrupted!";
fp@950:             delete [] processData;
fp@950:             throw MasterException(err.str());
fp@950:         }
fp@950: 
fp@950:         cout << "    " << hex << setfill('0');
fp@950:         for (j = 0; j < fmmu.data_size; j++) {
fp@950:             cout << setw(2)
fp@950:                 << (unsigned int) *(processData + dataOffset + j) << " ";
fp@950:         }
fp@950:         cout << endl;
fp@950:     }
fp@950: 
fp@950:     delete [] processData;
fp@948: }
fp@948: 
fp@948: /****************************************************************************/
fp@948: 
fp@966: void Master::listSlavePdos(
fp@966:         uint16_t slavePosition,
fp@966:         bool quiet,
fp@966:         bool withHeader
fp@966:         )
fp@936: {
fp@935:     ec_ioctl_slave_t slave;
fp@935:     ec_ioctl_sync_t sync;
fp@935:     ec_ioctl_pdo_t pdo;
fp@935:     ec_ioctl_pdo_entry_t entry;
fp@935:     unsigned int i, j, k;
fp@935:     
fp@935:     getSlave(&slave, slavePosition);
fp@935: 
fp@965:     if (withHeader)
fp@936:         cout << "=== Slave " << slavePosition << " ===" << endl;
fp@936: 
fp@935:     for (i = 0; i < slave.sync_count; i++) {
fp@935:         getSync(&sync, slavePosition, i);
fp@935: 
fp@935:         cout << "SM" << i << ":"
fp@935:             << " PhysAddr 0x"
fp@935:             << hex << setfill('0') << setw(4) << sync.physical_start_address
fp@935:             << ", DefaultSize "
fp@935:             << dec << setfill(' ') << setw(4) << sync.default_size
fp@935:             << ", ControlRegister 0x"
fp@935:             << hex << setfill('0') << setw(2)
fp@935:             << (unsigned int) sync.control_register
fp@935:             << ", Enable " << dec << (unsigned int) sync.enable
fp@935:             << endl;
fp@935: 
fp@935:         for (j = 0; j < sync.pdo_count; j++) {
fp@935:             getPdo(&pdo, slavePosition, i, j);
fp@935: 
fp@935:             cout << "  " << (pdo.dir ? "T" : "R") << "xPdo 0x"
fp@935:                 << hex << setfill('0') << setw(4) << pdo.index
fp@935:                 << " \"" << pdo.name << "\"" << endl;
fp@935: 
fp@966:             if (quiet)
fp@966:                 continue;
fp@966: 
fp@935:             for (k = 0; k < pdo.entry_count; k++) {
fp@935:                 getPdoEntry(&entry, slavePosition, i, j, k);
fp@935: 
fp@935:                 cout << "    Pdo entry 0x"
fp@935:                     << hex << setfill('0') << setw(4) << entry.index
fp@935:                     << ":" << hex << setfill('0') << setw(2)
fp@935:                     << (unsigned int) entry.subindex
fp@935:                     << ", " << dec << (unsigned int) entry.bit_length
fp@935:                     << " bit, \"" << entry.name << "\"" << endl;
fp@935:             }
fp@935:         }
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@966: void Master::listSlaveSdos(
fp@966:         uint16_t slavePosition,
fp@966:         bool quiet,
fp@966:         bool withHeader
fp@966:         )
fp@965: {
fp@965:     ec_ioctl_slave_t slave;
fp@965:     ec_ioctl_sdo_t sdo;
fp@965:     ec_ioctl_sdo_entry_t entry;
fp@965:     unsigned int i, j, k;
fp@970:     const CoEDataType *d;
fp@965:     
fp@965:     getSlave(&slave, slavePosition);
fp@965: 
fp@965:     if (withHeader)
fp@965:         cout << "=== Slave " << slavePosition << " ===" << endl;
fp@965: 
fp@965:     for (i = 0; i < slave.sdo_count; i++) {
fp@965:         getSdo(&sdo, slavePosition, i);
fp@965: 
fp@970:         cout << "Sdo "
fp@965:             << hex << setfill('0') << setw(4) << sdo.sdo_index
fp@965:             << ", \"" << sdo.name << "\"" << endl;
fp@965: 
fp@966:         if (quiet)
fp@966:             continue;
fp@966: 
fp@965:         for (j = 0; j <= sdo.max_subindex; j++) {
fp@968:             getSdoEntry(&entry, slavePosition, -i, j);
fp@965: 
fp@983:             cout << "  " << hex << setfill('0')
fp@983:                 << setw(4) << sdo.sdo_index << ":" 
fp@983:                 << setw(2) << (unsigned int) entry.sdo_entry_subindex
fp@970:                 << ", ";
fp@970: 
fp@970:             if ((d = findDataType(entry.data_type))) {
fp@970:                 cout << d->name;
fp@970:             } else {
fp@970:                 cout << "type " << setw(4) << entry.data_type;
fp@970:             }
fp@970: 
fp@970:             cout << ", " << dec << entry.bit_length << " bit, \""
fp@965:                 << entry.description << "\"" << endl;
fp@965:         }
fp@965:     }
fp@965: }
fp@965: 
fp@965: /****************************************************************************/
fp@965: 
fp@938: void Master::generateSlaveXml(uint16_t slavePosition)
fp@938: {
fp@938:     ec_ioctl_slave_t slave;
fp@938:     ec_ioctl_sync_t sync;
fp@938:     ec_ioctl_pdo_t pdo;
fp@938:     ec_ioctl_pdo_entry_t entry;
fp@938:     unsigned int i, j, k;
fp@938:     
fp@938:     getSlave(&slave, slavePosition);
fp@938: 
fp@938:     cout
fp@938:         << "<?xml version=\"1.0\" ?>" << endl
fp@938:         << "  <EtherCATInfo>" << endl
fp@938:         << "    <!-- Slave " << slave.position << " -->" << endl
fp@938:         << "    <Vendor>" << endl
fp@938:         << "      <Id>" << slave.vendor_id << "</Id>" << endl
fp@938:         << "    </Vendor>" << endl
fp@938:         << "    <Descriptions>" << endl
fp@938:         << "      <Devices>" << endl
fp@938:         << "        <Device>" << endl
fp@938:         << "          <Type ProductCode=\"#x"
fp@938:         << hex << setfill('0') << setw(8) << slave.product_code
fp@938:         << "\" RevisionNo=\"#x"
fp@938:         << hex << setfill('0') << setw(8) << slave.revision_number
fp@938:         << "\"/>" << endl;
fp@938: 
fp@938:     for (i = 0; i < slave.sync_count; i++) {
fp@938:         getSync(&sync, slavePosition, i);
fp@938: 
fp@938:         for (j = 0; j < sync.pdo_count; j++) {
fp@938:             getPdo(&pdo, slavePosition, i, j);
fp@938: 
fp@938:             cout
fp@938:                 << "          <" << (pdo.dir ? "T" : "R") << "xPdo>" << endl
fp@938:                 << "            <Index>#x"
fp@938:                 << hex << setfill('0') << setw(4) << pdo.index
fp@938:                 << "</Index>" << endl
fp@938:                 << "            <Name>" << pdo.name << "</Name>" << endl;
fp@938: 
fp@938:             for (k = 0; k < pdo.entry_count; k++) {
fp@938:                 getPdoEntry(&entry, slavePosition, i, j, k);
fp@938: 
fp@938:                 cout
fp@938:                     << "            <Entry>" << endl
fp@938:                     << "              <Index>#x"
fp@938:                     << hex << setfill('0') << setw(4) << entry.index
fp@938:                     << "</Index>" << endl;
fp@938:                 if (entry.index)
fp@938:                     cout
fp@938:                         << "              <SubIndex>"
fp@938:                         << dec << (unsigned int) entry.subindex
fp@938:                         << "</SubIndex>" << endl;
fp@938:                 
fp@938:                 cout
fp@938:                     << "              <BitLen>"
fp@938:                     << (unsigned int) entry.bit_length
fp@938:                     << "</BitLen>" << endl;
fp@938: 
fp@938:                 if (entry.index) {
fp@938:                     cout
fp@938:                         << "              <Name>" << entry.name
fp@938:                         << "</Name>" << endl
fp@938:                         << "              <DataType>";
fp@938: 
fp@938:                     if (entry.bit_length == 1) {
fp@938:                         cout << "BOOL";
fp@938:                     } else if (!(entry.bit_length % 8)) {
fp@938:                         if (entry.bit_length <= 64)
fp@938:                             cout << "UINT" << (unsigned int) entry.bit_length;
fp@938:                         else
fp@938:                             cout << "STRING("
fp@938:                                 << (unsigned int) (entry.bit_length / 8)
fp@938:                                 << ")";
fp@938:                     } else {
fp@938:                         cerr << "Invalid bit length "
fp@938:                             << (unsigned int) entry.bit_length << endl;
fp@938:                     }
fp@938: 
fp@938:                         cout << "</DataType>" << endl;
fp@938:                 }
fp@938: 
fp@938:                 cout << "            </Entry>" << endl;
fp@938:             }
fp@938: 
fp@938:             cout
fp@938:                 << "          </" << (pdo.dir ? "T" : "R") << "xPdo>" << endl;
fp@938:         }
fp@938:     }
fp@938: 
fp@938:     cout
fp@938:         << "        </Device>" << endl
fp@938:         << "     </Devices>" << endl
fp@938:         << "  </Descriptions>" << endl
fp@938:         << "</EtherCATInfo>" << endl;
fp@938: }
fp@938: 
fp@938: /****************************************************************************/
fp@938: 
fp@948: unsigned int Master::domainCount()
fp@948: {
fp@948:     int ret;
fp@948: 
fp@948:     if ((ret = ioctl(fd, EC_IOCTL_DOMAIN_COUNT, 0)) < 0) {
fp@948:         stringstream err;
fp@948:         err << "Failed to get number of domains: " << strerror(errno);
fp@948:         throw MasterException(err.str());
fp@948:     }
fp@948: 
fp@948:     return ret;
fp@948: }
fp@948: 
fp@948: /****************************************************************************/
fp@948: 
fp@935: unsigned int Master::slaveCount()
fp@935: {
fp@957:     ec_ioctl_master_t data;
fp@957: 
fp@957:     getMaster(&data);
fp@957:     return data.slave_count;
fp@957: }
fp@957: 
fp@957: /****************************************************************************/
fp@957: 
fp@957: void Master::getMaster(ec_ioctl_master_t *data)
fp@957: {
fp@957:     if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
fp@957:         stringstream err;
fp@957:         err << "Failed to get master information: " << strerror(errno);
fp@957:         throw MasterException(err.str());
fp@957:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@948: void Master::getDomain(ec_ioctl_domain_t *data, unsigned int index)
fp@948: {
fp@948:     data->index = index;
fp@948: 
fp@948:     if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
fp@948:         stringstream err;
fp@948:         err << "Failed to get domain: ";
fp@948:         if (errno == EINVAL)
fp@948:             err << "Domain " << index << " does not exist!";
fp@948:         else
fp@948:             err << strerror(errno);
fp@948:         throw MasterException(err.str());
fp@948:     }
fp@948: }
fp@948: 
fp@948: /****************************************************************************/
fp@948: 
fp@950: void Master::getData(ec_ioctl_data_t *data, unsigned int domainIndex,
fp@950:         unsigned int dataSize, unsigned char *mem)
fp@950: {
fp@950:     data->domain_index = domainIndex;
fp@950:     data->data_size = dataSize;
fp@950:     data->target = mem;
fp@950: 
fp@950:     if (ioctl(fd, EC_IOCTL_DATA, data) < 0) {
fp@950:         stringstream err;
fp@950:         err << "Failed to get domain data: " << strerror(errno);
fp@950:         throw MasterException(err.str());
fp@950:     }
fp@950: }
fp@950: 
fp@950: /****************************************************************************/
fp@950: 
fp@935: void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
fp@935: {
fp@935:     slave->position = slaveIndex;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get slave: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Slave " << slaveIndex << " does not exist!";
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@950: void Master::getFmmu(
fp@950:         ec_ioctl_domain_fmmu_t *fmmu,
fp@950:         unsigned int domainIndex,
fp@950:         unsigned int fmmuIndex
fp@950:         )
fp@950: {
fp@950:     fmmu->domain_index = domainIndex;
fp@950:     fmmu->fmmu_index = fmmuIndex;
fp@950: 
fp@950:     if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
fp@950:         stringstream err;
fp@950:         err << "Failed to get domain FMMU: ";
fp@950:         if (errno == EINVAL)
fp@950:             err << "Either domain " << domainIndex << " does not exist, "
fp@950:                 << "or it contains less than " << (unsigned int) fmmuIndex + 1
fp@950:                 << " FMMus!";
fp@950:         else
fp@950:             err << strerror(errno);
fp@950:         throw MasterException(err.str());
fp@950:     }
fp@950: }
fp@950: 
fp@950: /****************************************************************************/
fp@950: 
fp@935: void Master::getSync(
fp@935:         ec_ioctl_sync_t *sync,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex
fp@935:         )
fp@935: {
fp@935:     sync->slave_position = slaveIndex;
fp@935:     sync->sync_index = syncIndex;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_SYNC, sync)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get sync manager: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@950:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers!";
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getPdo(
fp@935:         ec_ioctl_pdo_t *pdo,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex,
fp@935:         uint8_t pdoPos
fp@935:         )
fp@935: {
fp@935:     pdo->slave_position = slaveIndex;
fp@935:     pdo->sync_index = syncIndex;
fp@935:     pdo->pdo_pos = pdoPos;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_PDO, pdo)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get Pdo: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@950:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers, or sync manager "
fp@935:                 << (unsigned int) syncIndex << " contains less than "
fp@935:                 << pdoPos + 1 << " Pdos!" << endl;
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getPdoEntry(
fp@935:         ec_ioctl_pdo_entry_t *entry,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex,
fp@935:         uint8_t pdoPos,
fp@935:         uint8_t entryPos
fp@935:         )
fp@935: {
fp@935:     entry->slave_position = slaveIndex;
fp@935:     entry->sync_index = syncIndex;
fp@935:     entry->pdo_pos = pdoPos;
fp@935:     entry->entry_pos = entryPos;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_PDO_ENTRY, entry)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get Pdo entry: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@950:                 << "or it contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers, or sync manager "
fp@935:                 << (unsigned int) syncIndex << " contains less than "
fp@935:                 << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
fp@935:                 << " contains less than " << (unsigned int) entryPos + 1
fp@935:                 << " entries!" << endl;
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@965: void Master::getSdo(
fp@965:         ec_ioctl_sdo_t *sdo,
fp@965:         uint16_t slaveIndex,
fp@965:         uint16_t sdoPosition
fp@965:         )
fp@965: {
fp@965:     sdo->slave_position = slaveIndex;
fp@965:     sdo->sdo_position = sdoPosition;
fp@965: 
fp@965:     if (ioctl(fd, EC_IOCTL_SDO, sdo)) {
fp@965:         stringstream err;
fp@965:         err << "Failed to get Sdo: ";
fp@965:         if (errno == EINVAL)
fp@965:             err << "Either slave " << slaveIndex << " does not exist, "
fp@965:                 << "or it contains less than " << sdoPosition + 1 << " Sdos!"
fp@965:                 << endl;
fp@965:         else
fp@965:             err << strerror(errno);
fp@965:         throw MasterException(err.str());
fp@965:     }
fp@965: }
fp@965: 
fp@965: /****************************************************************************/
fp@965: 
fp@965: void Master::getSdoEntry(
fp@965:         ec_ioctl_sdo_entry_t *entry,
fp@965:         uint16_t slaveIndex,
fp@968:         int sdoSpec,
fp@965:         uint8_t entrySubindex
fp@965:         )
fp@965: {
fp@965:     entry->slave_position = slaveIndex;
fp@968:     entry->sdo_spec = sdoSpec;
fp@965:     entry->sdo_entry_subindex = entrySubindex;
fp@965: 
fp@965:     if (ioctl(fd, EC_IOCTL_SDO_ENTRY, entry)) {
fp@965:         stringstream err;
fp@965:         err << "Failed to get Sdo entry: ";
fp@968:         err << strerror(errno);
fp@965:         throw MasterException(err.str());
fp@965:     }
fp@965: }
fp@965: 
fp@965: /****************************************************************************/
fp@965: 
fp@960: void Master::requestState(
fp@960:         uint16_t slavePosition,
fp@960:         uint8_t state
fp@960:         )
fp@960: {
fp@960:     ec_ioctl_slave_state_t data;
fp@960: 
fp@960:     data.slave_position = slavePosition;
fp@960:     data.requested_state = state;
fp@960:     
fp@960:     if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
fp@960:         stringstream err;
fp@960:         err << "Failed to request slave state: ";
fp@960:         if (errno == EINVAL)
fp@960:             err << "Slave " << slavePosition << " does not exist!";
fp@960:         else
fp@960:             err << strerror(errno);
fp@960:         throw MasterException(err.str());
fp@960:     }
fp@960: }
fp@960: 
fp@960: /****************************************************************************/
fp@960: 
fp@935: string Master::slaveState(uint8_t state)
fp@922: {
fp@922:     switch (state) {
fp@922:         case 1: return "INIT";
fp@922:         case 2: return "PREOP";
fp@922:         case 4: return "SAFEOP";
fp@922:         case 8: return "OP";
fp@922:         default: return "???";
fp@922:     }
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@968: 
fp@968: void Master::printRawData(
fp@968:         const uint8_t *data,
fp@968:         unsigned int size)
fp@968: {
fp@968:     cout << hex << setfill('0');
fp@968:     while (size--) {
fp@968:         cout << "0x" << setw(2) << (unsigned int) *data++;
fp@968:         if (size)
fp@968:             cout << " ";
fp@968:     }
fp@968:     cout << endl;
fp@968: }
fp@968: 
fp@980: /*****************************************************************************/
fp@980: 
fp@980: /**
fp@980:  * Calculates the SII checksum field.
fp@980:  *
fp@980:  * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an
fp@980:  * initial value of 0xff (see IEC 61158-6-12 ch. 5.4).
fp@980:  *
fp@980:  * The below code was originally generated with PYCRC
fp@980:  * http://www.tty1.net/pycrc
fp@980:  *
fp@980:  * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff
fp@980:  *   --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit
fp@980:  *
fp@980:  * \return CRC8
fp@980:  */
fp@980: uint8_t Master::calcSiiCrc(
fp@980:         const uint8_t *data, /**< pointer to data */
fp@980:         size_t length /**< number of bytes in \a data */
fp@980:         )
fp@980: {
fp@980:     unsigned int i;
fp@980:     uint8_t bit, byte, crc = 0x48;
fp@980: 
fp@980:     while (length--) {
fp@980:         byte = *data++;
fp@980:         for (i = 0; i < 8; i++) {
fp@980:             bit = crc & 0x80;
fp@980:             crc = (crc << 1) | ((byte >> (7 - i)) & 0x01);
fp@980:             if (bit) crc ^= 0x07;
fp@980:         }
fp@980:     }
fp@980: 
fp@980:     for (i = 0; i < 8; i++) {
fp@980:         bit = crc & 0x80;
fp@980:         crc <<= 1;
fp@980:         if (bit) crc ^= 0x07;
fp@980:     }
fp@980: 
fp@980:     return crc;
fp@980: }
fp@980: 
fp@980: /*****************************************************************************/