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

     2  *

     3  * $Id$

     4  *

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

     6 

     7 #include <sys/stat.h>

     8 #include <fcntl.h>

     9 #include <errno.h>

    10 #include <sys/ioctl.h>

    11 

    12 #include <sstream>

    13 using namespace std;

    14 

    15 #if 0

    16 #include <iostream>

    17 #include <fstream>

    18 #include <cctype> // toupper()

    19 #include <list>

    20 #endif

    21 

    22 #include "MasterDevice.h"

    23 

    24 /****************************************************************************/

    25 

    26 struct CoEDataType {

    27     const char *name;

    28     uint16_t coeCode;

    29     unsigned int byteSize;

    30 };

    31 

    32 static const CoEDataType dataTypes[] = {

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

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

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

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

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

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

    39     {"string", 0x0009, 0},

    40     {"raw",    0xffff, 0},

    41     {}

    42 };

    43 

    44 /****************************************************************************/

    45 

    46 const CoEDataType *findDataType(const string &str)

    47 {

    48     const CoEDataType *d;

    49     

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

    51         if (str == d->name)

    52             return d;

    53 

    54     return NULL;

    55 }

    56 

    57 /****************************************************************************/

    58 

    59 const CoEDataType *findDataType(uint16_t code)

    60 {

    61     const CoEDataType *d;

    62     

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

    64         if (code == d->coeCode)

    65             return d;

    66 

    67     return NULL;

    68 }

    69 

    70 /****************************************************************************/

    71 

    72 MasterDevice::MasterDevice()

    73 {

    74     index = 0;

    75     fd = -1;

    76 }

    77 

    78 /****************************************************************************/

    79 

    80 MasterDevice::~MasterDevice()

    81 {

    82     close();

    83 }

    84 

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

    86 

    87 void MasterDevice::setIndex(unsigned int i)

    88 {

    89     index = i;

    90 }

    91 

    92 #if 0

    93 /*****************************************************************************/

    94 

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

    96 {

    97     return a.alias < b.alias

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

    99 }

   100 

   101 /** Lists the bus configuration.

   102  */

   103 void Master::showConfigs()

   104 {

   105     ec_ioctl_master_t master;

   106     unsigned int i;

   107     ec_ioctl_config_t config;

   108     ConfigList configList;

   109 

   110     open(Read);

   111     getMaster(&master);

   112 

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

   114         getConfig(&config, i);

   115         configList.push_back(config);

   116     }

   117 

   118     configList.sort();

   119 

   120     if (verbosity == Verbose) {

   121         showDetailedConfigs(configList);

   122     } else {

   123         listConfigs(configList);

   124     }

   125 }

   126 

   127 /****************************************************************************/

   128 

   129 void Master::outputData(int domainIndex)

   130 {

   131     open(Read);

   132 

   133     if (domainIndex == -1) {

   134         unsigned int i;

   135         ec_ioctl_master_t master;

   136 

   137         getMaster(&master);

   138 

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

   140             outputDomainData(i);

   141         }

   142     } else {

   143         outputDomainData(domainIndex);

   144     }

   145 }

   146 

   147 /****************************************************************************/

   148 

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

   150 {

   151     stringstream str;

   152     int debugLevel;

   153     

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

   155         stringstream err;

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

   157         throw MasterDeviceException(err.str());

   158     }

   159 

   160     str << commandArgs[0];

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

   162         >> debugLevel;

   163 

   164     if (str.fail()) {

   165         stringstream err;

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

   167         throw MasterDeviceException(err.str());

   168     }

   169 

   170     open(ReadWrite);

   171 

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

   173         stringstream err;

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

   175         throw MasterDeviceException(err.str());

   176     }

   177 }

   178 

   179 /****************************************************************************/

   180 

   181 void Master::showDomains(int domainIndex)

   182 {

   183     open(Read);

   184 

   185     if (domainIndex == -1) {

   186         unsigned int i;

   187         ec_ioctl_master_t master;

   188 

   189         getMaster(&master);

   190 

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

   192             showDomain(i);

   193         }

   194     } else {

   195         showDomain(domainIndex);

   196     }

   197 }

   198 

   199 /****************************************************************************/

   200 

   201 void Master::showMaster()

   202 {

   203     ec_ioctl_master_t data;

   204     stringstream err;

   205     unsigned int i;

   206     

   207     open(Read);

   208     getMaster(&data);

   209 

   210     cout

   211         << "Master" << index << endl

   212         << "  Phase: ";

   213 

   214     switch (data.phase) {

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

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

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

   218         default:

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

   220                 throw MasterDeviceException(err.str());

   221     }

   222 

   223     cout << endl

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

   225 

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

   227         cout << "  Device" << i << ": ";

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

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

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

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

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

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

   234             cout << "None.";

   235         } else {

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

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

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

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

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

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

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

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

   244                 << ")" << endl << dec

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

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

   247         }

   248         cout << endl;

   249     }

   250 }

   251 

   252 /****************************************************************************/

   253 

   254 void Master::listPdos(int slavePosition)

   255 {

   256     open(Read);

   257 

   258     if (slavePosition == -1) {

   259         unsigned int numSlaves = slaveCount(), i;

   260 

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

   262             listSlavePdos(i, true);

   263         }

   264     } else {

   265         listSlavePdos(slavePosition, false);

   266     }

   267 }

   268 

   269 /****************************************************************************/

   270 

   271 void Master::listSdos(int slavePosition)

   272 {

   273     open(Read);

   274 

   275     if (slavePosition == -1) {

   276         unsigned int numSlaves = slaveCount(), i;

   277 

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

   279             listSlaveSdos(i, true);

   280         }

   281     } else {

   282         listSlaveSdos(slavePosition, false);

   283     }

   284 }

   285 

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

   287 

   288 void Master::sdoDownload(

   289         int slavePosition,

   290         const string &dataTypeStr,

   291         const vector<string> &commandArgs

   292         )

   293 {

   294     stringstream strIndex, strSubIndex, strValue, err;

   295     ec_ioctl_slave_sdo_download_t data;

   296     unsigned int number;

   297     const CoEDataType *dataType = NULL;

   298 

   299     if (slavePosition < 0) {

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

   301         throw MasterDeviceException(err.str());

   302     }

   303     data.slave_position = slavePosition;

   304 

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

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

   307         throw MasterDeviceException(err.str());

   308     }

   309 

   310     strIndex << commandArgs[0];

   311     strIndex

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

   313         >> data.sdo_index;

   314     if (strIndex.fail()) {

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

   316         throw MasterDeviceException(err.str());

   317     }

   318 

   319     strSubIndex << commandArgs[1];

   320     strSubIndex

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

   322         >> number;

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

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

   325         throw MasterDeviceException(err.str());

   326     }

   327     data.sdo_entry_subindex = number;

   328 

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

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

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

   332             throw MasterDeviceException(err.str());

   333         }

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

   335         ec_ioctl_slave_sdo_entry_t entry;

   336 

   337         open(ReadWrite);

   338 

   339         try {

   340             getSdoEntry(&entry, slavePosition,

   341                     data.sdo_index, data.sdo_entry_subindex);

   342         } catch (MasterDeviceException &e) {

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

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

   345             throw MasterDeviceException(err.str());

   346         }

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

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

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

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

   351             throw MasterDeviceException(err.str());

   352         }

   353     }

   354 

   355     if (dataType->byteSize) {

   356         data.data_size = dataType->byteSize;

   357     } else {

   358         data.data_size = DefaultBufferSize;

   359     }

   360 

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

   362 

   363     strValue << commandArgs[2];

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

   365     strValue.exceptions(ios::failbit);

   366 

   367     try {

   368         switch (dataType->coeCode) {

   369             case 0x0002: // int8

   370                 {

   371                     int16_t val; // uint8_t is interpreted as char

   372                     strValue >> val;

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

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

   375                     *data.data = val;

   376                     break;

   377                 }

   378             case 0x0003: // int16

   379                 {

   380                     int16_t val;

   381                     strValue >> val;

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

   383                     break;

   384                 }

   385             case 0x0004: // int32

   386                 {

   387                     int32_t val;

   388                     strValue >> val;

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

   390                     break;

   391                 }

   392             case 0x0005: // uint8

   393                 {

   394                     uint16_t val; // uint8_t is interpreted as char

   395                     strValue >> val;

   396                     if (val > 0xff)

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

   398                     *data.data = val;

   399                     break;

   400                 }

   401             case 0x0006: // uint16

   402                 {

   403                     uint16_t val;

   404                     strValue >> val;

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

   406                     break;

   407                 }

   408             case 0x0007: // uint32

   409                 {

   410                     uint32_t val;

   411                     strValue >> val;

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

   413                     break;

   414                 }

   415             case 0x0009: // string

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

   417                     err << "String too large";

   418                     throw MasterDeviceException(err.str());

   419                 }

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

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

   422                 break;

   423 

   424             default:

   425                 delete [] data.data;

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

   427                 throw MasterDeviceException(err.str());

   428         }

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

   430         delete [] data.data;

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

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

   433         throw MasterDeviceException(err.str());

   434     }

   435 

   436     open(ReadWrite);

   437 

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

   439         stringstream err;

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

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

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

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

   444         } else {

   445             err << strerror(errno);

   446         }

   447         delete [] data.data;

   448         throw MasterDeviceException(err.str());

   449     }

   450 

   451     delete [] data.data;

   452 }

   453 

   454 /****************************************************************************/

   455 

   456 void Master::sdoUpload(

   457         int slavePosition,

   458         const string &dataTypeStr,

   459         const vector<string> &commandArgs

   460         )

   461 {

   462     stringstream strIndex, strSubIndex;

   463     int sval;

   464     ec_ioctl_slave_sdo_upload_t data;

   465     unsigned int uval;

   466     const CoEDataType *dataType = NULL;

   467 

   468     if (slavePosition < 0) {

   469         stringstream err;

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

   471         throw MasterDeviceException(err.str());

   472     }

   473     data.slave_position = slavePosition;

   474 

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

   476         stringstream err;

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

   478         throw MasterDeviceException(err.str());

   479     }

   480 

   481     strIndex << commandArgs[0];

   482     strIndex

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

   484         >> data.sdo_index;

   485     if (strIndex.fail()) {

   486         stringstream err;

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

   488         throw MasterDeviceException(err.str());

   489     }

   490 

   491     strSubIndex << commandArgs[1];

   492     strSubIndex

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

   494         >> uval;

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

   496         stringstream err;

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

   498         throw MasterDeviceException(err.str());

   499     }

   500     data.sdo_entry_subindex = uval;

   501 

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

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

   504             stringstream err;

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

   506             throw MasterDeviceException(err.str());

   507         }

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

   509         ec_ioctl_slave_sdo_entry_t entry;

   510 

   511         open(Read);

   512 

   513         try {

   514             getSdoEntry(&entry, slavePosition,

   515                     data.sdo_index, data.sdo_entry_subindex);

   516         } catch (MasterDeviceException &e) {

   517             stringstream err;

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

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

   520             throw MasterDeviceException(err.str());

   521         }

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

   523             stringstream err;

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

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

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

   527             throw MasterDeviceException(err.str());

   528         }

   529     }

   530 

   531     if (dataType->byteSize) {

   532         data.target_size = dataType->byteSize;

   533     } else {

   534         data.target_size = DefaultBufferSize;

   535     }

   536 

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

   538 

   539     open(Read);

   540 

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

   542         stringstream err;

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

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

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

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

   547         } else {

   548             err << strerror(errno);

   549         }

   550         delete [] data.target;

   551         close();

   552         throw MasterDeviceException(err.str());

   553     }

   554 

   555     close();

   556 

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

   558         stringstream err;

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

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

   561             << data.data_size << " byte.";

   562         throw MasterDeviceException(err.str());

   563     }

   564 

   565     cout << setfill('0');

   566     switch (dataType->coeCode) {

   567         case 0x0002: // int8

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

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

   570             break;

   571         case 0x0003: // int16

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

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

   574             break;

   575         case 0x0004: // int32

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

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

   578             break;

   579         case 0x0005: // uint8

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

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

   582             break;

   583         case 0x0006: // uint16

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

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

   586             break;

   587         case 0x0007: // uint32

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

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

   590             break;

   591         case 0x0009: // string

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

   593                 << endl;

   594             break;

   595         default:

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

   597             break;

   598     }

   599 

   600     delete [] data.target;

   601 }

   602 

   603 /****************************************************************************/

   604 

   605 void Master::showSlaves(int slavePosition)

   606 {

   607     open(Read);

   608 

   609     if (verbosity == Verbose) {

   610         if (slavePosition == -1) {

   611             unsigned int numSlaves = slaveCount(), i;

   612 

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

   614                 showSlave(i);

   615             }

   616         } else {

   617             showSlave(slavePosition);

   618         }

   619     } else {

   620         listSlaves(slavePosition);

   621     }

   622 }

   623 

   624 /****************************************************************************/

   625 

   626 struct CategoryName {

   627     uint16_t type;

   628     const char *name;

   629 };

   630 

   631 static const CategoryName categoryNames[] = {

   632     {0x000a, "STRINGS"},

   633     {0x0014, "DataTypes"},

   634     {0x001e, "General"},

   635     {0x0028, "FMMU"},

   636     {0x0029, "SyncM"},

   637     {0x0032, "TXPDO"},

   638     {0x0033, "RXPDO"},

   639     {0x003c, "DC"},

   640     {}

   641 };

   642 

   643 const char *getCategoryName(uint16_t type)

   644 {

   645     const CategoryName *cn = categoryNames;

   646 

   647     while (cn->type) {

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

   649             return cn->name;

   650         }

   651         cn++;

   652     }

   653 

   654     return "unknown";

   655 }

   656 

   657 void Master::siiRead(int slavePosition)

   658 {

   659     ec_ioctl_slave_sii_t data;

   660     ec_ioctl_slave_t slave;

   661     unsigned int i;

   662     const uint16_t *categoryHeader;

   663     uint16_t categoryType, categorySize;

   664     stringstream err;

   665 

   666     if (slavePosition < 0) {

   667         stringstream err;

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

   669         throw MasterDeviceException(err.str());

   670     }

   671     data.slave_position = slavePosition;

   672 

   673     open(Read);

   674 

   675     getSlave(&slave, slavePosition);

   676 

   677     if (!slave.sii_nwords)

   678         return;

   679 

   680     data.offset = 0;

   681     data.nwords = slave.sii_nwords;

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

   683 

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

   685         stringstream err;

   686         delete [] data.words;

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

   688         throw MasterDeviceException(err.str());

   689     }

   690 

   691     if (verbosity == Verbose) {

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

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

   694             if (i % BreakAfterBytes) {

   695                 cout << " ";

   696             } else {

   697                 cout << endl << "  ";

   698             }

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

   700         }

   701         cout << endl;

   702 

   703         if (data.nwords > 0x0040U) {

   704             // cycle through categories

   705             categoryHeader = data.words + 0x0040U;

   706             categoryType = le16tocpu(*categoryHeader);

   707             while (categoryType != 0xffff) {

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

   709                     << setw(4) << categoryType

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

   711 

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

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

   714                     throw MasterDeviceException(err.str());

   715                 }

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

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

   718 

   719                 if (categoryHeader + 2 + categorySize

   720                         > data.words + data.nwords) {

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

   722                     throw MasterDeviceException(err.str());

   723                 }

   724 

   725                 cout << hex;

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

   727                     if (i % BreakAfterBytes) {

   728                         cout << " ";

   729                     } else {

   730                         cout << endl << "  ";

   731                     }

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

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

   734                 }

   735                 cout << endl;

   736 

   737                 if (categoryHeader + 2 + categorySize + 1

   738                         > data.words + data.nwords) {

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

   740                     throw MasterDeviceException(err.str());

   741                 }

   742                 categoryHeader += 2 + categorySize;

   743                 categoryType = le16tocpu(*categoryHeader);

   744             }

   745         }

   746     } else {

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

   748             uint16_t *w = data.words + i;

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

   750         }

   751     }

   752 

   753     delete [] data.words;

   754 }

   755 

   756 /****************************************************************************/

   757 

   758 void Master::siiWrite(

   759         int slavePosition,

   760         bool force,

   761         const vector<string> &commandArgs

   762         )

   763 {

   764     stringstream err;

   765     ec_ioctl_slave_sii_t data;

   766     ifstream file;

   767     unsigned int byte_size;

   768     const uint16_t *categoryHeader;

   769     uint16_t categoryType, categorySize;

   770     uint8_t crc;

   771 

   772     if (slavePosition < 0) {

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

   774         throw MasterDeviceException(err.str());

   775     }

   776     data.slave_position = slavePosition;

   777 

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

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

   780         throw MasterDeviceException(err.str());

   781     }

   782 

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

   784     if (file.fail()) {

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

   786         throw MasterDeviceException(err.str());

   787     }

   788 

   789     // get length of file

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

   791     byte_size = file.tellg();

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

   793 

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

   795         stringstream err;

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

   797         throw MasterDeviceException(err.str());

   798     }

   799 

   800     data.nwords = byte_size / 2;

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

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

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

   804         throw MasterDeviceException(err.str());

   805     }

   806 

   807     // allocate buffer and read file into buffer

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

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

   810     file.close();

   811 

   812     if (!force) {

   813         // calculate checksum over words 0 to 6

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

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

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

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

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

   819             throw MasterDeviceException(err.str());

   820         }

   821 

   822         // cycle through categories to detect corruption

   823         categoryHeader = data.words + 0x0040U;

   824         categoryType = le16tocpu(*categoryHeader);

   825         while (categoryType != 0xffff) {

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

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

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

   829                 throw MasterDeviceException(err.str());

   830             }

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

   832             if (categoryHeader + 2 + categorySize + 1

   833                     > data.words + data.nwords) {

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

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

   836                 throw MasterDeviceException(err.str());

   837             }

   838             categoryHeader += 2 + categorySize;

   839             categoryType = le16tocpu(*categoryHeader);

   840         }

   841     }

   842 

   843     // send data to master

   844     open(ReadWrite);

   845     data.offset = 0;

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

   847         stringstream err;

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

   849         throw MasterDeviceException(err.str());

   850     }

   851 }

   852 

   853 /****************************************************************************/

   854 

   855 void Master::requestStates(

   856         int slavePosition,

   857         const vector<string> &commandArgs

   858         )

   859 {

   860     string stateStr;

   861     uint8_t state;

   862     

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

   864         stringstream err;

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

   866         throw MasterDeviceException(err.str());

   867     }

   868 

   869     stateStr = commandArgs[0];

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

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

   872 

   873     if (stateStr == "INIT") {

   874         state = 0x01;

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

   876         state = 0x02;

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

   878         state = 0x04;

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

   880         state = 0x08;

   881     } else {

   882         stringstream err;

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

   884         throw MasterDeviceException(err.str());

   885     }

   886 

   887     open(ReadWrite);

   888 

   889     if (slavePosition == -1) {

   890         unsigned int i, numSlaves = slaveCount();

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

   892             requestState(i, state);

   893     } else {

   894         requestState(slavePosition, state);

   895     }

   896 }

   897 

   898 /****************************************************************************/

   899 

   900 void Master::generateXml(int slavePosition)

   901 {

   902     open(Read);

   903 

   904     if (slavePosition == -1) {

   905         unsigned int numSlaves = slaveCount(), i;

   906 

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

   908             generateSlaveXml(i);

   909         }

   910     } else {

   911         generateSlaveXml(slavePosition);

   912     }

   913 }

   914 

   915 #endif

   916 /****************************************************************************/

   917 

   918 void MasterDevice::open(Permissions perm)

   919 {

   920     stringstream deviceName;

   921 

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

   923         return;

   924     }

   925     

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

   927     

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

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

   930         stringstream err;

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

   932             << strerror(errno);

   933         throw MasterDeviceException(err.str());

   934     }

   935 }

   936 

   937 /****************************************************************************/

   938 

   939 void MasterDevice::close()

   940 {

   941     if (fd == -1)

   942         return;

   943 

   944     ::close(fd);

   945     fd = -1;

   946 }

   947 

   948 #if 0

   949 

   950 /*****************************************************************************/

   951 

   952 /** Lists the complete bus configuration.

   953  */

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

   955 {

   956     ConfigList::const_iterator configIter;

   957     unsigned int j, k, l;

   958     ec_ioctl_config_pdo_t pdo;

   959     ec_ioctl_config_pdo_entry_t entry;

   960     ec_ioctl_config_sdo_t sdo;

   961 

   962     for (configIter = configList.begin();

   963             configIter != configList.end();

   964             configIter++) {

   965 

   966         cout << "Alias: "

   967             << dec << configIter->alias << endl

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

   969             << "Vendor Id: 0x"

   970             << hex << setfill('0')

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

   972             << "Product code: 0x"

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

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

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

   976 

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

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

   979                 cout << "SM" << dec << j << " ("

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

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

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

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

   984 

   985                     cout << "  Pdo 0x" << hex

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

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

   988 

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

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

   991 

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

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

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

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

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

   997                     }

   998                 }

   999             }

  1000         }

  1001 

  1002         cout << "Sdo configuration:" << endl;

  1003         if (configIter->sdo_count) {

  1004             for (j = 0; j < configIter->sdo_count; j++) {

  1005                 getConfigSdo(&sdo, configIter->config_index, j);

  1006 

  1007                 cout << "  0x"

  1008                     << hex << setfill('0')

  1009                     << setw(4) << sdo.index << ":"

  1010                     << setw(2) << (unsigned int) sdo.subindex

  1011                     << ", " << dec << sdo.size << " byte: " << hex;

  1012 

  1013                 switch (sdo.size) {

  1014                     case 1:

  1015                         cout << "0x" << setw(2)

  1016                             << (unsigned int) *(uint8_t *) &sdo.data;

  1017                         break;

  1018                     case 2:

  1019                         cout << "0x" << setw(4)

  1020                             << le16tocpu(*(uint16_t *) &sdo.data);

  1021                         break;

  1022                     case 4:

  1023                         cout << "0x" << setw(8)

  1024                             << le32tocpu(*(uint32_t *) &sdo.data);

  1025                         break;

  1026                     default:

  1027                         cout << "???";

  1028                 }

  1029 

  1030                 cout << endl;

  1031             }

  1032         } else {

  1033             cout << "  None." << endl;

  1034         }

  1035 

  1036         cout << endl;

  1037     }

  1038 }

  1039 

  1040 /*****************************************************************************/

  1041 

  1042 struct ConfigInfo {

  1043     string alias;

  1044     string pos;

  1045     string ident;

  1046     string att;

  1047     string op;

  1048 };

  1049 

  1050 /** Lists the bus configuration.

  1051  */

  1052 void Master::listConfigs(const ConfigList &configList)

  1053 {

  1054     ConfigList::const_iterator configIter;

  1055     stringstream str;

  1056     ConfigInfo info;

  1057     typedef list<ConfigInfo> ConfigInfoList;

  1058     ConfigInfoList list;

  1059     ConfigInfoList::const_iterator iter;

  1060     unsigned int maxAliasWidth = 0, maxPosWidth = 0,

  1061                  maxAttWidth = 0, maxOpWidth = 0;

  1062 

  1063     for (configIter = configList.begin();

  1064             configIter != configList.end();

  1065             configIter++) {

  1066 

  1067         str << dec << configIter->alias;

  1068         info.alias = str.str();

  1069         str.clear();

  1070         str.str("");

  1071 

  1072         str << configIter->position;

  1073         info.pos = str.str();

  1074         str.clear();

  1075         str.str("");

  1076 

  1077         str << hex << setfill('0')

  1078             << "0x" << setw(8) << configIter->vendor_id

  1079             << "/0x" << setw(8) << configIter->product_code;

  1080         info.ident = str.str();

  1081         str.clear();

  1082         str.str("");

  1083 

  1084         str << (configIter->attached ? "attached" : "-");

  1085         info.att = str.str();

  1086         str.clear();

  1087         str.str("");

  1088 

  1089         str << (configIter->operational ? "operational" : "-");

  1090         info.op = str.str();

  1091         str.clear();

  1092         str.str("");

  1093 

  1094         list.push_back(info);

  1095 

  1096         if (info.alias.length() > maxAliasWidth)

  1097             maxAliasWidth = info.alias.length();

  1098         if (info.pos.length() > maxPosWidth)

  1099             maxPosWidth = info.pos.length();

  1100         if (info.att.length() > maxAttWidth)

  1101             maxAttWidth = info.att.length();

  1102         if (info.op.length() > maxOpWidth)

  1103             maxOpWidth = info.op.length();

  1104     }

  1105 

  1106     for (iter = list.begin(); iter != list.end(); iter++) {

  1107         cout << setfill(' ') << right

  1108             << setw(maxAliasWidth) << iter->alias

  1109             << ":" << left

  1110             << setw(maxPosWidth) << iter->pos

  1111             << "  "

  1112             << iter->ident

  1113             << "  "

  1114             << setw(maxAttWidth) << iter->att << "  "

  1115             << setw(maxOpWidth) << iter->op << "  "

  1116             << endl;

  1117     }

  1118 }

  1119 

  1120 /****************************************************************************/

  1121 

  1122 void Master::outputDomainData(unsigned int domainIndex)

  1123 {

  1124     ec_ioctl_domain_t domain;

  1125     ec_ioctl_domain_data_t data;

  1126     unsigned char *processData;

  1127     unsigned int i;

  1128     

  1129     getDomain(&domain, domainIndex);

  1130 

  1131     if (!domain.data_size)

  1132         return;

  1133 

  1134     processData = new unsigned char[domain.data_size];

  1135 

  1136     try {