31 

    32 #if __BYTE_ORDER == __LITTLE_ENDIAN

    33 

    34 #define le16tocpu(x) x

    35 #define le32tocpu(x) x

    36 

    37 #define cputole16(x) x

    38 #define cputole32(x) x

    39 

    40 #elif __BYTE_ORDER == __BIG_ENDIAN

    41 

    42 #define le16tocpu(x) swap16(x)

    43 #define le32tocpu(x) swap32(x)

    44 

    45 #define cputole16(x) swap16(x)

    46 #define cputole32(x) swap32(x)

   312 void Master::sdoDownload(

   313         int slavePosition,

   314         const string &dataTypeStr,

   315         const vector<string> &commandArgs

   316         )

   317 {

   318     stringstream strIndex, strSubIndex, strValue, err;

   319     int number, sval;

   320     ec_ioctl_sdo_download_t data;

   321     unsigned int i, uval;

   322     const CoEDataType *dataType = NULL;

   323 

   324     if (slavePosition < 0) {

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

   326         throw MasterException(err.str());

   327     }

   328     data.slave_position = slavePosition;

   329 

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

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

   332         throw MasterException(err.str());

   333     }

   334 

   335     strIndex << commandArgs[0];

   336     strIndex >> hex >> number;

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

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

   339         throw MasterException(err.str());

   340     }

   341     data.sdo_index = number;

   342 

   343     strSubIndex << commandArgs[1];

   344     strSubIndex >> hex >> number;

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

   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             err << "Invalid data type '" << dataTypeStr << "'!";

   354             throw MasterException(err.str());

   355         }

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

   357         ec_ioctl_sdo_entry_t entry;

   358         unsigned int entryByteSize;

   359 

   360         open(Read);

   361 

   362         try {

   363             getSdoEntry(&entry, slavePosition,

   364                     data.sdo_index, data.sdo_entry_subindex);

   365         } catch (MasterException &e) {

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

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

   368             throw MasterException(err.str());

   369         }

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

   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.data_size = dataType->byteSize;

   380     } else {

   381         data.data_size = DefaultBufferSize;

   382     }

   383 

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

   385 

   386     strValue << commandArgs[2];

   387 

   388     switch (dataType->coeCode) {

   389         case 0x0002: // int8

   390             strValue >> sval;

   391             if ((uint32_t) sval > 0xff) {

   392                 delete [] data.data;

   393                 err << "Invalid value for type '"

   394                     << dataType->name << "'!";

   395                 throw MasterException(err.str());

   396             }

   397             *data.data = (int8_t) sval;

   398             break;

   399         case 0x0003: // int16

   400             strValue >> sval;

   401             if ((uint32_t) sval > 0xffff) {

   402                 delete [] data.data;

   403                 err << "Invalid value for type '"

   404                     << dataType->name << "'!";

   405                 throw MasterException(err.str());

   406             }

   407             *(int16_t *) data.data = cputole16(sval);

   408             break;

   409         case 0x0004: // int32

   410             strValue >> sval;

   411             *(int32_t *) data.data = cputole32(sval);

   412             break;

   413         case 0x0005: // uint8

   414             strValue >> uval;

   415             if ((uint32_t) uval > 0xff) {

   416                 delete [] data.data;

   417                 err << "Invalid value for type '"

   418                     << dataType->name << "'!";

   419                 throw MasterException(err.str());

   420             }

   421             *data.data = (uint8_t) uval;

   422             break;

   423         case 0x0006: // uint16

   424             strValue >> uval;

   425             if ((uint32_t) uval > 0xffff) {

   426                 delete [] data.data;

   427                 err << "Invalid value for type '"

   428                     << dataType->name << "'!";

   429                 throw MasterException(err.str());

   430             }

   431             *(uint16_t *) data.data = cputole16(uval);

   432             break;

   433         case 0x0007: // uint32

   434             strValue >> uval;

   435             *(uint32_t *) data.data = cputole32(uval);

   436             break;

   437         case 0x0009: // string

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

   439                 err << "String too big";

   440                 throw MasterException(err.str());

   441             }

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

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

   444             break;

   445         default:

   446             break;

   447     }

   448 

   449     if (strValue.fail()) {

   450         err << "Invalid value argument '" << commandArgs[2] << "'!";

   451         throw MasterException(err.str());

   452     }

   453 

   454     cerr << "dt " << dataType->name << endl;

   455     printRawData(data.data, data.data_size);

   456 

   457     open(ReadWrite);

   458 

   459     if (ioctl(fd, EC_IOCTL_SDO_DOWNLOAD, &data) < 0) {

   460         stringstream err;

   461         err << "Failed to download Sdo: " << strerror(errno);

   462         delete [] data.data;

   463         throw MasterException(err.str());

   464     }

   465 

   466     delete [] data.data;

   467 }

   468 

   469 /****************************************************************************/

   470