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

#define swap16(x) \

    22

        ((uint16_t)( \

    23

        (((uint16_t)(x) & 0x00ffU) << 8) | \

    24

        (((uint16_t)(x) & 0xff00U) >> 8) ))

    25

#define swap32(x) \

    26

        ((uint32_t)( \

    27

        (((uint32_t)(x) & 0x000000ffUL) << 24) | \

    28

        (((uint32_t)(x) & 0x0000ff00UL) <<  8) | \

    29

        (((uint32_t)(x) & 0x00ff0000UL) >>  8) | \

    30

        (((uint32_t)(x) & 0xff000000UL) >> 24) ))

    31

    32

#if __BYTE_ORDER == __LITTLE_ENDIAN

    33

    34

#define le16tocpu(x) x

    35

#define le32tocpu(x) x

    36

    37

#define cputole16(x) x

    38

#define cputole32(x) x

    39

    40

#elif __BYTE_ORDER == __BIG_ENDIAN

    41

    42

#define le16tocpu(x) swap16(x)

    43

#define le32tocpu(x) swap32(x)

    44

    45

#define cputole16(x) swap16(x)

    46

#define cputole32(x) swap32(x)

    47

    48

#endif

    49

    50

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

    51

    52

struct CoEDataType {

    53

    const char *name;

    54

    uint16_t coeCode;

    55

    unsigned int byteSize;

    56

};

    57

    58

static const CoEDataType dataTypes[] = {

    59

    {"int8",   0x0002, 1},

    60

    {"int16",  0x0003, 2},

    61

    {"int32",  0x0004, 4},

    62

    {"uint8",  0x0005, 1},

    63

    {"uint16", 0x0006, 2},

    64

    {"uint32", 0x0007, 4},

    65

    {"string", 0x0009, 0},

    66

    {"raw",    0xffff, 0},

    67

    {}

    68

};

    69

    70

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

    71

    72

const CoEDataType *findDataType(const string &str)

    73

{

    74

    const CoEDataType *d;

    75

    76

    for (d = dataTypes; d->name; d++)

    77

        if (str == d->name)

    78

            return d;

    79

    80

    return NULL;

    81

}

    82

    83

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

    84

    85

const CoEDataType *findDataType(uint16_t code)

    86

{

    87

    const CoEDataType *d;

    88

    89

    for (d = dataTypes; d->name; d++)

    90

        if (code == d->coeCode)

    91

            return d;

    92

    93

    return NULL;

    94

}

    95

    96

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

    97

    98

Master::Master()

    99

{

   100

    index = 0;

   101

    fd = -1;

   102

    currentPermissions = Read;

   103

}

   104

   105

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

   106

   107

Master::~Master()

   108

{

   109

    close();

   110

}

   111

   112

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

   113

   114

void Master::setIndex(unsigned int i)

   115

{

   116

    index = i;

   117

}

   118

   119

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

   120

   121

void Master::outputData(int domainIndex)

   122

{

   123

    open(Read);

   124

   125

    if (domainIndex == -1) {

   126

        unsigned int numDomains = domainCount(), i;

   127

   128

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

   129

            outputDomainData(i);

   130

        }

   131

    } else {

   132

        outputDomainData(domainIndex);

   133

    }

   134

}

   135

   136

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

   137

   138

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

   139

{

   140

    stringstream str;

   141

    int debugLevel;

   142

   143

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

   144

        stringstream err;

   145

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

   146

        throw MasterException(err.str());

   147

    }

   148

   149

    str << commandArgs[0];

   150

    str >> debugLevel;

   151

   152

    if (str.fail()) {

   153

        stringstream err;

   154

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

   155

        throw MasterException(err.str());

   156

    }

   157

   158

    open(ReadWrite);

   159

   160

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

   161

        stringstream err;

   162

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

   163

        throw MasterException(err.str());

   164

    }

   165

}

   166

   167

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

   168

   169

void Master::showDomains(int domainIndex)

   170

{

   171

    open(Read);

   172

   173

    if (domainIndex == -1) {

   174

        unsigned int numDomains = domainCount(), i;

   175

   176

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

   177

            showDomain(i);

   178

        }

   179

    } else {

   180

        showDomain(domainIndex);

   181

    }

   182

}

   183

   184

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

   185

   186

void Master::listSlaves()

   187

{

   188

    unsigned int numSlaves, i;

   189

    ec_ioctl_slave_t slave;

   190

    uint16_t lastAlias, aliasIndex;

   191

   192

    open(Read);

   193

   194

    numSlaves = slaveCount();

   195

   196

    lastAlias = 0;

   197

    aliasIndex = 0;

   198

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

   199

        getSlave(&slave, i);

   200

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

   201

   202

        if (slave.alias) {

   203

            lastAlias = slave.alias;

   204

            aliasIndex = 0;

   205

        }

   206

        if (lastAlias) {

   207

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

   208

        }

   209

   210

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

   211

   212

        if (strlen(slave.name)) {

   213

            cout << slave.name;

   214

        } else {

   215

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

   216

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

   217

        }

   218

   219

        cout << endl;

   220

    }

   221

}

   222

   223

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

   224

   225

void Master::showMaster()

   226

{

   227

    ec_ioctl_master_t data;

   228

    stringstream err;

   229

    unsigned int i;

   230

   231

    open(Read);

   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

    open(Read);

   281

   282

    if (slavePosition == -1) {

   283

        unsigned int numSlaves = slaveCount(), i;

   284

   285

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

   286

            listSlavePdos(i, quiet, true);

   287

        }

   288

    } else {

   289

        listSlavePdos(slavePosition, quiet, false);

   290

    }

   291

}

   292

   293

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

   294

   295

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

   296

{

   297

    open(Read);

   298

   299

    if (slavePosition == -1) {

   300

        unsigned int numSlaves = slaveCount(), i;

   301

   302

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

   303

            listSlaveSdos(i, quiet, true);

   304

        }

   305

    } else {

   306

        listSlaveSdos(slavePosition, quiet, false);

   307

    }

   308

}

   309

   310

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

   311

   312

void Master::sdoDownload(

   313

        int slavePosition,

   314

        const string &dataTypeStr,

   315

        const vector<string> &commandArgs

   316

        )

   317

{

   318

    stringstream strIndex, strSubIndex, strValue, err;

   319

    int number, sval;

   320

    ec_ioctl_sdo_download_t data;

   321

    unsigned int i, uval;

   322

    const CoEDataType *dataType = NULL;

   323

   324

    if (slavePosition < 0) {

   325

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

   326

        throw MasterException(err.str());

   327

    }

   328

    data.slave_position = slavePosition;

   329

   330

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

   331

        err << "'sdo_download' takes 3 arguments!";

   332

        throw MasterException(err.str());

   333

    }

   334

   335

    strIndex << commandArgs[0];

   336

    strIndex >> hex >> number;

   337

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

   338

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

   339

        throw MasterException(err.str());

   340

    }

   341

    data.sdo_index = number;

   342

   343

    strSubIndex << commandArgs[1];

   344

    strSubIndex >> hex >> number;

   345

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

   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

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

   354

            throw MasterException(err.str());

   355

        }

   356

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

   357

        ec_ioctl_sdo_entry_t entry;

   358

        unsigned int entryByteSize;

   359

   360

        open(Read);

   361

   362

        try {

   363

            getSdoEntry(&entry, slavePosition,

   364

                    data.sdo_index, data.sdo_entry_subindex);

   365

        } catch (MasterException &e) {

   366

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

   367

                << "Please specify --type.";

   368

            throw MasterException(err.str());

   369

        }

   370

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

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

   380

    } else {

   381

        data.data_size = DefaultBufferSize;

   382

    }

   383

   384

    data.data = new uint8_t[data.data_size + 1];

   385

   386

    strValue << commandArgs[2];

   387

   388

    switch (dataType->coeCode) {

   389

        case 0x0002: // int8

   390

            strValue >> sval;

   391

            if ((uint32_t) sval > 0xff) {

   392

                delete [] data.data;

   393

                err << "Invalid value for type '"

   394

                    << dataType->name << "'!";

   395

                throw MasterException(err.str());

   396

            }

   397

            *data.data = (int8_t) sval;

   398

            break;

   399

        case 0x0003: // int16

   400

            strValue >> sval;

   401

            if ((uint32_t) sval > 0xffff) {

   402

                delete [] data.data;

   403

                err << "Invalid value for type '"

   404

                    << dataType->name << "'!";

   405

                throw MasterException(err.str());

   406

            }

   407

            *(int16_t *) data.data = cputole16(sval);

   408

            break;

   409

        case 0x0004: // int32

   410

            strValue >> sval;

   411

            *(int32_t *) data.data = cputole32(sval);

   412

            break;

   413

        case 0x0005: // uint8

   414

            strValue >> uval;

   415

            if ((uint32_t) uval > 0xff) {

   416

                delete [] data.data;

   417

                err << "Invalid value for type '"

   418

                    << dataType->name << "'!";

   419

                throw MasterException(err.str());

   420

            }

   421

            *data.data = (uint8_t) uval;

   422

            break;

   423

        case 0x0006: // uint16

   424

            strValue >> uval;

   425

            if ((uint32_t) uval > 0xffff) {

   426

                delete [] data.data;

   427

                err << "Invalid value for type '"

   428

                    << dataType->name << "'!";

   429

                throw MasterException(err.str());

   430

            }

   431

            *(uint16_t *) data.data = cputole16(uval);

   432

            break;

   433

        case 0x0007: // uint32

   434

            strValue >> uval;

   435

            *(uint32_t *) data.data = cputole32(uval);

   436

            break;

   437

        case 0x0009: // string

   438

            if (strValue.str().size() >= data.data_size) {

   439

                err << "String too big";

   440

                throw MasterException(err.str());

   441

            }

   442

            data.data_size = strValue.str().size();

   443

            strValue >> (char *) data.data;

   444

            break;

   445

        default:

   446

            break;

   447

    }

   448

   449

    if (strValue.fail()) {

   450

        err << "Invalid value argument '" << commandArgs[2] << "'!";

   451

        throw MasterException(err.str());

   452

    }

   453

   454

    open(ReadWrite);

   455

   456

    if (ioctl(fd, EC_IOCTL_SDO_DOWNLOAD, &data) < 0) {

   457

        stringstream err;

   458

        err << "Failed to download Sdo: " << strerror(errno);

   459

        delete [] data.data;

   460

        throw MasterException(err.str());

   461

    }

   462

   463

    delete [] data.data;

   464

}

   465

   466

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

   467

   468

void Master::sdoUpload(

   469

        int slavePosition,

   470

        const string &dataTypeStr,

   471

        const vector<string> &commandArgs

   472

        )

   473

{

   474

    stringstream strIndex, strSubIndex;

   475

    int number, sval;

   476

    ec_ioctl_sdo_upload_t data;

   477

    unsigned int i, uval;

   478

    const CoEDataType *dataType = NULL;

   479

   480

    if (slavePosition < 0) {

   481

        stringstream err;

   482

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

   483

        throw MasterException(err.str());

   484

    }

   485

    data.slave_position = slavePosition;

   486

   487

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

   488

        stringstream err;

   489

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

   490

        throw MasterException(err.str());

   491

    }

   492

   493

    strIndex << commandArgs[0];

   494

    strIndex >> hex >> number;

   495

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

   496

        stringstream err;

   497