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_upload =

    17     "[OPTIONS] <INDEX> <SUBINDEX>\n"

    18     "\n"

    19     "Upload an Sdo entry from a slave.\n"

    20     "\n"

    21     "The data type of the Sdo entry is taken from the Sdo dictionary by\n"

    22     "default. It can be overridden with the --type option. If the slave\n"

    23     "does not support the Sdo information service or the Sdo is not in the\n"

    24     "dictionary, the --type option is mandatory.\n"

    25     "\n"

    26     "These are the valid Sdo entry data types:\n"

    27     "  int8, int16, int32, uint8, uint16, uint32, string.\n"

    28     "\n"

    29     "Arguments:\n"

    30     "  INDEX    is the Sdo index and must be an unsigned 16 bit number.\n"

    31     "  SUBINDEX is the Sdo entry subindex and must be an unsigned 8 bit\n"

    32     "           number.\n"

    33     "\n"

    34     "Command-specific options:\n"

    35     "  --slave -s <index>  Positive numerical ring position (mandatory).\n"

    36     "  --type  -t <type>   Forced Sdo entry data type (see above).\n"

    37     "\n"

    38     "Numerical values can be specified either with decimal (no prefix),\n"

    39     "octal (prefix '0') or hexadecimal (prefix '0x') base.\n";

    40 

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

    42 

    43 void command_upload(void)

    44 {

    45     stringstream err, strIndex, strSubIndex;

    46     int sval;

    47     ec_ioctl_slave_sdo_upload_t data;

    48     unsigned int uval;

    49     const CoEDataType *dataType = NULL;

    50 

    51     if (slavePosition < 0) {

    52         err << "'" << commandName << "' requires a slave! "

    53             << "Please specify --slave.";

    54         throw InvalidUsageException(err);

    55     }

    56     data.slave_position = slavePosition;

    57 

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

    59         err << "'" << commandName << "' takes two arguments!";

    60         throw InvalidUsageException(err);

    61     }

    62 

    63     strIndex << commandArgs[0];

    64     strIndex

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

    66         >> data.sdo_index;

    67     if (strIndex.fail()) {

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

    69         throw InvalidUsageException(err);

    70     }

    71 

    72     strSubIndex << commandArgs[1];

    73     strSubIndex

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

    75         >> uval;

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

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

    78         throw InvalidUsageException(err);

    79     }

    80     data.sdo_entry_subindex = uval;

    81 

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

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

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

    85             throw InvalidUsageException(err);

    86         }

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

    88         ec_ioctl_slave_sdo_entry_t entry;

    89 

    90         masterDev.open(MasterDevice::Read);

    91 

    92         try {

    93             masterDev.getSdoEntry(&entry, slavePosition,

    94                     data.sdo_index, data.sdo_entry_subindex);

    95         } catch (MasterDeviceException &e) {

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

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

    98             throw CommandException(err);

    99         }

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

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

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

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

   104             throw CommandException(err);

   105         }

   106     }

   107 

   108     if (dataType->byteSize) {

   109         data.target_size = dataType->byteSize;

   110     } else {

   111         data.target_size = DefaultBufferSize;

   112     }

   113 

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

   115 

   116     masterDev.open(MasterDevice::Read);

   117 

   118 	try {

   119 		masterDev.sdoUpload(&data);

   120 	} catch (MasterDeviceException &e) {

   121         delete [] data.target;

   122         throw e;

   123     }

   124 

   125     masterDev.close();

   126 

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

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

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

   130             << data.data_size << " byte.";

   131         throw CommandException(err);

   132     }

   133 

   134     cout << setfill('0');

   135     switch (dataType->coeCode) {

   136         case 0x0002: // int8

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

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

   139             break;

   140         case 0x0003: // int16

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

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

   143             break;

   144         case 0x0004: // int32

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

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

   147             break;

   148         case 0x0005: // uint8

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

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

   151             break;

   152         case 0x0006: // uint16

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

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

   155             break;

   156         case 0x0007: // uint32

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

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

   159             break;

   160         case 0x0009: // string

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

   162                 << endl;

   163             break;

   164         default:

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

   166             break;

   167     }

   168 

   169     delete [] data.target;

   170 }

   171 

   172 /*****************************************************************************/