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 <fstream>

    17

#include <cctype> // toupper()

    18

using namespace std;

    19

    20

#include "Master.h"

    21

    22

#define swap16(x) \

    23

        ((uint16_t)( \

    24

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

    25

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

    26

#define swap32(x) \

    27

        ((uint32_t)( \

    28

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

    29

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

    30

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

    31

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

    32

    33

#if __BYTE_ORDER == __LITTLE_ENDIAN

    34

    35

#define le16tocpu(x) x

    36

#define le32tocpu(x) x

    37

    38

#define cputole16(x) x

    39

#define cputole32(x) x

    40

    41

#elif __BYTE_ORDER == __BIG_ENDIAN

    42

    43

#define le16tocpu(x) swap16(x)

    44

#define le32tocpu(x) swap32(x)

    45

    46

#define cputole16(x) swap16(x)

    47

#define cputole32(x) swap32(x)

    48

    49

#endif

    50

    51

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

    52

    53

struct CoEDataType {

    54

    const char *name;

    55

    uint16_t coeCode;

    56

    unsigned int byteSize;

    57

};

    58

    59

static const CoEDataType dataTypes[] = {

    60

    {"int8",   0x0002, 1},

    61

    {"int16",  0x0003, 2},

    62

    {"int32",  0x0004, 4},

    63

    {"uint8",  0x0005, 1},

    64

    {"uint16", 0x0006, 2},

    65

    {"uint32", 0x0007, 4},

    66

    {"string", 0x0009, 0},

    67

    {"raw",    0xffff, 0},

    68

    {}

    69

};

    70

    71

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

    72

    73

const CoEDataType *findDataType(const string &str)

    74

{

    75

    const CoEDataType *d;

    76

    77

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

    78

        if (str == d->name)

    79

            return d;

    80

    81

    return NULL;

    82

}

    83

    84

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

    85

    86

const CoEDataType *findDataType(uint16_t code)

    87

{

    88

    const CoEDataType *d;

    89

    90

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

    91

        if (code == d->coeCode)

    92

            return d;

    93

    94

    return NULL;

    95

}

    96

    97

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

    98

    99

Master::Master()

   100

{

   101

    index = 0;

   102

    fd = -1;

   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

/**

   122

 * Writes the Secondary slave address (alias) to the slave's SII.

   123

 */

   124

void Master::writeAlias(

   125

        int slavePosition,

   126

        bool force,

   127

        const vector<string> &commandArgs

   128

        )

   129

{

   130

    ec_ioctl_sii_t data;

   131

    ec_ioctl_slave_t slave;

   132

    unsigned int i;

   133

    uint16_t alias;

   134

    stringstream err, strAlias;

   135

    int number;

   136

   137

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

   138

        stringstream err;

   139

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

   140

        throw MasterException(err.str());

   141

    }

   142

   143

    strAlias << commandArgs[0];

   144

    strAlias >> hex >> number;

   145

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

   146

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

   147

        throw MasterException(err.str());

   148

    }

   149

    alias = number;

   150

   151

    if (slavePosition == -1) {

   152

        unsigned int numSlaves, i;

   153

   154

        if (!force) {

   155

            err << "This will write the alias addresses of all slaves to 0x"

   156

                << hex << setfill('0') << setw(4) << alias << "! "

   157

                << "Please specify --force to proceed.";

   158

            throw MasterException(err.str());

   159

        }

   160

   161

        open(ReadWrite);

   162

        numSlaves = slaveCount();

   163

   164

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

   165

            writeSlaveAlias(i, alias);

   166

        }

   167

    } else {

   168

        open(ReadWrite);

   169

        writeSlaveAlias(slavePosition, alias);

   170

    }

   171

}

   172

   173

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

   174

   175

void Master::outputData(int domainIndex)

   176

{

   177

    open(Read);

   178

   179

    if (domainIndex == -1) {

   180

        unsigned int numDomains = domainCount(), i;

   181

   182

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

   183

            outputDomainData(i);

   184

        }

   185

    } else {

   186

        outputDomainData(domainIndex);

   187

    }

   188

}

   189

   190

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

   191

   192

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

   193

{

   194

    stringstream str;

   195

    int debugLevel;

   196

   197

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

   198

        stringstream err;

   199

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

   200

        throw MasterException(err.str());

   201

    }

   202

   203

    str << commandArgs[0];

   204

    str >> debugLevel;

   205

   206

    if (str.fail()) {

   207

        stringstream err;

   208

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

   209

        throw MasterException(err.str());

   210

    }

   211

   212

    open(ReadWrite);

   213

   214

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

   215

        stringstream err;

   216

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

   217

        throw MasterException(err.str());

   218

    }

   219

}

   220

   221

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

   222

   223

void Master::showDomains(int domainIndex)

   224

{

   225

    open(Read);

   226

   227

    if (domainIndex == -1) {

   228

        unsigned int numDomains = domainCount(), i;

   229

   230

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

   231

            showDomain(i);

   232

        }

   233

    } else {

   234

        showDomain(domainIndex);

   235

    }

   236

}

   237

   238

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

   239

   240

void Master::listSlaves()

   241

{

   242

    unsigned int numSlaves, i;

   243

    ec_ioctl_slave_t slave;

   244

    uint16_t lastAlias, aliasIndex;

   245

   246

    open(Read);

   247

   248

    numSlaves = slaveCount();

   249

   250

    lastAlias = 0;

   251

    aliasIndex = 0;

   252

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

   253

        getSlave(&slave, i);

   254

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

   255

   256

        if (slave.alias) {

   257

            lastAlias = slave.alias;

   258

            aliasIndex = 0;

   259

        }

   260

        if (lastAlias) {

   261

            cout << "#"

   262

                << hex << setfill('0') << setw(4) << lastAlias

   263

                << ":" << dec << aliasIndex;

   264

            aliasIndex++;

   265

        }

   266

   267

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

   268

   269

        if (strlen(slave.name)) {

   270

            cout << slave.name;

   271

        } else {

   272

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

   273

                << setw(8) << slave.vendor_id << ":"

   274

                << setw(8) << slave.product_code << dec;

   275

        }

   276

   277

        cout << endl;

   278

    }

   279

}

   280

   281

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

   282

   283

void Master::showMaster()

   284

{

   285

    ec_ioctl_master_t data;

   286

    stringstream err;

   287

    unsigned int i;

   288

   289

    open(Read);

   290

    getMaster(&data);

   291

   292

    cout

   293

        << "Master" << index << endl

   294

        << "  State: ";

   295

   296

    switch (data.mode) {

   297

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

   298

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

   299

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

   300

        default:

   301

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

   302

                throw MasterException(err.str());

   303

    }

   304

   305

    cout << endl

   306

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

   307

   308

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

   309

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

   310

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

   311

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

   312

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

   313

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

   314

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

   315

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

   316

            cout << "None.";

   317

        } else {

   318

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

   319

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

   320

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

   321

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

   322

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

   323

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

   324

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

   325

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

   326

                << ")" << endl << dec

   327

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

   328

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

   329

        }

   330

        cout << endl;

   331

    }

   332

}

   333

   334

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

   335

   336

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

   337

{

   338

    open(Read);

   339

   340

    if (slavePosition == -1) {

   341

        unsigned int numSlaves = slaveCount(), i;

   342

   343

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

   344

            listSlavePdos(i, quiet, true);

   345

        }

   346

    } else {

   347

        listSlavePdos(slavePosition, quiet, false);

   348

    }

   349

}

   350

   351

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

   352

   353

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

   354

{

   355

    open(Read);

   356

   357

    if (slavePosition == -1) {

   358

        unsigned int numSlaves = slaveCount(), i;

   359

   360

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

   361

            listSlaveSdos(i, quiet, true);

   362

        }

   363

    } else {

   364

        listSlaveSdos(slavePosition, quiet, false);

   365

    }

   366

}

   367

   368

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

   369

   370

void Master::sdoDownload(

   371

        int slavePosition,

   372

        const string &dataTypeStr,

   373

        const vector<string> &commandArgs

   374

        )

   375

{

   376

    stringstream strIndex, strSubIndex, strValue, err;

   377

    int number, sval;

   378

    ec_ioctl_sdo_download_t data;

   379

    unsigned int i, uval;

   380

    const CoEDataType *dataType = NULL;

   381

   382

    if (slavePosition < 0) {

   383

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

   384

        throw MasterException(err.str());

   385

    }

   386

    data.slave_position = slavePosition;

   387

   388

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

   389

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

   390

        throw MasterException(err.str());

   391

    }

   392

   393

    strIndex << commandArgs[0];

   394

    strIndex >> hex >> number;

   395

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

   396

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

   397

        throw MasterException(err.str());

   398

    }

   399

    data.sdo_index = number;

   400

   401

    strSubIndex << commandArgs[1];

   402

    strSubIndex >> hex >> number;

   403

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

   404

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

   405

        throw MasterException(err.str());

   406

    }

   407

    data.sdo_entry_subindex = number;

   408

   409

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

   410

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

   411

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

   412

            throw MasterException(err.str());

   413

        }

   414

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

   415

        ec_ioctl_sdo_entry_t entry;

   416

        unsigned int entryByteSize;

   417

   418

        open(ReadWrite);

   419

   420

        try {

   421

            getSdoEntry(&entry, slavePosition,

   422

                    data.sdo_index, data.sdo_entry_subindex);

   423

        } catch (MasterException &e) {

   424

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

   425

                << "Please specify --type.";

   426

            throw MasterException(err.str());

   427

        }

   428

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

   429

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

   430

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

   431

                << " Please specify --type.";

   432

            throw MasterException(err.str());

   433

        }

   434

    }

   435

   436

    if (dataType->byteSize) {

   437

        data.data_size = dataType->byteSize;

   438

    } else {

   439

        data.data_size = DefaultBufferSize;

   440

    }

   441

   442

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

   443

   444

    strValue << commandArgs[2];

   445

   446

    switch (dataType->coeCode) {

   447

        case 0x0002: // int8

   448

            strValue >> sval;

   449

            if ((uint32_t) sval > 0xff) {

   450

                delete [] data.data;

   451

                err << "Invalid value for type '"

   452

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

   453

                throw MasterException(err.str());

   454

            }

   455

            *data.data = (int8_t) sval;

   456

            break;

   457

        case 0x0003: // int16

   458

            strValue >> sval;

   459

            if ((uint32_t) sval > 0xffff) {

   460

                delete [] data.data;

   461

                err << "Invalid value for type '"

   462

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

   463

                throw MasterException(err.str());

   464

            }

   465

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

   466

            break;

   467

        case 0x0004: // int32

   468

            strValue >> sval;

   469

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

   470

            break;

   471

        case 0x0005: // uint8

   472

            strValue >> uval;

   473

            if ((uint32_t) uval > 0xff) {

   474

                delete [] data.data;

   475

                err << "Invalid value for type '"

   476

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

   477

                throw MasterException(err.str());

   478

            }

   479

            *data.data = (uint8_t) uval;

   480

            break;

   481

        case 0x0006: // uint16

   482

            strValue >> uval;

   483

            if ((uint32_t) uval > 0xffff) {

   484

                delete [] data.data;

   485

                err << "Invalid value for type '"

   486

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

   487

                throw MasterException(err.str());

   488

            }

   489

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

   490

            break;

   491

        case 0x0007: // uint32

   492

            strValue >> uval;

   493

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

   494

            break;

   495

        case 0x0009: // string

   496

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