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

// FIXME

    17

const char *help_upload =

    18

    "[OPTIONS]\n"

    19

    "\n"

    20

    "\n"

    21

    "Command-specific options:\n";

    22

    23

/****************************************************************************/

    24

    25

void command_upload(void)

    26

{

    27

    stringstream strIndex, strSubIndex;

    28

    int sval;

    29

    ec_ioctl_slave_sdo_upload_t data;

    30

    unsigned int uval;

    31

    const CoEDataType *dataType = NULL;

    32

    33

    if (slavePosition < 0) {

    34

        stringstream err;

    35

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

    36

        throw MasterDeviceException(err.str());

    37

    }

    38

    data.slave_position = slavePosition;

    39

    40

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

    41

        stringstream err;

    42

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

    43

        throw MasterDeviceException(err.str());

    44

    }

    45

    46

    strIndex << commandArgs[0];

    47

    strIndex

    48

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

    49

        >> data.sdo_index;

    50

    if (strIndex.fail()) {

    51

        stringstream err;

    52

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

    53

        throw MasterDeviceException(err.str());

    54

    }

    55

    56

    strSubIndex << commandArgs[1];

    57

    strSubIndex

    58

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

    59

        >> uval;

    60

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

    61

        stringstream err;

    62

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

    63

        throw MasterDeviceException(err.str());

    64

    }

    65

    data.sdo_entry_subindex = uval;

    66

    67

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

    68

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

    69

            stringstream err;

    70

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

    71

            throw MasterDeviceException(err.str());

    72

        }

    73

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

    74

        ec_ioctl_slave_sdo_entry_t entry;

    75

    76

        masterDev.open(MasterDevice::Read);

    77

    78

        try {

    79

            masterDev.getSdoEntry(&entry, slavePosition,

    80

                    data.sdo_index, data.sdo_entry_subindex);

    81

        } catch (MasterDeviceException &e) {

    82

            stringstream err;

    83

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

    84

                << "Please specify --type.";

    85

            throw ExecutionFailureException(err);

    86

        }

    87

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

    88

            stringstream err;

    89

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

    90

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

    91

                << " Please specify --type.";

    92

            throw ExecutionFailureException(err);

    93

        }

    94

    }

    95

    96

    if (dataType->byteSize) {

    97

        data.target_size = dataType->byteSize;

    98

    } else {

    99

        data.target_size = DefaultBufferSize;

   100

    }

   101

   102

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

   103

   104

    masterDev.open(MasterDevice::Read);

   105

   106

	try {

   107

		masterDev.sdoUpload(&data);

   108

	} catch (MasterDeviceException &e) {

   109

        delete [] data.target;

   110

        throw e;

   111

    }

   112

   113

    masterDev.close();

   114

   115

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

   116

        stringstream err;

   117

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

   118

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

   119

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

   120

        throw MasterDeviceException(err.str());

   121

    }

   122

   123

    cout << setfill('0');

   124

    switch (dataType->coeCode) {

   125

        case 0x0002: // int8

   126

            sval = *(int8_t *) data.target;

   127

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

   128

            break;

   129

        case 0x0003: // int16

   130

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

   131

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

   132

            break;

   133

        case 0x0004: // int32

   134

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

   135

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

   136

            break;

   137

        case 0x0005: // uint8

   138

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

   139

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

   140

            break;

   141

        case 0x0006: // uint16

   142

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

   143

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

   144

            break;

   145

        case 0x0007: // uint32

   146

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

   147

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

   148

            break;

   149

        case 0x0009: // string

   150

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

   151

                << endl;

   152

            break;

   153

        default:

   154

            printRawData(data.target, data.data_size);

   155

            break;

   156

    }

   157

   158

    delete [] data.target;

   159

}

   160

   161

/*****************************************************************************/