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

    verbosity = Normal;

   104

    fd = -1;

   105

}

   106

   107

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

   108

   109

Master::~Master()

   110

{

   111

    close();

   112

}

   113

   114

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

   115

   116

void Master::setIndex(unsigned int i)

   117

{

   118

    index = i;

   119

}

   120

   121

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

   122

   123

void Master::setVerbosity(Verbosity v)

   124

{

   125

    verbosity = v;

   126

}

   127

   128

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

   129

   130

/**

   131

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

   132

 */

   133

void Master::writeAlias(

   134

        int slavePosition,

   135

        bool force,

   136

        const vector<string> &commandArgs

   137

        )

   138

{

   139

    uint16_t alias;

   140

    stringstream err, strAlias;

   141

    int number;

   142

   143

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

   144

        stringstream err;

   145

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

   146

        throw MasterException(err.str());

   147

    }

   148

   149

    strAlias << commandArgs[0];

   150

    strAlias

   151

        >> resetiosflags(ios::basefield) // guess base from prefix

   152

        >> number;

   153

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

   154

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

   155

        throw MasterException(err.str());

   156

    }

   157

    alias = number;

   158

   159

    if (slavePosition == -1) {

   160

        unsigned int numSlaves, i;

   161

   162

        if (!force) {

   163

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

   164

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

   165

            throw MasterException(err.str());

   166

        }

   167

   168

        open(ReadWrite);

   169

        numSlaves = slaveCount();

   170

   171

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

   172

            writeSlaveAlias(i, alias);

   173

        }

   174

    } else {

   175

        open(ReadWrite);

   176

        writeSlaveAlias(slavePosition, alias);

   177

    }

   178

}

   179

   180

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

   181

   182

bool operator<(const ec_ioctl_config_t &a, const ec_ioctl_config_t &b)

   183

{

   184

    return a.alias < b.alias

   185

        || (a.alias == b.alias && a.position < b.position);

   186

}

   187

   188

/** Lists the bus configuration.

   189

 */

   190

void Master::showConfigs()

   191

{

   192

    ec_ioctl_master_t master;

   193

    unsigned int i;

   194

    ec_ioctl_config_t config;

   195

    ConfigList configList;

   196

   197

    open(Read);

   198

    getMaster(&master);

   199

   200

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

   201

        getConfig(&config, i);

   202

        configList.push_back(config);

   203

    }

   204

   205

    configList.sort();

   206

   207

    if (verbosity == Verbose) {

   208

        showDetailedConfigs(configList);

   209

    } else {

   210

        listConfigs(configList);

   211

    }

   212

}

   213

   214

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

   215

   216

void Master::outputData(int domainIndex)

   217

{

   218

    open(Read);

   219

   220

    if (domainIndex == -1) {

   221

        unsigned int i;

   222

        ec_ioctl_master_t master;

   223

   224

        getMaster(&master);

   225

   226

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

   227

            outputDomainData(i);

   228

        }

   229

    } else {

   230

        outputDomainData(domainIndex);

   231

    }

   232

}

   233

   234

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

   235

   236

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

   237

{

   238

    stringstream str;

   239

    int debugLevel;

   240

   241

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

   242

        stringstream err;

   243

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

   244

        throw MasterException(err.str());

   245

    }

   246

   247

    str << commandArgs[0];

   248

    str >> resetiosflags(ios::basefield) // guess base from prefix

   249

        >> debugLevel;

   250

   251

    if (str.fail()) {

   252

        stringstream err;

   253

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

   254

        throw MasterException(err.str());

   255

    }

   256

   257

    open(ReadWrite);

   258

   259

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

   260

        stringstream err;

   261

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

   262

        throw MasterException(err.str());

   263

    }

   264

}

   265

   266

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

   267

   268

void Master::showDomains(int domainIndex)

   269

{

   270

    open(Read);

   271

   272

    if (domainIndex == -1) {

   273

        unsigned int i;

   274

        ec_ioctl_master_t master;

   275

   276

        getMaster(&master);

   277

   278

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

   279

            showDomain(i);

   280

        }

   281

    } else {

   282

        showDomain(domainIndex);

   283

    }

   284

}

   285

   286

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

   287

   288

void Master::showMaster()

   289

{

   290

    ec_ioctl_master_t data;

   291

    stringstream err;

   292

    unsigned int i;

   293

   294

    open(Read);

   295

    getMaster(&data);

   296

   297

    cout

   298

        << "Master" << index << endl

   299

        << "  Phase: ";

   300

   301

    switch (data.phase) {

   302

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

   303

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

   304

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

   305

        default:

   306

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

   307

                throw MasterException(err.str());

   308

    }

   309

   310

    cout << endl

   311

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

   312

   313

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

   314

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

   315

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

   316

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

   317

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

   318

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

   319

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

   320

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

   321

            cout << "None.";

   322

        } else {

   323

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

   324

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

   325

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

   326

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

   327

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

   328

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

   329

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

   330

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

   331

                << ")" << endl << dec

   332

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

   333

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

   334

        }

   335

        cout << endl;

   336

    }

   337

}

   338

   339

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

   340

   341

void Master::listPdos(int slavePosition)

   342

{

   343

    open(Read);

   344

   345

    if (slavePosition == -1) {

   346

        unsigned int numSlaves = slaveCount(), i;

   347

   348

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

   349

            listSlavePdos(i, true);

   350

        }

   351

    } else {

   352

        listSlavePdos(slavePosition, false);

   353

    }

   354

}

   355

   356

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

   357

   358

void Master::listSdos(int slavePosition)

   359

{

   360

    open(Read);

   361

   362

    if (slavePosition == -1) {

   363

        unsigned int numSlaves = slaveCount(), i;

   364

   365

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

   366

            listSlaveSdos(i, true);

   367

        }

   368

    } else {

   369

        listSlaveSdos(slavePosition, false);

   370

    }

   371

}

   372

   373

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

   374

   375

void Master::sdoDownload(

   376

        int slavePosition,

   377

        const string &dataTypeStr,

   378

        const vector<string> &commandArgs

   379

        )

   380

{

   381

    stringstream strIndex, strSubIndex, strValue, err;

   382

    ec_ioctl_slave_sdo_download_t data;

   383

    unsigned int number;

   384

    const CoEDataType *dataType = NULL;

   385

   386

    if (slavePosition < 0) {

   387

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

   388

        throw MasterException(err.str());

   389

    }

   390

    data.slave_position = slavePosition;

   391

   392

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

   393

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

   394

        throw MasterException(err.str());

   395

    }

   396

   397

    strIndex << commandArgs[0];

   398

    strIndex

   399

        >> resetiosflags(ios::basefield) // guess base from prefix

   400

        >> data.sdo_index;

   401

    if (strIndex.fail()) {

   402

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

   403

        throw MasterException(err.str());

   404

    }

   405

   406

    strSubIndex << commandArgs[1];

   407

    strSubIndex

   408

        >> resetiosflags(ios::basefield) // guess base from prefix

   409

        >> number;

   410

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

   411

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

   412

        throw MasterException(err.str());

   413

    }

   414

    data.sdo_entry_subindex = number;

   415

   416

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

   417

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

   418

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

   419

            throw MasterException(err.str());

   420

        }

   421

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

   422

        ec_ioctl_slave_sdo_entry_t entry;

   423

   424

        open(ReadWrite);

   425

   426

        try {

   427

            getSdoEntry(&entry, slavePosition,

   428

                    data.sdo_index, data.sdo_entry_subindex);

   429

        } catch (MasterException &e) {

   430

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

   431

                << "Please specify --type.";

   432

            throw MasterException(err.str());

   433

        }

   434

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

   435

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

   436

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

   437

                << " Please specify --type.";

   438

            throw MasterException(err.str());

   439

        }

   440

    }

   441

   442

    if (dataType->byteSize) {

   443

        data.data_size = dataType->byteSize;

   444

    } else {

   445

        data.data_size = DefaultBufferSize;

   446

    }

   447

   448

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

   449

   450

    strValue << commandArgs[2];

   451

    strValue >> resetiosflags(ios::basefield); // guess base from prefix

   452

    strValue.exceptions(ios::failbit);

   453

   454

    try {

   455

        switch (dataType->coeCode) {

   456

            case 0x0002: // int8

   457

                {

   458

                    int16_t val; // uint8_t is interpreted as char

   459

                    strValue >> val;

   460

                    if (val > 127 || val < -128)

   461

                        throw ios::failure("Value out of range");

   462

                    *data.data = val;

   463

                    break;

   464

                }

   465

            case 0x0003: // int16

   466

                {

   467

                    int16_t val;

   468

                    strValue >> val;

   469

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

   470

                    break;

   471

                }

   472

            case 0x0004: // int32

   473

                {

   474

                    int32_t val;

   475

                    strValue >> val;

   476

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

   477

                    break;

   478

                }

   479

            case 0x0005: // uint8

   480

                {

   481

                    uint16_t val; // uint8_t is interpreted as char

   482

                    strValue >> val;

   483

                    if (val > 0xff)

   484

                        throw ios::failure("Value out of range");

   485

                    *data.data = val;

   486

                    break;

   487

                }

   488

            case 0x0006: // uint16

   489

                {

   490

                    uint16_t val;

   491

                    strValue >> val;

   492

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

   493

                    break;

   494

                }