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

/**

   177

 * Lists the bus configuration.

   178

 */

   179

void Master::showConfig()

   180

{

   181

    ec_ioctl_master_t master;

   182

    unsigned int i, j, k, l;

   183

    ec_ioctl_config_t config;

   184

    ec_ioctl_config_pdo_t pdo;

   185

    ec_ioctl_config_pdo_entry_t entry;

   186

    ec_ioctl_config_sdo_t sdo;

   187

   188

    open(Read);

   189

    getMaster(&master);

   190

   191

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

   192

        getConfig(&config, i);

   193

   194

        cout << "Alias: 0x"

   195

            << hex << setfill('0') << setw(4) << config.alias << endl

   196

            << "Position: " << dec << config.position << endl

   197

            << "Vendor Id: 0x"

   198

            << hex << setw(8) << config.vendor_id << endl

   199

            << "Product code: 0x"

   200

            << hex << setw(8) << config.product_code << endl

   201

            << "Attached: " << (config.attached ? "yes" : "no") << endl;

   202

   203

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

   204

            if (config.pdo_count[j]) {

   205

                cout << (j ? "Input" : "Output")

   206

                    << " Pdo assignment / mapping " << endl;

   207

                for (k = 0; k < config.pdo_count[j]; k++) {

   208

                    getConfigPdo(&pdo, i, j, k);

   209

   210

                    cout << "  Pdo 0x"

   211

                        << hex << setfill('0') << setw(4) << pdo.index

   212

                        << " \"" << pdo.name << "\"" << endl;

   213

   214

                    for (l = 0; l < pdo.entry_count; l++) {

   215

                        getConfigPdoEntry(&entry, i, j, k, l);

   216

   217

                        cout << "    Pdo entry 0x"

   218

                            << hex << setfill('0') << setw(4) << entry.index

   219

                            << ":" << setw(2) << (unsigned int) entry.subindex

   220

                            << ", " << dec << (unsigned int) entry.bit_length

   221

                            << " bit, \"" << entry.name << "\"" << endl;

   222

                    }

   223

                }

   224

            }

   225

        }

   226

   227

        if (config.sdo_count) {

   228

            cout << "Sdo configuration:" << endl;

   229

            for (j = 0; j < config.sdo_count; j++) {

   230

                getConfigSdo(&sdo, i, j);

   231

   232

                cout << "  0x"

   233

                    << hex << setfill('0') << setw(4) << sdo.index

   234

                    << ":" << setw(2) << (unsigned int) sdo.subindex

   235

                    << ", " << sdo.size << " byte: " << hex;

   236

   237

                switch (sdo.size) {

   238

                    case 1:

   239

                        cout << "0x" << setw(2)

   240

                            << (unsigned int) *(uint8_t *) &sdo.data;

   241

                        break;

   242

                    case 2:

   243

                        cout << "0x" << setw(4)

   244

                            << le16tocpu(*(uint16_t *) &sdo.data);

   245

                        break;

   246

                    case 4:

   247

                        cout << "0x" << setw(8)

   248

                            << le32tocpu(*(uint32_t *) &sdo.data);

   249

                        break;

   250

                    default:

   251

                        cout << "???";

   252

                }

   253

   254

                cout << endl;

   255

            }

   256

        }

   257

   258

        cout << endl;

   259

    }

   260

}

   261

   262

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

   263

   264

void Master::outputData(int domainIndex)

   265

{

   266

    open(Read);

   267

   268

    if (domainIndex == -1) {

   269

        unsigned int numDomains = domainCount(), i;

   270

   271

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

   272

            outputDomainData(i);

   273

        }

   274

    } else {

   275

        outputDomainData(domainIndex);

   276

    }

   277

}

   278

   279

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

   280

   281

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

   282

{

   283

    stringstream str;

   284

    int debugLevel;

   285

   286

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

   287

        stringstream err;

   288

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

   289

        throw MasterException(err.str());

   290

    }

   291

   292

    str << commandArgs[0];

   293

    str >> debugLevel;

   294

   295

    if (str.fail()) {

   296

        stringstream err;

   297

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

   298

        throw MasterException(err.str());

   299

    }

   300

   301

    open(ReadWrite);

   302

   303

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

   304

        stringstream err;

   305

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

   306

        throw MasterException(err.str());

   307

    }

   308

}

   309

   310

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

   311

   312

void Master::showDomains(int domainIndex)

   313

{

   314

    open(Read);

   315

   316

    if (domainIndex == -1) {

   317

        unsigned int numDomains = domainCount(), i;

   318

   319

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

   320

            showDomain(i);

   321

        }

   322

    } else {

   323

        showDomain(domainIndex);

   324

    }

   325

}

   326

   327

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

   328

   329

struct SlaveInfo {

   330

    string pos;

   331

    string alias;

   332

    string relPos;

   333

    string state;

   334

    string flag;

   335

    string name;

   336

};

   337

   338

void Master::listSlaves()

   339

{

   340

    unsigned int numSlaves, i;

   341

    ec_ioctl_slave_t slave;

   342

    uint16_t lastAlias, aliasIndex;

   343

    SlaveInfo slaveInfo;

   344

    typedef list<SlaveInfo> SlaveInfoList;

   345

    SlaveInfoList slaveInfoList;

   346

    SlaveInfoList::const_iterator iter;

   347

    stringstream str;

   348

    unsigned int maxPosWidth = 0, maxAliasWidth = 0,

   349

                 maxRelPosWidth = 0, maxStateWidth = 0;

   350

   351

    open(Read);

   352

   353

    numSlaves = slaveCount();

   354

   355

    lastAlias = 0;

   356

    aliasIndex = 0;

   357

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

   358

        getSlave(&slave, i);

   359

   360

        str << dec << i;

   361

        slaveInfo.pos = str.str();

   362

        str.clear();

   363

        str.str("");

   364

   365

        if (slave.alias) {

   366

            lastAlias = slave.alias;

   367

            aliasIndex = 0;

   368

        }

   369

        if (lastAlias) {

   370

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

   371

            slaveInfo.alias = str.str();

   372

            str.str("");

   373

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

   374

            slaveInfo.relPos = str.str();

   375

            str.str("");

   376

            aliasIndex++;

   377

        } else {

   378

            slaveInfo.alias = "";

   379

            slaveInfo.relPos = "";

   380

        }

   381

   382

        slaveInfo.state = slaveState(slave.state);

   383

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

   384

   385

        if (strlen(slave.name)) {

   386

            slaveInfo.name = slave.name;

   387

        } else {

   388

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

   389

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

   390

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

   391

            slaveInfo.name = str.str();

   392

            str.str("");

   393

        }

   394

   395

        slaveInfoList.push_back(slaveInfo);

   396

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

   397

            maxPosWidth = slaveInfo.pos.length();

   398

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

   399

            maxAliasWidth = slaveInfo.alias.length();

   400

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

   401

            maxRelPosWidth = slaveInfo.relPos.length();

   402

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

   403

            maxStateWidth = slaveInfo.state.length();

   404

    }

   405

   406

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

   407

        cout << setfill(' ') << right

   408

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

   409

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

   410

            << left

   411

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

   412

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

   413

            << iter->flag << "  "

   414

            << iter->name << endl;

   415

    }

   416

}

   417

   418

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

   419

   420

void Master::showMaster()

   421

{

   422

    ec_ioctl_master_t data;

   423

    stringstream err;

   424

    unsigned int i;

   425

   426

    open(Read);

   427

    getMaster(&data);

   428

   429

    cout

   430

        << "Master" << index << endl

   431

        << "  State: ";

   432

   433

    switch (data.mode) {

   434

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

   435

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

   436

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

   437

        default:

   438

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

   439

                throw MasterException(err.str());

   440

    }

   441

   442

    cout << endl

   443

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

   444

   445

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

   446

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

   447

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

   448

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

   449

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

   450

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

   451

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

   452

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

   453

            cout << "None.";

   454

        } else {

   455

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

   456

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

   457

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

   458

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

   459

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

   460

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

   461

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

   462

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

   463

                << ")" << endl << dec

   464

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

   465

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

   466

        }

   467

        cout << endl;

   468

    }

   469

}

   470

   471

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

   472

   473

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

   474

{

   475

    open(Read);

   476

   477

    if (slavePosition == -1) {

   478

        unsigned int numSlaves = slaveCount(), i;

   479

   480

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

   481

            listSlavePdos(i, quiet, true);

   482

        }

   483

    } else {

   484

        listSlavePdos(slavePosition, quiet, false);

   485

    }

   486

}

   487

   488

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

   489

   490

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

   491

{

   492

    open(Read);

   493

   494

    if (slavePosition == -1) {

   495

        unsigned int numSlaves = slaveCount(), i;

   496