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

#include <list>

    19

using namespace std;

    20

    21

#include "Master.h"

    22

    23

#define swap16(x) \

    24

        ((uint16_t)( \

    25

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

    26

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

    27

#define swap32(x) \

    28

        ((uint32_t)( \

    29

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

    30

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

    31

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

    32

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

    33

    34

#if __BYTE_ORDER == __LITTLE_ENDIAN

    35

    36

#define le16tocpu(x) x

    37

#define le32tocpu(x) x

    38

    39

#define cputole16(x) x

    40

#define cputole32(x) x

    41

    42

#elif __BYTE_ORDER == __BIG_ENDIAN

    43

    44

#define le16tocpu(x) swap16(x)

    45

#define le32tocpu(x) swap32(x)

    46

    47

#define cputole16(x) swap16(x)

    48

#define cputole32(x) swap32(x)

    49

    50

#endif

    51

    52

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

    53

    54

struct CoEDataType {

    55

    const char *name;

    56

    uint16_t coeCode;

    57

    unsigned int byteSize;

    58

};

    59

    60

static const CoEDataType dataTypes[] = {

    61

    {"int8",   0x0002, 1},

    62

    {"int16",  0x0003, 2},

    63

    {"int32",  0x0004, 4},

    64

    {"uint8",  0x0005, 1},

    65

    {"uint16", 0x0006, 2},

    66

    {"uint32", 0x0007, 4},

    67

    {"string", 0x0009, 0},

    68

    {"raw",    0xffff, 0},

    69

    {}

    70

};

    71

    72

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

    73

    74

const CoEDataType *findDataType(const string &str)

    75

{

    76

    const CoEDataType *d;

    77

    78

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

    79

        if (str == d->name)

    80

            return d;

    81

    82

    return NULL;

    83

}

    84

    85

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

    86

    87

const CoEDataType *findDataType(uint16_t code)

    88

{

    89

    const CoEDataType *d;

    90

    91

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

    92

        if (code == d->coeCode)

    93

            return d;

    94

    95

    return NULL;

    96

}

    97

    98

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

    99

   100

Master::Master()

   101

{

   102

    index = 0;

   103

    fd = -1;

   104

}

   105

   106

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

   107

   108

Master::~Master()

   109

{

   110

    close();

   111

}

   112

   113

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

   114

   115

void Master::setIndex(unsigned int i)

   116

{

   117

    index = i;

   118

}

   119

   120

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

   121

   122

/**

   123

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

   124

 */

   125

void Master::writeAlias(

   126

        int slavePosition,

   127

        bool force,

   128

        const vector<string> &commandArgs

   129

        )

   130

{

   131

    ec_ioctl_sii_t data;

   132

    ec_ioctl_slave_t slave;

   133

    unsigned int i;

   134

    uint16_t alias;

   135

    stringstream err, strAlias;

   136

    int number;

   137

   138

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

   139

        stringstream err;

   140

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

   141

        throw MasterException(err.str());

   142

    }

   143

   144

    strAlias << commandArgs[0];

   146

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

   147

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

   148

        throw MasterException(err.str());

   149

    }

   150

    alias = number;

   151

   152

    if (slavePosition == -1) {

   153

        unsigned int numSlaves, i;

   154

   155

        if (!force) {

   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

/** Lists the bus configuration.

   176

 */

   177

void Master::showConfigs(bool verbose)

   178

{

   179

    if (verbose) {

   180

        showConfigs();

   181

    } else {

   182

        listConfigs();

   183

    }

   184

}

   185

   186

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

   187

   188

void Master::outputData(int domainIndex)

   189

{

   190

    open(Read);

   191

   192

    if (domainIndex == -1) {

   193

        unsigned int i;

   194

        ec_ioctl_master_t master;

   195

   196

        getMaster(&master);

   197

   198

        for (i = 0; i < master.domain_count; i++) {

   199

            outputDomainData(i);

   200

        }

   201

    } else {

   202

        outputDomainData(domainIndex);

   203

    }

   204

}

   205

   206

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

   207

   208

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

   209

{

   210

    stringstream str;

   211

    int debugLevel;

   212

   213

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

   214

        stringstream err;

   215

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

   216

        throw MasterException(err.str());

   217

    }

   218

   219

    str << commandArgs[0];

   220

    str >> debugLevel;

   221

   222

    if (str.fail()) {

   223

        stringstream err;

   224

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

   225

        throw MasterException(err.str());

   226

    }

   227

   228

    open(ReadWrite);

   229

   230

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

   231

        stringstream err;

   232

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

   233

        throw MasterException(err.str());

   234

    }

   235

}

   236

   237

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

   238

   239

void Master::showDomains(int domainIndex)

   240

{

   241

    open(Read);

   242

   243

    if (domainIndex == -1) {

   244

        unsigned int i;

   245

        ec_ioctl_master_t master;

   246

   247

        getMaster(&master);

   248

   249

        for (i = 0; i < master.domain_count; i++) {

   250

            showDomain(i);

   251

        }

   252

    } else {

   253

        showDomain(domainIndex);

   254

    }

   255

}

   256

   257

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

   258

   259

void Master::showMaster()

   260

{

   261

    ec_ioctl_master_t data;

   262

    stringstream err;

   263

    unsigned int i;

   264

   265

    open(Read);

   266

    getMaster(&data);

   267

   268

    cout

   269

        << "Master" << index << endl

   270

        << "  Phase: ";

   271

   272

    switch (data.phase) {

   273

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

   274

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

   275

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

   276

        default:

   277

                err << "Invalid master phase " << data.phase;

   278

                throw MasterException(err.str());

   279

    }

   280

   281

    cout << endl

   282

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

   283

   284

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

   285

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

   286

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

   287

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

   288

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

   289

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

   290

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

   291

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

   292

            cout << "None.";

   293

        } else {

   294

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

   295

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

   296

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

   297

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

   298

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

   299

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

   300

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

   301

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

   302

                << ")" << endl << dec

   303

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

   304

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

   305

        }

   306

        cout << endl;

   307

    }

   308

}

   309

   310

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

   311

   312

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

   313

{

   314

    open(Read);

   315

   316

    if (slavePosition == -1) {

   317

        unsigned int numSlaves = slaveCount(), i;

   318

   319

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

   320

            listSlavePdos(i, quiet, true);

   321

        }

   322

    } else {

   323

        listSlavePdos(slavePosition, quiet, false);

   324

    }

   325

}

   326

   327

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

   328

   329

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

   330

{

   331

    open(Read);

   332

   333

    if (slavePosition == -1) {

   334

        unsigned int numSlaves = slaveCount(), i;

   335

   336

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

   337

            listSlaveSdos(i, quiet, true);

   338

        }

   339

    } else {

   340

        listSlaveSdos(slavePosition, quiet, false);

   341

    }

   342

}

   343

   344

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

   345

   346

void Master::sdoDownload(

   347

        int slavePosition,

   348

        const string &dataTypeStr,

   349

        const vector<string> &commandArgs

   350

        )

   351

{

   352

    stringstream strIndex, strSubIndex, strValue, err;

   353

    int number, sval;

   354

    ec_ioctl_sdo_download_t data;

   355

    unsigned int i, uval;

   356

    const CoEDataType *dataType = NULL;

   357

   358

    if (slavePosition < 0) {

   359

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

   360

        throw MasterException(err.str());

   361

    }

   362

    data.slave_position = slavePosition;

   363

   364

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

   365

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

   366

        throw MasterException(err.str());

   367

    }

   368

   369

    strIndex << commandArgs[0];

   370

    strIndex >> hex >> number;

   371

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

   372

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

   373

        throw MasterException(err.str());

   374

    }

   375

    data.sdo_index = number;

   376

   377

    strSubIndex << commandArgs[1];

   378

    strSubIndex >> hex >> number;

   379

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

   380

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

   381

        throw MasterException(err.str());

   382

    }

   383

    data.sdo_entry_subindex = number;

   384

   385

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

   386

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

   387

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

   388

            throw MasterException(err.str());

   389

        }

   390

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

   391

        ec_ioctl_sdo_entry_t entry;

   392

        unsigned int entryByteSize;

   393

   394

        open(ReadWrite);

   395

   396

        try {

   397

            getSdoEntry(&entry, slavePosition,

   398

                    data.sdo_index, data.sdo_entry_subindex);

   399

        } catch (MasterException &e) {

   400

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

   401

                << "Please specify --type.";

   402

            throw MasterException(err.str());

   403

        }

   404

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

   405

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

   406

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

   407

                << " Please specify --type.";

   408

            throw MasterException(err.str());

   409

        }

   410

    }

   411

   412

    if (dataType->byteSize) {

   413

        data.data_size = dataType->byteSize;

   414

    } else {

   415

        data.data_size = DefaultBufferSize;

   416

    }

   417

   418

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

   419

   420

    strValue << commandArgs[2];

   421

   422

    switch (dataType->coeCode) {

   423

        case 0x0002: // int8

   424

            strValue >> sval;

   425

            if ((uint32_t) sval > 0xff) {

   426

                delete [] data.data;

   427

                err << "Invalid value for type '"

   428

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

   429

                throw MasterException(err.str());

   430

            }

   431

            *data.data = (int8_t) sval;

   432

            break;

   433

        case 0x0003: // int16

   434

            strValue >> sval;

   435

            if ((uint32_t) sval > 0xffff) {

   436

                delete [] data.data;

   437

                err << "Invalid value for type '"

   438

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

   439

                throw MasterException(err.str());

   440

            }

   441

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

   442

            break;

   443

        case 0x0004: // int32

   444

            strValue >> sval;

   445

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

   446

            break;

   447

        case 0x0005: // uint8

   448

            strValue >> uval;

   449

            if ((uint32_t) uval > 0xff) {

   450

                delete [] data.data;

   451

                err << "Invalid value for type '"

   452

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

   453

                throw MasterException(err.str());

   454

            }

   455

            *data.data = (uint8_t) uval;

   456

            break;

   457

        case 0x0006: // uint16

   458

            strValue >> uval;

   459

            if ((uint32_t) uval > 0xffff) {

   460

                delete [] data.data;

   461

                err << "Invalid value for type '"

   462

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

   463

                throw MasterException(err.str());

   464

            }

   465

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

   466

            break;

   467

        case 0x0007: // uint32

   468

            strValue >> uval;

   469

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

   470

            break;

   471

        case 0x0009: // string

   472

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

   473

                err << "String too big";

   474

                throw MasterException(err.str());

   475

            }

   476

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

   477

            strValue >> (char *) data.data;

   478

            break;

   479

        default:

   480

            break;

   481

    }

   482

   483

    if (strValue.fail()) {

   484

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

   485

        throw MasterException(err.str());

   486

    }

   487

   488

    open(ReadWrite);

   489

   490

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

   491

        stringstream err;

   492

        err << "Failed to download Sdo: ";

   493

        if (errno == EIO && data.abort_code) {

   494

            err << "Abort code 0x" << hex << setfill('0') << setw(8)

   495

                << data.abort_code;