20 

    21 #if __BYTE_ORDER == __LITTLE_ENDIAN

    22 

    23 #define le16tocpu(x) x

    24 #define le32tocpu(x) x

    25 

    26 #elif __BYTE_ORDER == __BIG_ENDIAN

    27 

    28 #define le16tocpu(x) \

    29         ((uint16_t)( \

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

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

    32 #define le32tocpu(x) \

    33         ((uint32_t)( \

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

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

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

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

    38 

    39 #endif

    40 

    41 /****************************************************************************/

    42 

    43 struct CoEDataType {

    44     const char *name;

    45     uint16_t coeCode;

    46     unsigned int byteSize;

    47 };

    48 

    49 static const CoEDataType dataTypes[] = {

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

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

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

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

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

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

    56     {"string", 0x0009, 0},

    57     {"raw",    0xffff, 0},

    58     {}

    59 };

    60 

    61 /****************************************************************************/

    62 

    63 const CoEDataType *findDataType(const string &str)

    64 {

    65     const CoEDataType *d;

    66     

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

    68         if (str == d->name)

    69             return d;

    70 

    71     return NULL;

    72 }

    73 

    74 /****************************************************************************/

    75 

    76 const CoEDataType *findDataType(uint16_t code)

    77 {

    78     const CoEDataType *d;

    79     

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

    81         if (code == d->coeCode)

    82             return d;

    83 

    84     return NULL;

    85 }

   308 void Master::sdoUpload(

   309         int slavePosition,

   310         const string &dataTypeStr,

   311         const vector<string> &commandArgs

   312         )

   313 {

   314     stringstream strIndex, strSubIndex;

   315     int number, sval;

   316     ec_ioctl_sdo_upload_t data;

   317     unsigned int i, uval;

   318     const CoEDataType *dataType = NULL;

   319 

   320     if (slavePosition < 0) {

   321         stringstream err;

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

   323         throw MasterException(err.str());

   324     }

   325     data.slave_position = slavePosition;

   326 

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

   328         stringstream err;

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

   330         throw MasterException(err.str());

   331     }

   332 

   333     strIndex << commandArgs[0];

   334     strIndex >> hex >> number;

   335     if (strIndex.fail() || number < 0x0000 || number > 0xffff) {

   336         stringstream err;

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

   338         throw MasterException(err.str());

   339     }

   340     data.sdo_index = number;

   341 

   342     strSubIndex << commandArgs[1];

   343     strSubIndex >> hex >> number;

   344     if (strSubIndex.fail() || number < 0x00 || number > 0xff) {

   345         stringstream err;

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

   347         throw MasterException(err.str());

   348     }

   349     data.sdo_entry_subindex = number;

   350 

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

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

   353             stringstream err;

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

   355             throw MasterException(err.str());

   356         }

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

   358         ec_ioctl_sdo_entry_t entry;

   359         unsigned int entryByteSize;

   360         try {

   361             getSdoEntry(&entry, slavePosition,

   362                     data.sdo_index, data.sdo_entry_subindex);

   363         } catch (MasterException &e) {

   364             stringstream err;

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

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

   367             throw MasterException(err.str());

   368         }

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

   370             stringstream err;

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

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

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

   374             throw MasterException(err.str());

   375         }

   376     }

   377 

   378     if (dataType->byteSize) {

   379         data.target_size = dataType->byteSize;

   380     } else {

   381         data.target_size = DefaultTargetSize;

   382     }

   383 

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

   385 

   386     if (ioctl(fd, EC_IOCTL_SDO_UPLOAD, &data) < 0) {

   387         stringstream err;

   388         err << "Failed to upload Sdo: " << strerror(errno);

   389         delete [] data.target;

   390         throw MasterException(err.str());

   391     }

   392 

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

   394         stringstream err;

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

   396             << " with " << dataType->byteSize << " byte(s), but got "

   397             << data.data_size << " byte(s).";

   398         throw MasterException(err.str());

   399     }

   400 

   401     cout << setfill('0');

   402     switch (dataType->coeCode) {

   403         case 0x0002: // int8

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

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

   406             break;

   407         case 0x0003: // int16

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

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

   410             break;

   411         case 0x0004: // int32

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

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

   414             break;

   415         case 0x0005: // uint8

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

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

   418             break;

   419         case 0x0006: // uint16

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

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

   422             break;

   423         case 0x0007: // uint32

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

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

   426             break;

   427         case 0x0009: // string

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

   429                 << endl;

   430             break;

   431         default:

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

   433             break;

   434     }

   435 

   436     delete [] data.target;

   437 }

   438 

   439 /****************************************************************************/

   440 

  1062 

  1063 void Master::printRawData(

  1064         const uint8_t *data,

  1065         unsigned int size)

  1066 {

  1067     cout << hex << setfill('0');

  1068     while (size--) {

  1069         cout << "0x" << setw(2) << (unsigned int) *data++;

  1070         if (size)

  1071             cout << " ";

  1072     }

  1073     cout << endl;

  1074 }

  1075 

  1076 /****************************************************************************/