1

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

     2

 *

     3

 * $Id$

     4

 *

     5

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

     6

     7

#include <sys/types.h>

     8

#include <sys/stat.h>

     9

#include <fcntl.h>

    10

#include <errno.h>

    11

#include <sys/ioctl.h>

    12

    13

#include <iostream>

    14

#include <iomanip>

    15

#include <sstream>

    16

#include <cctype> // toupper()

    17

using namespace std;

    18

    19

#include "Master.h"

    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

}

    86

    87

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

    88

    89

Master::Master()

    90

{

    91

    index = 0;

    92

    fd = -1;

    94

}

    95

    96

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

    97

    98

Master::~Master()

    99

{

   100

    close();

   101

}

   102

   103

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

   104

   108

}

   109

   110

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

   111

   112

void Master::outputData(int domainIndex)

   113

{

   116

    if (domainIndex == -1) {

   117

        unsigned int numDomains = domainCount(), i;

   118

   119

        for (i = 0; i < numDomains; i++) {

   120

            outputDomainData(i);

   121

        }

   122

    } else {

   123

        outputDomainData(domainIndex);

   124

    }

   125

}

   126

   127

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

   128

   129

void Master::setDebug(const vector<string> &commandArgs)

   130

{

   131

    stringstream str;

   132

    int debugLevel;

   133

   134

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

   135

        stringstream err;

   136

        err << "'debug' takes exactly one argument!";

   137

        throw MasterException(err.str());

   138

    }

   139

   140

    str << commandArgs[0];

   141

    str >> debugLevel;

   142

   143

    if (str.fail()) {

   144

        stringstream err;

   145

        err << "Invalid debug level '" << commandArgs[0] << "'!";

   146

        throw MasterException(err.str());

   147

    }

   148

   151

    if (ioctl(fd, EC_IOCTL_SET_DEBUG, debugLevel) < 0) {

   152

        stringstream err;

   153

        err << "Failed to set debug level: " << strerror(errno);

   154

        throw MasterException(err.str());

   155

    }

   156

}

   157

   158

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

   159

   160

void Master::showDomains(int domainIndex)

   161

{

   164

    if (domainIndex == -1) {

   165

        unsigned int numDomains = domainCount(), i;

   166

   167

        for (i = 0; i < numDomains; i++) {

   168

            showDomain(i);

   169

        }

   170

    } else {

   171

        showDomain(domainIndex);

   172

    }

   173

}

   174

   175

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

   176

   177

void Master::listSlaves()

   178

{

   180

    ec_ioctl_slave_t slave;

   181

    uint16_t lastAlias, aliasIndex;

   182

   187

    lastAlias = 0;

   188

    aliasIndex = 0;

   189

    for (i = 0; i < numSlaves; i++) {

   190

        getSlave(&slave, i);

   191

        cout << setw(2) << i << "  ";

   192

   193

        if (slave.alias) {

   194

            lastAlias = slave.alias;

   195

            aliasIndex = 0;

   196

        }

   197

        if (lastAlias) {

   198

            cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;

   199

        }

   200

   201

        cout << "  " << slaveState(slave.state) << "  ";

   202

   203

        if (strlen(slave.name)) {

   204

            cout << slave.name;

   205

        } else {

   206

            cout << "0x" << hex << setfill('0') << slave.vendor_id

   207

                << ":0x" << slave.product_code;

   208

        }

   209

   210

        cout << endl;

   211

    }

   212

}

   213

   214

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

   215

   216

void Master::showMaster()

   217

{

   218

    ec_ioctl_master_t data;

   219

    stringstream err;

   220

    unsigned int i;

   221

   223

    getMaster(&data);

   224

   225

    cout

   226

        << "Master" << index << endl

   227

        << "  State: ";

   228

   229

    switch (data.mode) {

   230

        case 0: cout << "Waiting for device..."; break;

   231

        case 1: cout << "Idle"; break;

   232

        case 2: cout << "Operation"; break;

   233

        default:

   234

                err << "Invalid master state " << data.mode;

   235

                throw MasterException(err.str());

   236

    }

   237

   238

    cout << endl

   239

        << "  Slaves: " << data.slave_count << endl;

   240

   241

    for (i = 0; i < 2; i++) {

   242

        cout << "  Device" << i << ": ";

   243

        if (data.devices[i].address[0] == 0x00

   244

                && data.devices[i].address[1] == 0x00

   245

                && data.devices[i].address[2] == 0x00

   246

                && data.devices[i].address[3] == 0x00

   247

                && data.devices[i].address[4] == 0x00

   248

                && data.devices[i].address[5] == 0x00) {

   249

            cout << "None.";

   250

        } else {

   251

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

   252

                << setw(2) << (unsigned int) data.devices[i].address[0] << ":"

   253

                << setw(2) << (unsigned int) data.devices[i].address[1] << ":"

   254

                << setw(2) << (unsigned int) data.devices[i].address[2] << ":"

   255

                << setw(2) << (unsigned int) data.devices[i].address[3] << ":"

   256

                << setw(2) << (unsigned int) data.devices[i].address[4] << ":"

   257

                << setw(2) << (unsigned int) data.devices[i].address[5] << " ("

   258

                << (data.devices[i].attached ? "attached" : "waiting...")

   259

                << ")" << endl << dec

   260

                << "    Tx count: " << data.devices[i].tx_count << endl

   261

                << "    Rx count: " << data.devices[i].rx_count;

   262

        }

   263

        cout << endl;

   264

    }

   265

}

   266

   267

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

   268

   269

void Master::listPdos(int slavePosition, bool quiet)

   270

{

   273

    if (slavePosition == -1) {

   274

        unsigned int numSlaves = slaveCount(), i;

   275

   276

        for (i = 0; i < numSlaves; i++) {

   277

            listSlavePdos(i, quiet, true);

   278

        }

   279

    } else {

   280

        listSlavePdos(slavePosition, quiet, false);

   281

    }

   282

}

   283

   284

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

   285

   286

void Master::listSdos(int slavePosition, bool quiet)

   287

{

   290

    if (slavePosition == -1) {

   291

        unsigned int numSlaves = slaveCount(), i;

   292

   293

        for (i = 0; i < numSlaves; i++) {

   294

            listSlaveSdos(i, quiet, true);

   295

        }

   296

    } else {

   297

        listSlaveSdos(slavePosition, quiet, false);

   298

    }

   299

}

   300

   301

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

   302

   303

void Master::sdoUpload(

   304

        int slavePosition,

   305

        const string &dataTypeStr,

   306

        const vector<string> &commandArgs

   307

        )

   308

{

   309

    stringstream strIndex, strSubIndex;

   310

    int number, sval;

   311

    ec_ioctl_sdo_upload_t data;

   312

    unsigned int i, uval;

   313

    const CoEDataType *dataType = NULL;

   314

   315

    if (slavePosition < 0) {

   316

        stringstream err;

   317

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

   318

        throw MasterException(err.str());

   319

    }

   320

    data.slave_position = slavePosition;

   321

   322

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

   323

        stringstream err;

   324

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

   325

        throw MasterException(err.str());

   326

    }

   327

   328

    strIndex << commandArgs[0];

   329

    strIndex >> hex >> number;

   330

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

   331

        stringstream err;

   332

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

   333

        throw MasterException(err.str());

   334

    }

   335

    data.sdo_index = number;

   336

   337

    strSubIndex << commandArgs[1];

   338

    strSubIndex >> hex >> number;

   339

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

   340

        stringstream err;

   341

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

   342

        throw MasterException(err.str());

   343

    }

   344

    data.sdo_entry_subindex = number;

   345

   346

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

   347

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

   348

            stringstream err;

   349

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

   350

            throw MasterException(err.str());

   351

        }

   352

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

   353

        ec_ioctl_sdo_entry_t entry;

   354

        unsigned int entryByteSize;

   358

        try {

   359

            getSdoEntry(&entry, slavePosition,

   360

                    data.sdo_index, data.sdo_entry_subindex);

   361

        } catch (MasterException &e) {

   362

            stringstream err;

   363

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

   364

                << "Please specify --type.";

   365

            throw MasterException(err.str());

   366

        }

   367

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

   368

            stringstream err;

   369

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

   370

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

   371

                << " Please specify --type.";

   372

            throw MasterException(err.str());

   373

        }

   374

    }

   375

   376

    if (dataType->byteSize) {

   377

        data.target_size = dataType->byteSize;

   378

    } else {

   379

        data.target_size = DefaultTargetSize;

   380

    }

   381

   382

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

   383

   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;

   395

   396

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

   397

        stringstream err;

   398

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

   399

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

   400

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

   401

        throw MasterException(err.str());

   402

    }

   403

   404

    cout << setfill('0');

   405

    switch (dataType->coeCode) {

   406

        case 0x0002: // int8

   407

            sval = *(int8_t *) data.target;

   408

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

   409

            break;

   410

        case 0x0003: // int16

   411

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

   412

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

   413

            break;

   414

        case 0x0004: // int32

   415

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

   416

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

   417

            break;

   418

        case 0x0005: // uint8

   419

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

   420

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

   421

            break;

   422

        case 0x0006: // uint16

   423

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

   424

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

   425

            break;

   426

        case 0x0007: // uint32

   427

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

   428

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

   429

            break;

   430

        case 0x0009: // string

   431

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

   432

                << endl;

   433

            break;

   434

        default:

   435

            printRawData(data.target, data.data_size);

   436

            break;

   437

    }

   438

   439

    delete [] data.target;

   440

}

   441

   442

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

   443

   444

void Master::requestStates(

   445

        int slavePosition,

   446

        const vector<string> &commandArgs

   447

        )

   448

{

   449

    string stateStr;

   450

    uint8_t state;

   451

   452

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

   453

        stringstream err;

   454

        err << "'state' takes exactly one argument!";

   455

        throw MasterException(err.str());

   456

    }

   457

   458

    stateStr = commandArgs[0];

   459

    transform(stateStr.begin(), stateStr.end(),

   460

            stateStr.begin(), (int (*) (int)) std::toupper);

   461

   462

    if (stateStr == "INIT") {

   463

        state = 0x01;

   464

    } else if (stateStr == "PREOP") {

   465

        state = 0x02;

   466

    } else if (stateStr == "SAFEOP") {

   467

        state = 0x04;

   468

    } else if (stateStr == "OP") {

   469

        state = 0x08;

   470

    } else {

   471

        stringstream err;

   472

        err << "Invalid state '" << commandArgs[0] << "'!";

   473

        throw MasterException(err.str());

   474

    }

   475

   478

    if (slavePosition == -1) {

   479

        unsigned int i, numSlaves = slaveCount();

   480

        for (i = 0; i < numSlaves; i++)

   481

            requestState(i, state);

   482

    } else {

   483

        requestState(slavePosition, state);

   484

    }

   485

}

   486

   487

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

   488

   489

void Master::generateXml(int slavePosition)

   490

{

   493

    if (slavePosition == -1) {

   494

        unsigned int numSlaves = slaveCount(), i;

   495

   496

        for (i = 0; i < numSlaves; i++) {