1 /*****************************************************************************

     2  *

     3  * $Id$

     4  *

     5  ****************************************************************************/

     6 

     7 #include <sys/types.h>

     8 #include <sys/stat.h>

     9 #include <fcntl.h>

    10 #include <errno.h>

    11 #include <sys/ioctl.h>

    12 

    13 #include <iostream>

    14 #include <iomanip>

    15 #include <sstream>

    16 #include <fstream>

    17 #include <cctype> // toupper()

    18 #include <list>

    19 using namespace std;

    20 

    21 #include "Master.h"

    22 

    23 #define swap16(x) \

    24         ((uint16_t)( \

    25         (((uint16_t)(x) & 0x00ffU) << 8) | \

    26         (((uint16_t)(x) & 0xff00U) >> 8) ))

    27 #define swap32(x) \

    28         ((uint32_t)( \

    29         (((uint32_t)(x) & 0x000000ffUL) << 24) | \

    30         (((uint32_t)(x) & 0x0000ff00UL) <<  8) | \

    31         (((uint32_t)(x) & 0x00ff0000UL) >>  8) | \

    32         (((uint32_t)(x) & 0xff000000UL) >> 24) ))

    33 

    34 #if __BYTE_ORDER == __LITTLE_ENDIAN

    35 

    36 #define le16tocpu(x) x

    37 #define le32tocpu(x) x

    38 

    39 #define cputole16(x) x

    40 #define cputole32(x) x

    41 

    42 #elif __BYTE_ORDER == __BIG_ENDIAN

    43 

    44 #define le16tocpu(x) swap16(x)

    45 #define le32tocpu(x) swap32(x)

    46 

    47 #define cputole16(x) swap16(x)

    48 #define cputole32(x) swap32(x)

    49 

    50 #endif

    51 

    52 /****************************************************************************/

    53 

    54 struct CoEDataType {

    55     const char *name;

    56     uint16_t coeCode;

    57     unsigned int byteSize;

    58 };

    59 

    60 static const CoEDataType dataTypes[] = {

    61     {"int8",   0x0002, 1},

    62     {"int16",  0x0003, 2},

    63     {"int32",  0x0004, 4},

    64     {"uint8",  0x0005, 1},

    65     {"uint16", 0x0006, 2},

    66     {"uint32", 0x0007, 4},

    67     {"string", 0x0009, 0},

    68     {"raw",    0xffff, 0},

    69     {}

    70 };

    71 

    72 /****************************************************************************/

    73 

    74 const CoEDataType *findDataType(const string &str)

    75 {

    76     const CoEDataType *d;

    77     

    78     for (d = dataTypes; d->name; d++)

    79         if (str == d->name)

    80             return d;

    81 

    82     return NULL;

    83 }

    84 

    85 /****************************************************************************/

    86 

    87 const CoEDataType *findDataType(uint16_t code)

    88 {

    89     const CoEDataType *d;

    90     

    91     for (d = dataTypes; d->name; d++)

    92         if (code == d->coeCode)

    93             return d;

    94 

    95     return NULL;

    96 }

    97 

    98 /****************************************************************************/

    99 

   100 Master::Master()

   101 {

   102     index = 0;

   103     verbosity = Normal;

   104     fd = -1;

   105 }

   106 

   107 /****************************************************************************/

   108 

   109 Master::~Master()

   110 {

   111     close();

   112 }

   113 

   114 /****************************************************************************/

   115 

   116 void Master::setIndex(unsigned int i)

   117 {

   118     index = i;

   119 }

   120 

   121 /****************************************************************************/

   122 

   123 void Master::setVerbosity(Verbosity v)

   124 {

   125     verbosity = v;

   126 }

   127 

   128 /*****************************************************************************/

   129 

   130 /**

   131  * Writes the Secondary slave address (alias) to the slave's SII.

   132  */

   133 void Master::writeAlias(

   134         int slavePosition,

   135         bool force,

   136         const vector<string> &commandArgs

   137         )

   138 {

   139     uint16_t alias;

   140     stringstream err, strAlias;

   141     int number;

   142 

   143     if (commandArgs.size() != 1) {

   144         stringstream err;

   145         err << "'alias' takes exactly one argument!";

   146         throw MasterException(err.str());

   147     }

   148 

   149     strAlias << commandArgs[0];

   150     strAlias

   151         >> resetiosflags(ios::basefield) // guess base from prefix

   152         >> number;

   153     if (strAlias.fail() || number < 0x0000 || number > 0xffff) {

   154         err << "Invalid alias '" << commandArgs[0] << "'!";

   155         throw MasterException(err.str());

   156     }

   157     alias = number;

   158 

   159     if (slavePosition == -1) {

   160         unsigned int numSlaves, i;

   161 

   162         if (!force) {

   163             err << "This will write the alias addresses of all slaves to "

   164                 << alias << "! Please specify --force to proceed.";

   165             throw MasterException(err.str());

   166         }

   167 

   168         open(ReadWrite);

   169         numSlaves = slaveCount();

   170 

   171         for (i = 0; i < numSlaves; i++) {

   172             writeSlaveAlias(i, alias);

   173         }

   174     } else {

   175         open(ReadWrite);

   176         writeSlaveAlias(slavePosition, alias);

   177     }

   178 }

   179 

   180 /*****************************************************************************/

   181 

   182 bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)

   183 {

   184     return a.alias < b.alias

   185         || (a.alias == b.alias && a.position < b.position);

   186 }

   187 

   188 /** Lists the bus configuration.

   189  */

   190 void Master::showConfigs()

   191 {

   192     ec_ioctl_master_t master;

   193     unsigned int i;

   194     ec_ioctl_config_t config;

   195     ConfigList configList;

   196 

   197     open(Read);

   198     getMaster(&master);

   199 

   200     for (i = 0; i < master.config_count; i++) {

   201         getConfig(&config, i);

   202         configList.push_back(config);

   203     }

   204 

   205     configList.sort();

   206 

   207     if (verbosity == Verbose) {

   208         showDetailedConfigs(configList);

   209     } else {

   210         listConfigs(configList);

   211     }

   212 }

   213 

   214 /****************************************************************************/

   215 

   216 void Master::outputData(int domainIndex)

   217 {

   218     open(Read);

   219 

   220     if (domainIndex == -1) {

   221         unsigned int i;

   222         ec_ioctl_master_t master;

   223 

   224         getMaster(&master);

   225 

   226         for (i = 0; i < master.domain_count; i++) {

   227             outputDomainData(i);

   228         }

   229     } else {

   230         outputDomainData(domainIndex);

   231     }

   232 }

   233 

   234 /****************************************************************************/

   235 

   236 void Master::setDebug(const vector<string> &commandArgs)

   237 {

   238     stringstream str;

   239     int debugLevel;

   240     

   241     if (commandArgs.size() != 1) {

   242         stringstream err;

   243         err << "'debug' takes exactly one argument!";

   244         throw MasterException(err.str());

   245     }

   246 

   247     str << commandArgs[0];

   248     str >> resetiosflags(ios::basefield) // guess base from prefix

   249         >> debugLevel;

   250 

   251     if (str.fail()) {

   252         stringstream err;

   253         err << "Invalid debug level '" << commandArgs[0] << "'!";

   254         throw MasterException(err.str());

   255     }

   256 

   257     open(ReadWrite);

   258 

   259     if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {

   260         stringstream err;

   261         err << "Failed to set debug level: " << strerror(errno);

   262         throw MasterException(err.str());

   263     }

   264 }

   265 

   266 /****************************************************************************/

   267 

   268 void Master::showDomains(int domainIndex)

   269 {

   270     open(Read);

   271 

   272     if (domainIndex == -1) {

   273         unsigned int i;

   274         ec_ioctl_master_t master;

   275 

   276         getMaster(&master);

   277 

   278         for (i = 0; i < master.domain_count; i++) {

   279             showDomain(i);

   280         }

   281     } else {

   282         showDomain(domainIndex);

   283     }

   284 }

   285 

   286 /****************************************************************************/

   287 

   288 void Master::showMaster()

   289 {

   290     ec_ioctl_master_t data;

   291     stringstream err;

   292     unsigned int i;

   293     

   294     open(Read);

   295     getMaster(&data);

   296 

   297     cout

   298         << "Master" << index << endl

   299         << "  Phase: ";

   300 

   301     switch (data.phase) {

   302         case 0: cout << "Waiting for device..."; break;

   303         case 1: cout << "Idle"; break;

   304         case 2: cout << "Operation"; break;

   305         default:

   306                 err << "Invalid master phase " << data.phase;

   307                 throw MasterException(err.str());

   308     }

   309 

   310     cout << endl

   311         << "  Slaves: " << data.slave_count << endl;

   312 

   313     for (i = 0; i < 2; i++) {

   314         cout << "  Device" << i << ": ";

   315         if (data.devices[i].address[0] == 0x00

   316                 && data.devices[i].address[1] == 0x00

   317                 && data.devices[i].address[2] == 0x00

   318                 && data.devices[i].address[3] == 0x00

   319                 && data.devices[i].address[4] == 0x00

   320                 && data.devices[i].address[5] == 0x00) {

   321             cout << "None.";

   322         } else {

   323             cout << hex << setfill('0')

   324                 << setw(2) << (unsigned int) data.devices[i].address[0] << ":"

   325                 << setw(2) << (unsigned int) data.devices[i].address[1] << ":"

   326                 << setw(2) << (unsigned int) data.devices[i].address[2] << ":"

   327                 << setw(2) << (unsigned int) data.devices[i].address[3] << ":"

   328                 << setw(2) << (unsigned int) data.devices[i].address[4] << ":"

   329                 << setw(2) << (unsigned int) data.devices[i].address[5] << " ("

   330                 << (data.devices[i].attached ? "attached" : "waiting...")

   331                 << ")" << endl << dec

   332                 << "    Tx count: " << data.devices[i].tx_count << endl

   333                 << "    Rx count: " << data.devices[i].rx_count;

   334         }

   335         cout << endl;

   336     }

   337 }

   338 

   339 /****************************************************************************/

   340 

   341 void Master::listPdos(int slavePosition)

   342 {

   343     open(Read);

   344 

   345     if (slavePosition == -1) {

   346         unsigned int numSlaves = slaveCount(), i;

   347 

   348         for (i = 0; i < numSlaves; i++) {

   349             listSlavePdos(i, true);

   350         }

   351     } else {

   352         listSlavePdos(slavePosition, false);

   353     }

   354 }

   355 

   356 /****************************************************************************/

   357 

   358 void Master::listSdos(int slavePosition)

   359 {

   360     open(Read);

   361 

   362     if (slavePosition == -1) {

   363         unsigned int numSlaves = slaveCount(), i;

   364 

   365         for (i = 0; i < numSlaves; i++) {

   366             listSlaveSdos(i, true);

   367         }

   368     } else {

   369         listSlaveSdos(slavePosition, false);

   370     }

   371 }

   372 

   373 /****************************************************************************/

   374 

   375 void Master::sdoDownload(

   376         int slavePosition,

   377         const string &dataTypeStr,

   378         const vector<string> &commandArgs

   379         )

   380 {

   381     stringstream strIndex, strSubIndex, strValue, err;

   382     ec_ioctl_slave_sdo_download_t data;

   383     unsigned int number;

   384     const CoEDataType *dataType = NULL;

   385 

   386     if (slavePosition < 0) {

   387         err << "'sdo_download' requires a slave! Please specify --slave.";

   388         throw MasterException(err.str());

   389     }

   390     data.slave_position = slavePosition;

   391 

   392     if (commandArgs.size() != 3) {

   393         err << "'sdo_download' takes 3 arguments!";

   394         throw MasterException(err.str());

   395     }

   396 

   397     strIndex << commandArgs[0];

   398     strIndex

   399         >> resetiosflags(ios::basefield) // guess base from prefix

   400         >> data.sdo_index;

   401     if (strIndex.fail()) {

   402         err << "Invalid Sdo index '" << commandArgs[0] << "'!";

   403         throw MasterException(err.str());

   404     }

   405 

   406     strSubIndex << commandArgs[1];

   407     strSubIndex

   408         >> resetiosflags(ios::basefield) // guess base from prefix

   409         >> number;

   410     if (strSubIndex.fail() || number > 0xff) {

   411         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";

   412         throw MasterException(err.str());

   413     }

   414     data.sdo_entry_subindex = number;

   415 

   416     if (dataTypeStr != "") { // data type specified

   417         if (!(dataType = findDataType(dataTypeStr))) {

   418             err << "Invalid data type '" << dataTypeStr << "'!";

   419             throw MasterException(err.str());

   420         }

   421     } else { // no data type specified: fetch from dictionary

   422         ec_ioctl_slave_sdo_entry_t entry;

   423 

   424         open(ReadWrite);

   425 

   426         try {

   427             getSdoEntry(&entry, slavePosition,

   428                     data.sdo_index, data.sdo_entry_subindex);

   429         } catch (MasterException &e) {

   430             err << "Failed to determine Sdo entry data type. "

   431                 << "Please specify --type.";

   432             throw MasterException(err.str());

   433         }

   434         if (!(dataType = findDataType(entry.data_type))) {

   435             err << "Pdo entry has unknown data type 0x"

   436                 << hex << setfill('0') << setw(4) << entry.data_type << "!"

   437                 << " Please specify --type.";

   438             throw MasterException(err.str());

   439         }

   440     }

   441 

   442     if (dataType->byteSize) {

   443         data.data_size = dataType->byteSize;

   444     } else {

   445         data.data_size = DefaultBufferSize;

   446     }

   447 

   448     data.data = new uint8_t[data.data_size + 1];

   449 

   450     strValue << commandArgs[2];

   451     strValue >> resetiosflags(ios::basefield); // guess base from prefix

   452     strValue.exceptions(ios::failbit);

   453 

   454     try {

   455         switch (dataType->coeCode) {

   456             case 0x0002: // int8

   457                 {

   458                     int16_t val; // uint8_t is interpreted as char

   459                     strValue >> val;

   460                     if (val > 127 || val < -128)

   461                         throw ios::failure("Value out of range");

   462                     *data.data = val;

   463                     break;

   464                 }

   465             case 0x0003: // int16

   466                 {

   467                     int16_t val;

   468                     strValue >> val;

   469                     *(int16_t *) data.data = cputole16(val);

   470                     break;

   471                 }

   472             case 0x0004: // int32

   473                 {

   474                     int32_t val;

   475                     strValue >> val;

   476                     *(int32_t *) data.data = cputole32(val);

   477                     break;

   478                 }

   479             case 0x0005: // uint8

   480                 {

   481                     uint16_t val; // uint8_t is interpreted as char

   482                     strValue >> val;

   483                     if (val > 0xff)

   484                         throw ios::failure("Value out of range");

   485                     *data.data = val;

   486                     break;

   487                 }

   488             case 0x0006: // uint16

   489                 {

   490                     uint16_t val;

   491                     strValue >> val;

   492                     *(uint16_t *) data.data = cputole16(val);

   493                     break;

   494                 }

   495             case 0x0007: // uint32

   496                 {

   497                     uint32_t val;

   498                     strValue >> val;

   499                     *(uint32_t *) data.data = cputole32(val);

   500                     break;

   501                 }

   502             case 0x0009: // string

   503                 if (strValue.str().size() >= data.data_size) {

   504                     err << "String too large";

   505                     throw MasterException(err.str());

   506                 }

   507                 data.data_size = strValue.str().size();

   508                 strValue >> (char *) data.data;

   509                 break;

   510 

   511             default:

   512                 delete [] data.data;

   513                 err << "Unknown data type 0x" << hex << dataType->coeCode;

   514                 throw MasterException(err.str());

   515         }

   516     } catch (ios::failure &e) {

   517         delete [] data.data;

   518         err << "Invalid value argument '" << commandArgs[2]

   519             << "' for type '" << dataType->name << "'!";

   520         throw MasterException(err.str());

   521     }

   522 

   523     open(ReadWrite);

   524 

   525     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &data) < 0) {

   526         stringstream err;

   527         err << "Failed to download Sdo: ";

   528         if (errno == EIO && data.abort_code) {

   529             err << "Abort code 0x" << hex << setfill('0')

   530                 << setw(8) << data.abort_code;

   531         } else {

   532             err << strerror(errno);

   533         }

   534         delete [] data.data;

   535         throw MasterException(err.str());

   536     }

   537 

   538     delete [] data.data;

   539 }

   540 

   541 /****************************************************************************/

   542 

   543 void Master::sdoUpload(

   544         int slavePosition,

   545         const string &dataTypeStr,

   546         const vector<string> &commandArgs

   547         )

   548 {

   549     stringstream strIndex, strSubIndex;

   550     int sval;

   551     ec_ioctl_slave_sdo_upload_t data;

   552     unsigned int uval;

   553     const CoEDataType *dataType = NULL;

   554 

   555     if (slavePosition < 0) {

   556         stringstream err;

   557         err << "'sdo_upload' requires a slave! Please specify --slave.";

   558         throw MasterException(err.str());

   559     }

   560     data.slave_position = slavePosition;

   561 

   562     if (commandArgs.size() != 2) {

   563         stringstream err;

   564         err << "'sdo_upload' takes two arguments!";

   565         throw MasterException(err.str());

   566     }

   567 

   568     strIndex << commandArgs[0];

   569     strIndex

   570         >> resetiosflags(ios::basefield) // guess base from prefix

   571         >> data.sdo_index;

   572     if (strIndex.fail()) {

   573         stringstream err;

   574         err << "Invalid Sdo index '" << commandArgs[0] << "'!";

   575         throw MasterException(err.str());

   576     }

   577 

   578     strSubIndex << commandArgs[1];

   579     strSubIndex

   580         >> resetiosflags(ios::basefield) // guess base from prefix

   581         >> uval;

   582     if (strSubIndex.fail() || uval > 0xff) {

   583         stringstream err;

   584         err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";

   585         throw MasterException(err.str());

   586     }

   587     data.sdo_entry_subindex = uval;

   588 

   589     if (dataTypeStr != "") { // data type specified

   590         if (!(dataType = findDataType(dataTypeStr))) {

   591             stringstream err;

   592             err << "Invalid data type '" << dataTypeStr << "'!";

   593             throw MasterException(err.str());

   594         }

   595     } else { // no data type specified: fetch from dictionary

   596         ec_ioctl_slave_sdo_entry_t entry;

   597 

   598         open(Read);

   599 

   600         try {

   601             getSdoEntry(&entry, slavePosition,

   602                     data.sdo_index, data.sdo_entry_subindex);

   603         } catch (MasterException &e) {

   604             stringstream err;

   605             err << "Failed to determine Sdo entry data type. "

   606                 << "Please specify --type.";

   607             throw MasterException(err.str());

   608         }

   609         if (!(dataType = findDataType(entry.data_type))) {

   610             stringstream err;

   611             err << "Pdo entry has unknown data type 0x"

   612                 << hex << setfill('0') << setw(4) << entry.data_type << "!"

   613                 << " Please specify --type.";

   614             throw MasterException(err.str());

   615         }

   616     }

   617 

   618     if (dataType->byteSize) {

   619         data.target_size = dataType->byteSize;

   620     } else {

   621         data.target_size = DefaultBufferSize;

   622     }

   623 

   624     data.target = new uint8_t[data.target_size + 1];

   625 

   626     open(Read);

   627 

   628     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &data) < 0) {

   629         stringstream err;

   630         err << "Failed to upload Sdo: ";

   631         if (errno == EIO && data.abort_code) {

   632             err << "Abort code 0x" << hex << setfill('0')

   633                 << setw(8) << data.abort_code;

   634         } else {

   635             err << strerror(errno);

   636         }

   637         delete [] data.target;

   638         close();

   639         throw MasterException(err.str());

   640     }

   641 

   642     close();

   643 

   644     if (dataType->byteSize && data.data_size != dataType->byteSize) {

   645         stringstream err;

   646         err << "Data type mismatch. Expected " << dataType->name

   647             << " with " << dataType->byteSize << " byte, but got "

   648             << data.data_size << " byte.";

   649         throw MasterException(err.str());

   650     }

   651 

   652     cout << setfill('0');

   653     switch (dataType->coeCode) {

   654         case 0x0002: // int8

   655             sval = *(int8_t *) data.target;

   656             cout << sval << " 0x" << hex << setw(2) << sval << endl;

   657             break;

   658         case 0x0003: // int16

   659             sval = le16tocpu(*(int16_t *) data.target);

   660             cout << sval << " 0x" << hex << setw(4) << sval << endl;

   661             break;

   662         case 0x0004: // int32

   663             sval = le32tocpu(*(int32_t *) data.target);

   664             cout << sval << " 0x" << hex << setw(8) << sval << endl;

   665             break;

   666         case 0x0005: // uint8

   667             uval = (unsigned int) *(uint8_t *) data.target;

   668             cout << uval << " 0x" << hex << setw(2) << uval << endl;

   669             break;

   670         case 0x0006: // uint16

   671             uval = le16tocpu(*(uint16_t *) data.target);

   672             cout << uval << " 0x" << hex << setw(4) << uval << endl;

   673             break;

   674         case 0x0007: // uint32

   675             uval = le32tocpu(*(uint32_t *) data.target);

   676             cout << uval << " 0x" << hex << setw(8) << uval << endl;

   677             break;

   678         case 0x0009: // string

   679             cout << string((const char *) data.target, data.data_size)

   680                 << endl;

   681             break;

   682         default:

   683             printRawData(data.target, data.data_size);

   684             break;

   685     }

   686 

   687     delete [] data.target;

   688 }

   689 

   690 /****************************************************************************/

   691 

   692 void Master::showSlaves(int slavePosition)

   693 {

   694     open(Read);

   695 

   696     if (verbosity == Verbose) {

   697         if (slavePosition == -1) {

   698             unsigned int numSlaves = slaveCount(), i;

   699 

   700             for (i = 0; i < numSlaves; i++) {

   701                 showSlave(i);

   702             }

   703         } else {

   704             showSlave(slavePosition);

   705         }

   706     } else {

   707         listSlaves(slavePosition);

   708     }

   709 }

   710 

   711 /****************************************************************************/

   712 

   713 struct CategoryName {

   714     uint16_t type;

   715     const char *name;

   716 };

   717 

   718 static const CategoryName categoryNames[] = {

   719     {0x000a, "STRINGS"},

   720     {0x0014, "DataTypes"},

   721     {0x001e, "General"},

   722     {0x0028, "FMMU"},

   723     {0x0029, "SyncM"},

   724     {0x0032, "TXPDO"},

   725     {0x0033, "RXPDO"},

   726     {0x003c, "DC"},

   727     {}

   728 };

   729 

   730 const char *getCategoryName(uint16_t type)

   731 {

   732     const CategoryName *cn = categoryNames;

   733 

   734     while (cn->type) {

   735         if (cn->type == type) {

   736             return cn->name;

   737         }

   738         cn++;

   739     }

   740 

   741     return "unknown";

   742 }

   743 

   744 void Master::siiRead(int slavePosition)

   745 {

   746     ec_ioctl_slave_sii_t data;

   747     ec_ioctl_slave_t slave;

   748     unsigned int i;

   749     const uint16_t *categoryHeader;

   750     uint16_t categoryType, categorySize;

   751     stringstream err;

   752 

   753     if (slavePosition < 0) {

   754         stringstream err;

   755         err << "'sii_read' requires a slave! Please specify --slave.";

   756         throw MasterException(err.str());

   757     }

   758     data.slave_position = slavePosition;

   759 

   760     open(Read);

   761 

   762     getSlave(&slave, slavePosition);

   763 

   764     if (!slave.sii_nwords)

   765         return;

   766 

   767     data.offset = 0;

   768     data.nwords = slave.sii_nwords;

   769     data.words = new uint16_t[data.nwords];

   770 

   771     if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {

   772         stringstream err;

   773         delete [] data.words;

   774         err << "Failed to read SII: " << strerror(errno);

   775         throw MasterException(err.str());

   776     }

   777 

   778     if (verbosity == Verbose) {

   779         cout << "SII Area:" << hex << setfill('0');

   780         for (i = 0; i < min(data.nwords, 0x0040U) * 2; i++) {

   781             if (i % BreakAfterBytes) {

   782                 cout << " ";

   783             } else {

   784                 cout << endl << "  ";

   785             }

   786             cout << setw(2) << (unsigned int) *((uint8_t *) data.words + i);

   787         }

   788         cout << endl;

   789 

   790         if (data.nwords > 0x0040U) {

   791             // cycle through categories

   792             categoryHeader = data.words + 0x0040U;

   793             categoryType = le16tocpu(*categoryHeader);

   794             while (categoryType != 0xffff) {

   795                 cout << "SII Category 0x" << hex

   796                     << setw(4) << categoryType

   797                     << " (" << getCategoryName(categoryType) << ")" << flush;

   798 

   799                 if (categoryHeader + 1 > data.words + data.nwords) {

   800                     err << "SII data seem to be corrupted!";

   801                     throw MasterException(err.str());

   802                 }

   803                 categorySize = le16tocpu(*(categoryHeader + 1));

   804                 cout << ", " << dec << categorySize << " words" << flush;

   805 

   806                 if (categoryHeader + 2 + categorySize

   807                         > data.words + data.nwords) {

   808                     err << "SII data seem to be corrupted!";

   809                     throw MasterException(err.str());

   810                 }

   811 

   812                 cout << hex;

   813                 for (i = 0; i < categorySize * 2U; i++) {

   814                     if (i % BreakAfterBytes) {

   815                         cout << " ";

   816                     } else {

   817                         cout << endl << "  ";

   818                     }

   819                     cout << setw(2) << (unsigned int)

   820                         *((uint8_t *) (categoryHeader + 2) + i);

   821                 }

   822                 cout << endl;

   823 

   824                 if (categoryHeader + 2 + categorySize + 1

   825                         > data.words + data.nwords) {

   826                     err << "SII data seem to be corrupted!"; 

   827                     throw MasterException(err.str());

   828                 }

   829                 categoryHeader += 2 + categorySize;

   830                 categoryType = le16tocpu(*categoryHeader);

   831             }

   832         }

   833     } else {

   834         for (i = 0; i < data.nwords; i++) {

   835             uint16_t *w = data.words + i;

   836             cout << *(uint8_t *) w << *((uint8_t *) w + 1);

   837         }

   838     }

   839 

   840     delete [] data.words;

   841 }

   842 

   843 /****************************************************************************/

   844 

   845 void Master::siiWrite(

   846         int slavePosition,

   847         bool force,

   848         const vector<string> &commandArgs

   849         )

   850 {

   851     stringstream err;

   852     ec_ioctl_slave_sii_t data;

   853     ifstream file;

   854     unsigned int byte_size;

   855     const uint16_t *categoryHeader;

   856     uint16_t categoryType, categorySize;

   857     uint8_t crc;

   858 

   859     if (slavePosition < 0) {

   860         err << "'sii_write' requires a slave! Please specify --slave.";

   861         throw MasterException(err.str());

   862     }

   863     data.slave_position = slavePosition;

   864 

   865     if (commandArgs.size() != 1) {

   866         err << "'ssi_write' takes exactly one argument!";

   867         throw MasterException(err.str());

   868     }

   869 

   870     file.open(commandArgs[0].c_str(), ifstream::in | ifstream::binary);

   871     if (file.fail()) {

   872         err << "Failed to open '" << commandArgs[0] << "'!";

   873         throw MasterException(err.str());

   874     }

   875 

   876     // get length of file

   877     file.seekg(0, ios::end);

   878     byte_size = file.tellg();

   879     file.seekg(0, ios::beg);

   880 

   881     if (!byte_size || byte_size % 2) {

   882         stringstream err;

   883         err << "Invalid file size! Must be non-zero and even.";

   884         throw MasterException(err.str());

   885     }

   886 

   887     data.nwords = byte_size / 2;

   888     if (data.nwords < 0x0041 && !force) {

   889         err << "SII data too short (" << data.nwords << " words)! Mimimum is"

   890                 " 40 fixed words + 1 delimiter. Use --force to write anyway.";

   891         throw MasterException(err.str());

   892     }

   893 

   894     // allocate buffer and read file into buffer

   895     data.words = new uint16_t[data.nwords];

   896     file.read((char *) data.words, byte_size);

   897     file.close();

   898 

   899     if (!force) {

   900         // calculate checksum over words 0 to 6

   901         crc = calcSiiCrc((const uint8_t *) data.words, 14);

   902         if (crc != ((const uint8_t *) data.words)[14]) {

   903             err << "CRC incorrect. Must be 0x"

   904                 << hex << setfill('0') << setw(2) << (unsigned int) crc

   905                 << ". Use --force to write anyway.";

   906             throw MasterException(err.str());

   907         }

   908 

   909         // cycle through categories to detect corruption

   910         categoryHeader = data.words + 0x0040U;

   911         categoryType = le16tocpu(*categoryHeader);

   912         while (categoryType != 0xffff) {

   913             if (categoryHeader + 1 > data.words + data.nwords) {

   914                 err << "SII data seem to be corrupted! "

   915                     << "Use --force to write anyway.";

   916                 throw MasterException(err.str());

   917             }

   918             categorySize = le16tocpu(*(categoryHeader + 1));

   919             if (categoryHeader + 2 + categorySize + 1

   920                     > data.words + data.nwords) {

   921                 err << "SII data seem to be corrupted! "

   922                     "Use --force to write anyway.";

   923                 throw MasterException(err.str());

   924             }

   925             categoryHeader += 2 + categorySize;

   926             categoryType = le16tocpu(*categoryHeader);

   927         }

   928     }

   929 

   930     // send data to master

   931     open(ReadWrite);

   932     data.offset = 0;

   933     if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {

   934         stringstream err;

   935         err << "Failed to write SII: " << strerror(errno);

   936         throw MasterException(err.str());

   937     }

   938 }

   939 

   940 /****************************************************************************/

   941 

   942 void Master::requestStates(

   943         int slavePosition,

   944         const vector<string> &commandArgs

   945         )

   946 {

   947     string stateStr;

   948     uint8_t state;

   949     

   950     if (commandArgs.size() != 1) {

   951         stringstream err;

   952         err << "'state' takes exactly one argument!";

   953         throw MasterException(err.str());

   954     }

   955 

   956     stateStr = commandArgs[0];

   957     transform(stateStr.begin(), stateStr.end(),

   958             stateStr.begin(), (int (*) (int)) std::toupper);

   959 

   960     if (stateStr == "INIT") {

   961         state = 0x01;

   962     } else if (stateStr == "PREOP") {

   963         state = 0x02;

   964     } else if (stateStr == "SAFEOP") {

   965         state = 0x04;

   966     } else if (stateStr == "OP") {

   967         state = 0x08;

   968     } else {

   969         stringstream err;

   970         err << "Invalid state '" << commandArgs[0] << "'!";

   971         throw MasterException(err.str());

   972     }

   973 

   974     open(ReadWrite);

   975 

   976     if (slavePosition == -1) {

   977         unsigned int i, numSlaves = slaveCount();

   978         for (i = 0; i < numSlaves; i++)

   979             requestState(i, state);

   980     } else {

   981         requestState(slavePosition, state);

   982     }

   983 }

   984 

   985 /****************************************************************************/

   986 

   987 void Master::generateXml(int slavePosition)

   988 {

   989     open(Read);

   990 

   991     if (slavePosition == -1) {

   992         unsigned int numSlaves = slaveCount(), i;

   993 

   994         for (i = 0; i < numSlaves; i++) {

   995             generateSlaveXml(i);

   996         }

   997     } else {

   998         generateSlaveXml(slavePosition);

   999     }

  1000 }

  1001 

  1002 /****************************************************************************/

  1003 

  1004 void Master::open(Permissions perm)

  1005 {

  1006     stringstream deviceName;

  1007 

  1008     if (fd != -1) { // already open

  1009         return;

  1010     }

  1011     

  1012     deviceName << "/dev/EtherCAT" << index;

  1013     

  1014     if ((fd = ::open(deviceName.str().c_str(),

  1015                     perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {

  1016         stringstream err;

  1017         err << "Failed to open master device " << deviceName.str() << ": "

  1018             << strerror(errno);

  1019         throw MasterException(err.str());

  1020     }

  1021 }

  1022 

  1023 /****************************************************************************/

  1024 

  1025 void Master::close()

  1026 {

  1027     if (fd == -1)

  1028         return;

  1029 

  1030     ::close(fd);

  1031     fd = -1;

  1032 }

  1033 

  1034 /*****************************************************************************/

  1035 

  1036 /**

  1037  * Writes the Secondary slave address (alias) to the slave's SII.

  1038  */

  1039 void Master::writeSlaveAlias(

  1040         uint16_t slavePosition,

  1041         uint16_t alias

  1042         )

  1043 {

  1044     ec_ioctl_slave_sii_t data;

  1045     ec_ioctl_slave_t slave;

  1046     stringstream err;

  1047     uint8_t crc;

  1048 

  1049     open(ReadWrite);

  1050 

  1051     getSlave(&slave, slavePosition);

  1052 

  1053     if (slave.sii_nwords < 8) {

  1054         err << "Current SII contents are too small to set an alias "

  1055             << "(" << slave.sii_nwords << " words)!";

  1056         throw MasterException(err.str());

  1057     }

  1058 

  1059     data.slave_position = slavePosition;

  1060     data.offset = 0;

  1061     data.nwords = 8;

  1062     data.words = new uint16_t[data.nwords];

  1063 

  1064     // read first 8 SII words

  1065     if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, &data) < 0) {

  1066         delete [] data.words;

  1067         err << "Failed to read SII: " << strerror(errno);

  1068         throw MasterException(err.str());

  1069     }

  1070 

  1071     // write new alias address in word 4

  1072     data.words[4] = cputole16(alias);

  1073 

  1074     // calculate checksum over words 0 to 6

  1075     crc = calcSiiCrc((const uint8_t *) data.words, 14);

  1076 

  1077     // write new checksum into first byte of word 7

  1078     *(uint8_t *) (data.words + 7) = crc;

  1079 

  1080     // write first 8 words with new alias and checksum

  1081     if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, &data) < 0) {

  1082         delete [] data.words;

  1083         err << "Failed to write SII: " << strerror(errno);

  1084         throw MasterException(err.str());

  1085     }

  1086 

  1087     delete [] data.words;

  1088 }

  1089 

  1090 /*****************************************************************************/

  1091 

  1092 /** Lists the complete bus configuration.

  1093  */

  1094 void Master::showDetailedConfigs(const ConfigList &configList)

  1095 {

  1096     ConfigList::const_iterator configIter;

  1097     unsigned int j, k, l;

  1098     ec_ioctl_config_pdo_t pdo;

  1099     ec_ioctl_config_pdo_entry_t entry;

  1100     ec_ioctl_config_sdo_t sdo;

  1101 

  1102     for (configIter = configList.begin();

  1103             configIter != configList.end();

  1104             configIter++) {

  1105 

  1106         cout << "Alias: "

  1107             << dec << configIter->alias << endl

  1108             << "Position: " << configIter->position << endl

  1109             << "Vendor Id: 0x"

  1110             << hex << setfill('0')

  1111             << setw(8) << configIter->vendor_id << endl

  1112             << "Product code: 0x"

  1113             << setw(8) << configIter->product_code << endl

  1114             << "Attached: " << (configIter->attached ? "yes" : "no") << endl

  1115             << "Operational: " << (configIter->operational ? "yes" : "no") << endl;

  1116 

  1117         for (j = 0; j < EC_MAX_SYNC_MANAGERS; j++) {

  1118             if (configIter->syncs[j].pdo_count) {

  1119                 cout << "SM" << dec << j << " ("

  1120                     << (configIter->syncs[j].dir == EC_DIR_INPUT

  1121                             ? "Input" : "Output") << ")" << endl;

  1122                 for (k = 0; k < configIter->syncs[j].pdo_count; k++) {

  1123                     getConfigPdo(&pdo, configIter->config_index, j, k);

  1124 

  1125                     cout << "  Pdo 0x" << hex

  1126                         << setw(4) << pdo.index

  1127                         << " \"" << pdo.name << "\"" << endl;

  1128 

  1129                     for (l = 0; l < pdo.entry_count; l++) {

  1130                         getConfigPdoEntry(&entry, configIter->config_index, j, k, l);

  1131 

  1132                         cout << "    Pdo entry 0x" << hex

  1133                             << setw(4) << entry.index << ":"

  1134                             << setw(2) << (unsigned int) entry.subindex

  1135                             << ", " << dec << (unsigned int) entry.bit_length

  1136                             << " bit, \"" << entry.name << "\"" << endl;