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

     2  *

     3  * $Id$

     4  *

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

     6 

     7 #include <iostream>

     8 #include <iomanip>

     9 using namespace std;

    10 

    11 #include "globals.h"

    12 #include "coe_datatypes.h"

    13 

    14 /****************************************************************************/

    15 

    16 const char *help_sdo_upload =

    17     "[OPTIONS]\n"

    18     "\n"

    19     "\n"

    20     "Command-specific options:\n";

    21 

    22 /****************************************************************************/

    23 

    24 void command_sdo_upload(void)

    25 {

    26     stringstream strIndex, strSubIndex;

    27     int sval;

    28     ec_ioctl_slave_sdo_upload_t data;

    29     unsigned int uval;

    30     const CoEDataType *dataType = NULL;

    31 

    32     if (slavePosition < 0) {

    33         stringstream err;

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

    35         throw MasterDeviceException(err.str());

    36     }

    37     data.slave_position = slavePosition;

    38 

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

    40         stringstream err;

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

    42         throw MasterDeviceException(err.str());

    43     }

    44 

    45     strIndex << commandArgs[0];

    46     strIndex

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

    48         >> data.sdo_index;

    49     if (strIndex.fail()) {

    50         stringstream err;

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

    52         throw MasterDeviceException(err.str());

    53     }

    54 

    55     strSubIndex << commandArgs[1];

    56     strSubIndex

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

    58         >> uval;

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

    60         stringstream err;

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

    62         throw MasterDeviceException(err.str());

    63     }

    64     data.sdo_entry_subindex = uval;

    65 

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

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

    68             stringstream err;

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

    70             throw MasterDeviceException(err.str());

    71         }

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

    73         ec_ioctl_slave_sdo_entry_t entry;

    74 

    75         masterDev.open(MasterDevice::Read);

    76 

    77         try {

    78             masterDev.getSdoEntry(&entry, slavePosition,

    79                     data.sdo_index, data.sdo_entry_subindex);

    80         } catch (MasterDeviceException &e) {

    81             stringstream err;

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

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

    84             throw ExecutionFailureException(err);

    85         }

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

    87             stringstream err;

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

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

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

    91             throw ExecutionFailureException(err);

    92         }

    93     }

    94 

    95     if (dataType->byteSize) {

    96         data.target_size = dataType->byteSize;

    97     } else {

    98         data.target_size = DefaultBufferSize;

    99     }

   100 

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

   102 

   103     masterDev.open(MasterDevice::Read);

   104 

   105 	try {

   106 		masterDev.sdoUpload(&data);

   107 	} catch (MasterDeviceException &e) {

   108         delete [] data.target;

   109         throw e;

   110     }

   111 

   112     masterDev.close();

   113 

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

   115         stringstream err;

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

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

   118             << data.data_size << " byte.";

   119         throw MasterDeviceException(err.str());

   120     }

   121 

   122     cout << setfill('0');

   123     switch (dataType->coeCode) {

   124         case 0x0002: // int8

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

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

   127             break;

   128         case 0x0003: // int16

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

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

   131             break;

   132         case 0x0004: // int32

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

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

   135             break;

   136         case 0x0005: // uint8

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

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

   139             break;

   140         case 0x0006: // uint16

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

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

   143             break;

   144         case 0x0007: // uint32

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

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

   147             break;

   148         case 0x0009: // string

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

   150                 << endl;

   151             break;

   152         default:

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

   154             break;

   155     }

   156 

   157     delete [] data.target;

   158 }

   159 

   160 /*****************************************************************************/