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;

    93

}

    94

    95

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

    96

    97

Master::~Master()

    98

{

    99

    close();

   100

}

   101

   102

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

   103

   104

void Master::open(unsigned int index)

   105

{

   106

    stringstream deviceName;

   107

   108

    Master::index = index;

   109

   110

    deviceName << "/dev/EtherCAT" << index;

   111

   112

    if ((fd = ::open(deviceName.str().c_str(), O_RDONLY)) == -1) {

   113

        stringstream err;

   114

        err << "Failed to open master device " << deviceName.str() << ": "

   115

            << strerror(errno);

   116

        throw MasterException(err.str());

   117

    }

   118

}

   119

   120

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

   121

   122

void Master::close()

   123

{

   124

    if (fd == -1)

   125

        return;

   126

   127

    ::close(fd);

   128

}

   129

   130

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

   131

   132

void Master::outputData(int domainIndex)

   133

{

   134

    if (domainIndex == -1) {

   135

        unsigned int numDomains = domainCount(), i;

   136

   137

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

   138

            outputDomainData(i);

   139

        }

   140

    } else {

   141

        outputDomainData(domainIndex);

   142

    }

   143

}

   144

   145

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

   146

   147

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

   148

{

   149

    stringstream str;

   150

    int debugLevel;

   151

   152

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

   153

        stringstream err;

   154

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

   155

        throw MasterException(err.str());

   156

    }

   157

   158

    str << commandArgs[0];

   159

    str >> debugLevel;

   160

   161

    if (str.fail()) {

   162

        stringstream err;

   163

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

   164

        throw MasterException(err.str());

   165

    }

   166

   167

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

   168

        stringstream err;

   169

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

   170

        throw MasterException(err.str());

   171

    }

   172

}

   173

   174

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

   175

   176

void Master::showDomains(int domainIndex)

   177

{

   178

    if (domainIndex == -1) {

   179

        unsigned int numDomains = domainCount(), i;

   180

   181

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

   182

            showDomain(i);

   183

        }

   184

    } else {

   185

        showDomain(domainIndex);

   186

    }

   187

}

   188

   189

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

   190

   191

void Master::listSlaves()

   192

{

   193

    unsigned int numSlaves = slaveCount(), i;

   194

    ec_ioctl_slave_t slave;

   195

    uint16_t lastAlias, aliasIndex;

   196

   197

    lastAlias = 0;

   198

    aliasIndex = 0;

   199

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

   200

        getSlave(&slave, i);

   201

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

   202

   203

        if (slave.alias) {

   204

            lastAlias = slave.alias;

   205

            aliasIndex = 0;

   206

        }

   207

        if (lastAlias) {

   208

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

   209

        }

   210

   211

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

   212

   213

        if (strlen(slave.name)) {

   214

            cout << slave.name;

   215

        } else {

   216

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

   217

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

   218

        }

   219

   220

        cout << endl;

   221

    }

   222

}

   223

   224

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

   225

   226

void Master::showMaster()

   227

{

   228

    ec_ioctl_master_t data;

   229

    stringstream err;

   230

    unsigned int i;

   231

   232

    getMaster(&data);

   233

   234

    cout

   235

        << "Master" << index << endl

   236

        << "  State: ";

   237

   238

    switch (data.mode) {

   239

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

   240

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

   241

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

   242

        default:

   243

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

   244

                throw MasterException(err.str());

   245

    }

   246

   247

    cout << endl

   248

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

   249

   250

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

   251

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

   252

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

   253

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

   254

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

   255

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

   256

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

   257

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

   258

            cout << "None.";

   259

        } else {

   260

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

   261

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

   262

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

   263

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

   264

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

   265

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

   266

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

   267

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

   268

                << ")" << endl << dec

   269

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

   270

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

   271

        }

   272

        cout << endl;

   273

    }

   274

}

   275

   276

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

   277

   278

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

   279

{

   280

    if (slavePosition == -1) {

   281

        unsigned int numSlaves = slaveCount(), i;

   282

   283

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

   284

            listSlavePdos(i, quiet, true);

   285

        }

   286

    } else {

   287

        listSlavePdos(slavePosition, quiet, false);

   288

    }

   289

}

   290

   291

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

   292

   293

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

   294

{

   295

    if (slavePosition == -1) {

   296

        unsigned int numSlaves = slaveCount(), i;

   297

   298

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

   299

            listSlaveSdos(i, quiet, true);

   300

        }

   301

    } else {

   302

        listSlaveSdos(slavePosition, quiet, false);

   303

    }

   304

}

   305

   306

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

   307

   308

void Master::sdoUpload(

   309

        int slavePosition,

   310

        const string &dataTypeStr,

   311

        const vector<string> &commandArgs

   312

        )

   313

{

   314

    stringstream strIndex, strSubIndex;

   315

    int number, sval;

   316

    ec_ioctl_sdo_upload_t data;

   317

    unsigned int i, uval;

   318

    const CoEDataType *dataType = NULL;

   319

   320

    if (slavePosition < 0) {

   321

        stringstream err;

   322

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

   323

        throw MasterException(err.str());

   324

    }

   325

    data.slave_position = slavePosition;

   326

   327

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

   328

        stringstream err;

   329

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

   330

        throw MasterException(err.str());

   331

    }

   332

   333

    strIndex << commandArgs[0];

   334

    strIndex >> hex >> number;

   335

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

   336

        stringstream err;

   337

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

   338

        throw MasterException(err.str());

   339

    }

   340

    data.sdo_index = number;

   341

   342

    strSubIndex << commandArgs[1];

   343

    strSubIndex >> hex >> number;

   344

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

   345

        stringstream err;

   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

            stringstream err;

   354

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

   355

            throw MasterException(err.str());

   356

        }

   357

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

   358

        ec_ioctl_sdo_entry_t entry;

   359

        unsigned int entryByteSize;

   360

        try {

   361

            getSdoEntry(&entry, slavePosition,

   362

                    data.sdo_index, data.sdo_entry_subindex);

   363

        } catch (MasterException &e) {

   364

            stringstream err;

   365

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

   366

                << "Please specify --type.";

   367

            throw MasterException(err.str());

   368

        }

   369

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

   370

            stringstream err;

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

   380

    } else {

   381

        data.target_size = DefaultTargetSize;

   382

    }

   383

   384

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

   385

   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;

   390

        throw MasterException(err.str());

   391

    }

   392

   393

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

   394

        stringstream err;

   395

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

   396

            << " with " << dataType->byteSize << " byte(s), but got "

   397

            << data.data_size << " byte(s).";

   398

        throw MasterException(err.str());

   399

    }

   400

   401

    cout << setfill('0');

   402

    switch (dataType->coeCode) {

   403

        case 0x0002: // int8

   404

            sval = *(int8_t *) data.target;

   405

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

   406

            break;

   407

        case 0x0003: // int16

   408

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

   409

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

   410

            break;

   411

        case 0x0004: // int32

   412

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

   413

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

   414

            break;

   415

        case 0x0005: // uint8

   416

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

   417

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

   418

            break;

   419

        case 0x0006: // uint16

   420

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

   421

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

   422

            break;

   423

        case 0x0007: // uint32

   424

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

   425

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

   426

            break;

   427

        case 0x0009: // string

   428

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

   429

                << endl;

   430

            break;

   431

        default:

   432

            printRawData(data.target, data.data_size);

   433

            break;

   434

    }

   435

   436

    delete [] data.target;

   437

}

   438

   439

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

   440

   441

void Master::requestStates(

   442

        int slavePosition,

   443

        const vector<string> &commandArgs

   444

        )

   445

{

   446

    string stateStr;

   447

    uint8_t state;

   448

   449

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

   450

        stringstream err;

   451

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

   452

        throw MasterException(err.str());

   453

    }

   454

   455

    stateStr = commandArgs[0];

   456

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

   457

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

   458

   459

    if (stateStr == "INIT") {

   460

        state = 0x01;

   461

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

   462

        state = 0x02;

   463

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

   464

        state = 0x04;

   465

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

   466

        state = 0x08;

   467

    } else {

   468

        stringstream err;

   469

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

   470

        throw MasterException(err.str());

   471

    }

   472

   473

    if (slavePosition == -1) {

   474

        unsigned int i, numSlaves = slaveCount();

   475

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

   476

            requestState(i, state);

   477

    } else {

   478

        requestState(slavePosition, state);

   479

    }

   480

}

   481

   482

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

   483

   484

void Master::generateXml(int slavePosition)

   485

{

   486

    if (slavePosition == -1) {

   487

        unsigned int numSlaves = slaveCount(), i;

   488

   489

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

   490

            generateSlaveXml(i);

   491

        }

   492

    } else {

   493

        generateSlaveXml(slavePosition);

   494

    }

   495

}

   496

   497

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

   498

   499

void Master::outputDomainData(unsigned int domainIndex)