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 <cctype> // toupper()

    17

using namespace std;

    18

    19

#include "Master.h"

    20

    21

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

    22

    23

Master::Master()

    24

{

    25

    index = 0;

    26

    fd = -1;

    27

}

    28

    29

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

    30

    31

Master::~Master()

    32

{

    33

    close();

    34

}

    35

    36

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

    37

    38

void Master::open(unsigned int index)

    39

{

    40

    stringstream deviceName;

    41

    42

    Master::index = index;

    43

    44

    deviceName << "/dev/EtherCAT" << index;

    45

    46

    if ((fd = ::open(deviceName.str().c_str(), O_RDONLY)) == -1) {

    47

        stringstream err;

    48

        err << "Failed to open master device " << deviceName.str() << ": "

    49

            << strerror(errno);

    50

        throw MasterException(err.str());

    51

    }

    52

}

    53

    54

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

    55

    56

void Master::close()

    57

{

    58

    if (fd == -1)

    59

        return;

    60

    61

    ::close(fd);

    62

}

    63

    64

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

    65

    66

void Master::outputData(int domainIndex)

    67

{

    68

    if (domainIndex == -1) {

    69

        unsigned int numDomains = domainCount(), i;

    70

    71

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

    72

            outputDomainData(i);

    73

        }

    74

    } else {

    75

        outputDomainData(domainIndex);

    76

    }

    77

}

    78

    79

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

    80

    81

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

    82

{

    83

    stringstream str;

    84

    int debugLevel;

    85

    86

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

    87

        stringstream err;

    88

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

    89

        throw MasterException(err.str());

    90

    }

    91

    92

    str << commandArgs[0];

    93

    str >> debugLevel;

    94

    95

    if (str.fail()) {

    96

        stringstream err;

    97

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

    98

        throw MasterException(err.str());

    99

    }

   100

   101

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

   102

        stringstream err;

   103

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

   104

        throw MasterException(err.str());

   105

    }

   106

}

   107

   108

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

   109

   110

void Master::showDomains(int domainIndex)

   111

{

   112

    if (domainIndex == -1) {

   113

        unsigned int numDomains = domainCount(), i;

   114

   115

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

   116

            showDomain(i);

   117

        }

   118

    } else {

   119

        showDomain(domainIndex);

   120

    }

   121

}

   122

   123

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

   124

   125

void Master::listSlaves()

   126

{

   127

    unsigned int numSlaves = slaveCount(), i;

   128

    ec_ioctl_slave_t slave;

   129

    uint16_t lastAlias, aliasIndex;

   130

   131

    lastAlias = 0;

   132

    aliasIndex = 0;

   133

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

   134

        getSlave(&slave, i);

   135

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

   136

   137

        if (slave.alias) {

   138

            lastAlias = slave.alias;

   139

            aliasIndex = 0;

   140

        }

   141

        if (lastAlias) {

   142

            cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;

   143

        }

   144

   145

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

   146

   147

        if (strlen(slave.name)) {

   148

            cout << slave.name;

   149

        } else {

   150

            cout << "0x" << hex << setfill('0') << slave.vendor_id

   151

                << ":0x" << slave.product_code;

   152

        }

   153

   154

        cout << endl;

   155

    }

   156

}

   157

   158

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

   159

   160

void Master::showMaster()

   161

{

   162

    ec_ioctl_master_t data;

   163

    stringstream err;

   164

    unsigned int i;

   165

   166

    getMaster(&data);

   167

   168

    cout

   169

        << "Master" << index << endl

   170

        << "  State: ";

   171

   172

    switch (data.mode) {

   173

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

   174

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

   175

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

   176

        default:

   177

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

   178

                throw MasterException(err.str());

   179

    }

   180

   181

    cout << endl

   182

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

   183

   184

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

   185

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

   186

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

   187

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

   188

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

   189

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

   190

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

   191

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

   192

            cout << "None.";

   193

        } else {

   194

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

   195

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

   196

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

   197

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

   198

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

   199

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

   200

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

   201

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

   202

                << ")" << endl << dec

   203

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

   204

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

   205

        }

   206

        cout << endl;

   207

    }

   208

}

   209

   210

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

   211

   212

void Master::listPdos(int slavePosition)

   213

{

   214

    if (slavePosition == -1) {

   215

        unsigned int numSlaves = slaveCount(), i;

   216

   217

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

   218

            listSlavePdos(i, true);

   219

        }

   220

    } else {

   221

        listSlavePdos(slavePosition, false);

   222

    }

   223

}

   224

   225

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

   226

   227

void Master::requestStates(

   228

        int slavePosition,

   229

        const vector<string> &commandArgs

   230

        )

   231

{

   232

    string stateStr;

   233

    uint8_t state;

   234

   235

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

   236

        stringstream err;

   237

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

   238

        throw MasterException(err.str());

   239

    }

   240

   241

    stateStr = commandArgs[0];

   242

    transform(stateStr.begin(), stateStr.end(),

   243

            stateStr.begin(), (int (*) (int)) std::toupper);

   244

   245

    if (stateStr == "INIT") {

   246

        state = 0x01;

   247

    } else if (stateStr == "PREOP") {

   248

        state = 0x02;

   249

    } else if (stateStr == "SAFEOP") {

   250

        state = 0x04;

   251

    } else if (stateStr == "OP") {

   252

        state = 0x08;

   253

    } else {

   254

        stringstream err;

   255

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

   256

        throw MasterException(err.str());

   257

    }

   258

   259

    if (slavePosition == -1) {

   260

        unsigned int i, numSlaves = slaveCount();

   261

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

   262

            requestState(i, state);

   263

    } else {

   264

        requestState(slavePosition, state);

   265

    }

   266

}

   267

   268

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

   269

   270

void Master::generateXml(int slavePosition)

   271

{

   272

    if (slavePosition == -1) {

   273

        unsigned int numSlaves = slaveCount(), i;

   274

   275

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

   276

            generateSlaveXml(i);

   277

        }

   278

    } else {

   279

        generateSlaveXml(slavePosition);

   280

    }

   281

}

   282

   283

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

   284

   285

void Master::outputDomainData(unsigned int domainIndex)

   286

{

   287

    ec_ioctl_domain_t domain;

   288

    ec_ioctl_data_t data;

   289

    unsigned char *processData;

   290

    unsigned int i;

   291

   292

    getDomain(&domain, domainIndex);

   293

   294

    if (!domain.data_size)

   295

        return;

   296

   297

    processData = new unsigned char[domain.data_size];

   298

   299

    try {

   300

        getData(&data, domainIndex, domain.data_size, processData);

   301

    } catch (MasterException &e) {

   302

        delete [] processData;

   303

        throw e;

   304

    }

   305

   306

    for (i = 0; i < data.data_size; i++)

   307

        cout << processData[i];

   308

    cout.flush();

   309

   310

    delete [] processData;

   311

}

   312

   313

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

   314

   315

void Master::showDomain(unsigned int domainIndex)

   316

{

   317

    ec_ioctl_domain_t domain;

   318

    unsigned char *processData;

   319

    ec_ioctl_data_t data;

   320

    unsigned int i, j;

   321

    ec_ioctl_domain_fmmu_t fmmu;

   322

    unsigned int dataOffset;

   323

   324

    getDomain(&domain, domainIndex);

   325

   326

	cout << "Domain" << domainIndex << ":"

   327

		<< " LogBaseAddr 0x"

   328

		<< hex << setfill('0') << setw(8) << domain.logical_base_address

   329

		<< ", Size " << dec << setfill(' ') << setw(3) << domain.data_size

   330

		<< ", WorkingCounter "

   331

		<< dec << domain.working_counter << "/"

   332

        << domain.expected_working_counter << endl;

   333

   334

    if (!domain.data_size)

   335

        return;

   336

   337

    processData = new unsigned char[domain.data_size];

   338

   339

    try {

   340

        getData(&data, domainIndex, domain.data_size, processData);

   341

    } catch (MasterException &e) {

   342

        delete [] processData;

   343

        throw e;

   344

    }

   345

   346

    for (i = 0; i < domain.fmmu_count; i++) {

   347

        getFmmu(&fmmu, domainIndex, i);

   348

   349

        cout << "  SlaveConfig "

   350

            << fmmu.slave_config_alias << ":" << fmmu.slave_config_position

   351

            << ", Dir "

   352

            << setfill(' ') << setw(3) << (fmmu.fmmu_dir ? "In" : "Out")

   353

            << ", LogAddr 0x" 

   354

            << hex << setfill('0') << setw(8) << fmmu.logical_address

   355

            << ", Size " << dec << fmmu.data_size << endl;

   356

   357

        dataOffset = fmmu.logical_address - domain.logical_base_address;

   358

        if (dataOffset + fmmu.data_size > domain.data_size) {

   359

            stringstream err;

   360

            err << "Fmmu information corrupted!";

   361

            delete [] processData;

   362

            throw MasterException(err.str());

   363

        }

   364

   365

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

   366

        for (j = 0; j < fmmu.data_size; j++) {

   367

            cout << setw(2)

   368

                << (unsigned int) *(processData + dataOffset + j) << " ";

   369

        }

   370

        cout << endl;

   371

    }

   372

   373

    delete [] processData;

   374

}

   375

   376

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

   377

   378

void Master::listSlavePdos(uint16_t slavePosition, bool printSlave)

   379

{

   380

    ec_ioctl_slave_t slave;

   381

    ec_ioctl_sync_t sync;

   382

    ec_ioctl_pdo_t pdo;

   383

    ec_ioctl_pdo_entry_t entry;

   384

    unsigned int i, j, k;

   385

   386

    getSlave(&slave, slavePosition);

   387

   388

    if (printSlave)

   389

        cout << "=== Slave " << slavePosition << " ===" << endl;

   390

   391

    for (i = 0; i < slave.sync_count; i++) {

   392

        getSync(&sync, slavePosition, i);

   393

   394

        cout << "SM" << i << ":"

   395

            << " PhysAddr 0x"

   396

            << hex << setfill('0') << setw(4) << sync.physical_start_address

   397

            << ", DefaultSize "

   398

            << dec << setfill(' ') << setw(4) << sync.default_size

   399

            << ", ControlRegister 0x"

   400

            << hex << setfill('0') << setw(2)

   401

            << (unsigned int) sync.control_register

   402

            << ", Enable " << dec << (unsigned int) sync.enable

   403

            << endl;

   404

   405

        for (j = 0; j < sync.pdo_count; j++) {

   406

            getPdo(&pdo, slavePosition, i, j);

   407

   408

            cout << "  " << (pdo.dir ? "T" : "R") << "xPdo 0x"

   409

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

   410

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

   411

   412

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

   413

                getPdoEntry(&entry, slavePosition, i, j, k);

   414

   415

                cout << "    Pdo entry 0x"

   416

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

   417

                    << ":" << hex << setfill('0') << setw(2)

   418

                    << (unsigned int) entry.subindex

   419

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

   420

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

   421

            }

   422

        }

   423

    }

   424

}

   425

   426

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

   427

   428

void Master::generateSlaveXml(uint16_t slavePosition)

   429

{

   430

    ec_ioctl_slave_t slave;

   431

    ec_ioctl_sync_t sync;

   432

    ec_ioctl_pdo_t pdo;

   433

    ec_ioctl_pdo_entry_t entry;

   434

    unsigned int i, j, k;

   435

   436

    getSlave(&slave, slavePosition);

   437

   438

    cout

   439

        << "<?xml version=\"1.0\" ?>" << endl

   440

        << "  <EtherCATInfo>" << endl

   441

        << "    <!-- Slave " << slave.position << " -->" << endl

   442

        << "    <Vendor>" << endl

   443

        << "      <Id>" << slave.vendor_id << "</Id>" << endl

   444

        << "    </Vendor>" << endl

   445

        << "    <Descriptions>" << endl

   446

        << "      <Devices>" << endl

   447

        << "        <Device>" << endl

   448

        << "          <Type ProductCode=\"#x"

   449

        << hex << setfill('0') << setw(8) << slave.product_code

   450

        << "\" RevisionNo=\"#x"

   451

        << hex << setfill('0') << setw(8) << slave.revision_number

   452

        << "\"/>" << endl;

   453

   454

    for (i = 0; i < slave.sync_count; i++) {

   455

        getSync(&sync, slavePosition, i);

   456

   457

        for (j = 0; j < sync.pdo_count; j++) {

   458

            getPdo(&pdo, slavePosition, i, j);

   459

   460

            cout

   461

                << "          <" << (pdo.dir ? "T" : "R") << "xPdo>" << endl

   462

                << "            <Index>#x"

   463

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

   464

                << "</Index>" << endl

   465

                << "            <Name>" << pdo.name << "</Name>" << endl;

   466

   467

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

   468

                getPdoEntry(&entry, slavePosition, i, j, k);

   469

   470

                cout

   471

                    << "            <Entry>" << endl

   472

                    << "              <Index>#x"

   473

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

   474

                    << "</Index>" << endl;

   475

                if (entry.index)

   476

                    cout

   477

                        << "              <SubIndex>"

   478

                        << dec << (unsigned int) entry.subindex

   479

                        << "</SubIndex>" << endl;

   480

   481

                cout

   482

                    << "              <BitLen>"

   483

                    << (unsigned int) entry.bit_length

   484

                    << "</BitLen>" << endl;

   485

   486

                if (entry.index) {

   487

                    cout

   488

                        << "              <Name>" << entry.name

   489

                        << "</Name>" << endl

   490

                        << "              <DataType>";

   491

   492

                    if (entry.bit_length == 1) {

   493

                        cout << "BOOL";

   494

                    } else if (!(entry.bit_length % 8)) {

   495

                        if (entry.bit_length <= 64)

   496

                            cout << "UINT" << (unsigned int) entry.bit_length;

   497

                        else

   498

                            cout << "STRING("

   499