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];

   145

    strAlias >> hex >> number;

   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) {

   156

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

   157

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

   158

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

   159

            throw MasterException(err.str());

   160

        }

   161

   162

        open(ReadWrite);

   163

        numSlaves = slaveCount();

   164

   165

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

   166

            writeSlaveAlias(i, alias);

   167

        }

   168

    } else {

   169

        open(ReadWrite);

   170

        writeSlaveAlias(slavePosition, alias);

   171

    }

   172

}

   173

   174

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

   175

   176

void Master::outputData(int domainIndex)

   177

{

   178

    open(Read);

   179

   180

    if (domainIndex == -1) {

   181

        unsigned int numDomains = domainCount(), i;

   182

   183

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

   184

            outputDomainData(i);

   185

        }

   186

    } else {

   187

        outputDomainData(domainIndex);

   188

    }

   189

}

   190

   191

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

   192

   193

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

   194

{

   195

    stringstream str;

   196

    int debugLevel;

   197

   198

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

   199

        stringstream err;

   200

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

   201

        throw MasterException(err.str());

   202

    }

   203

   204

    str << commandArgs[0];

   205

    str >> debugLevel;

   206

   207

    if (str.fail()) {

   208

        stringstream err;

   209

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

   210

        throw MasterException(err.str());

   211

    }

   212

   213

    open(ReadWrite);

   214

   215

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

   216

        stringstream err;

   217

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

   218

        throw MasterException(err.str());

   219

    }

   220

}

   221

   222

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

   223

   224

void Master::showDomains(int domainIndex)

   225

{

   226

    open(Read);

   227

   228

    if (domainIndex == -1) {

   229

        unsigned int numDomains = domainCount(), i;

   230

   231

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

   232

            showDomain(i);

   233

        }

   234

    } else {

   235

        showDomain(domainIndex);

   236

    }

   237

}

   238

   239

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

   240

   241

struct SlaveInfo {

   242

    string pos;

   243

    string alias;

   244

    string relPos;

   245

    string state;

   246

    string flag;

   247

    string name;

   248

};

   249

   250

void Master::listSlaves()

   251

{

   252

    unsigned int numSlaves, i;

   253

    ec_ioctl_slave_t slave;

   254

    uint16_t lastAlias, aliasIndex;

   255

    SlaveInfo slaveInfo;

   256

    typedef list<SlaveInfo> SlaveInfoList;

   257

    SlaveInfoList slaveInfoList;

   258

    SlaveInfoList::const_iterator iter;

   259

    stringstream str;

   260

    unsigned int maxPosWidth = 0, maxAliasWidth = 0,

   261

                 maxRelPosWidth = 0, maxStateWidth = 0;

   262

   263

    open(Read);

   264

   265

    numSlaves = slaveCount();

   266

   267

    lastAlias = 0;

   268

    aliasIndex = 0;

   269

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

   270

        getSlave(&slave, i);

   271

   272

        str << dec << i;

   273

        slaveInfo.pos = str.str();

   274

        str.clear();

   275

        str.str("");

   276

   277

        if (slave.alias) {

   278

            lastAlias = slave.alias;

   279

            aliasIndex = 0;

   280

        }

   281

        if (lastAlias) {

   282

            str << "#" << hex << lastAlias;

   283

            slaveInfo.alias = str.str();

   284

            str.str("");

   285

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

   286

            slaveInfo.relPos = str.str();

   287

            str.str("");

   288

            aliasIndex++;

   289

        } else {

   290

            slaveInfo.alias = "";

   291

            slaveInfo.relPos = "";

   292

        }

   293

   294

        slaveInfo.state = slaveState(slave.state);

   295

        slaveInfo.flag = (slave.error_flag ? 'E' : '+');

   296

   297

        if (strlen(slave.name)) {

   298

            slaveInfo.name = slave.name;

   299

        } else {

   300

            str << hex << setfill('0')

   301

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

   302

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

   303

            slaveInfo.name = str.str();

   304

            str.str("");

   305

        }

   306

   307

        slaveInfoList.push_back(slaveInfo);

   308

        if (slaveInfo.pos.length() > maxPosWidth)

   309

            maxPosWidth = slaveInfo.pos.length();

   310

        if (slaveInfo.alias.length() > maxAliasWidth)

   311

            maxAliasWidth = slaveInfo.alias.length();

   312

        if (slaveInfo.relPos.length() > maxRelPosWidth)

   313

            maxRelPosWidth = slaveInfo.relPos.length();

   314

        if (slaveInfo.state.length() > maxStateWidth)

   315

            maxStateWidth = slaveInfo.state.length();

   316

    }

   317

   318

    for (iter = slaveInfoList.begin(); iter != slaveInfoList.end(); iter++) {

   319

        cout << setfill(' ') << right

   320

            << setw(maxPosWidth) << iter->pos << "  "

   321

            << setw(maxAliasWidth) << iter->alias

   322

            << left

   323

            << setw(maxRelPosWidth) << iter->relPos << "  "

   324

            << setw(maxStateWidth) << iter->state << "  "

   325

            << iter->flag << "  "

   326

            << iter->name << endl;

   327

    }

   328

}

   329

   330

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

   331

   332

void Master::showMaster()

   333

{

   334

    ec_ioctl_master_t data;

   335

    stringstream err;

   336

    unsigned int i;

   337

   338

    open(Read);

   339

    getMaster(&data);

   340

   341

    cout

   342

        << "Master" << index << endl

   343

        << "  State: ";

   344

   345

    switch (data.mode) {

   346

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

   347

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

   348

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

   349

        default:

   350

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

   351

                throw MasterException(err.str());

   352

    }

   353

   354

    cout << endl

   355

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

   356

   357

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

   358

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

   359

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

   360

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

   361

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

   362

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

   363

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

   364

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

   365

            cout << "None.";

   366

        } else {

   367

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

   368

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

   369

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

   370

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

   371

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

   372

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

   373

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

   374

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

   375

                << ")" << endl << dec

   376

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

   377

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

   378

        }

   379

        cout << endl;

   380

    }

   381

}

   382

   383

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

   384

   385

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

   386

{

   387

    open(Read);

   388

   389

    if (slavePosition == -1) {

   390

        unsigned int numSlaves = slaveCount(), i;

   391

   392

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

   393

            listSlavePdos(i, quiet, true);

   394

        }

   395

    } else {

   396

        listSlavePdos(slavePosition, quiet, false);

   397

    }

   398

}

   399

   400

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

   401

   402

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

   403

{

   404

    open(Read);

   405

   406

    if (slavePosition == -1) {

   407

        unsigned int numSlaves = slaveCount(), i;

   408

   409

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

   410

            listSlaveSdos(i, quiet, true);

   411

        }

   412

    } else {

   413

        listSlaveSdos(slavePosition, quiet, false);

   414

    }

   415

}

   416

   417

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

   418

   419

void Master::sdoDownload(

   420

        int slavePosition,

   421

        const string &dataTypeStr,

   422

        const vector<string> &commandArgs

   423

        )

   424

{

   425

    stringstream strIndex, strSubIndex, strValue, err;

   426

    int number, sval;

   427

    ec_ioctl_sdo_download_t data;

   428

    unsigned int i, uval;

   429

    const CoEDataType *dataType = NULL;

   430

   431

    if (slavePosition < 0) {

   432

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

   433

        throw MasterException(err.str());

   434

    }

   435

    data.slave_position = slavePosition;

   436

   437

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

   438

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

   439

        throw MasterException(err.str());

   440

    }

   441

   442

    strIndex << commandArgs[0];

   443

    strIndex >> hex >> number;

   444

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

   445

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

   446

        throw MasterException(err.str());

   447

    }

   448

    data.sdo_index = number;

   449

   450

    strSubIndex << commandArgs[1];

   451

    strSubIndex >> hex >> number;

   452

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

   453

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

   454

        throw MasterException(err.str());

   455

    }

   456

    data.sdo_entry_subindex = number;

   457

   458

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

   459

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

   460

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

   461

            throw MasterException(err.str());

   462

        }

   463

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

   464

        ec_ioctl_sdo_entry_t entry;

   465

        unsigned int entryByteSize;

   466

   467

        open(ReadWrite);

   468

   469

        try {

   470

            getSdoEntry(&entry, slavePosition,

   471

                    data.sdo_index, data.sdo_entry_subindex);

   472

        } catch (MasterException &e) {

   473

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

   474

                << "Please specify --type.";

   475

            throw MasterException(err.str());

   476

        }

   477

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

   478

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

   479

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

   480

                << " Please specify --type.";

   481

            throw MasterException(err.str());

   482

        }

   483

    }

   484

   485

    if (dataType->byteSize) {

   486

        data.data_size = dataType->byteSize;

   487

    } else {

   488

        data.data_size = DefaultBufferSize;

   489

    }

   490

   491

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

   492

   493

    strValue << commandArgs[2];

   494

   495

    switch (dataType->coeCode) {