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

using namespace std;

    17

    18

#include "Master.h"

    19

    20

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

    21

    22

Master::Master()

    23

{

    24

    index = 0;

    25

    fd = -1;

    26

}

    27

    28

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

    29

    30

Master::~Master()

    31

{

    32

    close();

    33

}

    34

    35

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

    36

    37

void Master::open(unsigned int index)

    38

{

    39

    stringstream deviceName;

    40

    41

    Master::index = index;

    42

    43

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

    44

    45

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

    46

        stringstream err;

    47

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

    48

            << strerror(errno);

    49

        throw MasterException(err.str());

    50

    }

    51

}

    52

    53

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

    54

    55

void Master::close()

    56

{

    57

    if (fd == -1)

    58

        return;

    59

    60

    ::close(fd);

    61

}

    62

    63

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

    64

    65

void Master::listSlaves()

    66

{

    67

    unsigned int numSlaves = slaveCount(), i;

    68

    ec_ioctl_slave_t slave;

    69

    uint16_t lastAlias, aliasIndex;

    70

    71

    lastAlias = 0;

    72

    aliasIndex = 0;

    73

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

    74

        getSlave(&slave, i);

    75

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

    76

    77

        if (slave.alias) {

    78

            lastAlias = slave.alias;

    79

            aliasIndex = 0;

    80

        }

    81

        if (lastAlias) {

    82

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

    83

        }

    84

    85

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

    86

    87

        if (strlen(slave.name)) {

    88

            cout << slave.name;

    89

        } else {

    90

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

    91

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

    92

        }

    93

    94

        cout << endl;

    95

    }

    96

}

    97

    98

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

    99

   100

void Master::listPdos(int slavePosition)

   101

{

   102

    if (slavePosition == -1) {

   103

        unsigned int numSlaves = slaveCount(), i;

   104

   105

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

   106

            listSlavePdos(i, true);

   107

        }

   108

    } else {

   109

        listSlavePdos(slavePosition, false);

   110

    }

   111

}

   112

   113

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

   114

   115

void Master::generateXml(int slavePosition)

   116

{

   117

    if (slavePosition == -1) {

   118

        unsigned int numSlaves = slaveCount(), i;

   119

   120

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

   121

            generateSlaveXml(i);

   122

        }

   123

    } else {

   124

        generateSlaveXml(slavePosition);

   125

    }

   126

}

   127

   128

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

   129

   130

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

   131

{

   132

    ec_ioctl_slave_t slave;

   133

    ec_ioctl_sync_t sync;

   134

    ec_ioctl_pdo_t pdo;

   135

    ec_ioctl_pdo_entry_t entry;

   136

    unsigned int i, j, k;

   137

   138

    getSlave(&slave, slavePosition);

   139

   140

    if (printSlave)

   141

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

   142

   143

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

   144

        getSync(&sync, slavePosition, i);

   145

   146

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

   147

            << " PhysAddr 0x"

   148

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

   149

            << ", DefaultSize "

   150

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

   151

            << ", ControlRegister 0x"

   152

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

   153

            << (unsigned int) sync.control_register

   154

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

   155

            << endl;

   156

   157

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

   158

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

   159

   160

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

   161

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

   162

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

   163

   164

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

   165

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

   166

   167

                cout << "    Pdo entry 0x"

   168

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

   169

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

   170

                    << (unsigned int) entry.subindex

   171

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

   172

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

   173

            }

   174

        }

   175

    }

   176

}

   177

   178

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

   179

   180

void Master::generateSlaveXml(uint16_t slavePosition)

   181

{

   182

    ec_ioctl_slave_t slave;

   183

    ec_ioctl_sync_t sync;

   184

    ec_ioctl_pdo_t pdo;

   185

    ec_ioctl_pdo_entry_t entry;

   186

    unsigned int i, j, k;

   187

   188

    getSlave(&slave, slavePosition);

   189

   190

    cout

   191

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

   192

        << "  <EtherCATInfo>" << endl

   193

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

   194

        << "    <Vendor>" << endl

   195

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

   196

        << "    </Vendor>" << endl

   197

        << "    <Descriptions>" << endl

   198

        << "      <Devices>" << endl

   199

        << "        <Device>" << endl

   200

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

   201

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

   202

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

   203

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

   204

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

   205

   206

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

   207

        getSync(&sync, slavePosition, i);

   208

   209

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

   210

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

   211

   212

            cout

   213

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

   214

                << "            <Index>#x"

   215

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

   216

                << "</Index>" << endl

   217

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

   218

   219

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

   220

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

   221

   222

                cout

   223

                    << "            <Entry>" << endl

   224

                    << "              <Index>#x"

   225

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

   226

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

   227

                if (entry.index)

   228

                    cout

   229

                        << "              <SubIndex>"

   230

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

   231

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

   232

   233

                cout

   234

                    << "              <BitLen>"

   235

                    << (unsigned int) entry.bit_length

   236

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

   237

   238

                if (entry.index) {

   239

                    cout

   240

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

   241

                        << "</Name>" << endl

   242

                        << "              <DataType>";

   243

   244

                    if (entry.bit_length == 1) {

   245

                        cout << "BOOL";

   246

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

   247

                        if (entry.bit_length <= 64)

   248

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

   249

                        else

   250

                            cout << "STRING("

   251

                                << (unsigned int) (entry.bit_length / 8)

   252

                                << ")";

   253

                    } else {

   254

                        cerr << "Invalid bit length "

   255

                            << (unsigned int) entry.bit_length << endl;

   256

                    }

   257

   258

                        cout << "</DataType>" << endl;

   259

                }

   260

   261

                cout << "            </Entry>" << endl;

   262

            }

   263

   264

            cout

   265

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

   266

        }

   267

    }

   268

   269

    cout

   270

        << "        </Device>" << endl

   271

        << "     </Devices>" << endl

   272

        << "  </Descriptions>" << endl

   273

        << "</EtherCATInfo>" << endl;

   274

}

   275

   276

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

   277

   278

unsigned int Master::slaveCount()

   279

{

   280

    int ret;

   281

   282

    if ((ret = ioctl(fd, EC_IOCTL_SLAVE_COUNT, 0)) < 0) {

   283

        stringstream err;

   284

        err << "Failed to get slave: " << strerror(errno);

   285

        throw MasterException(err.str());

   286

    }

   287

   288

    return ret;

   289

}

   290

   291

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

   292

   293

void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)

   294

{

   295

    slave->position = slaveIndex;

   296

   297

    if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {

   298

        stringstream err;

   299

        err << "Failed to get slave: ";

   300

        if (errno == EINVAL)

   301

            err << "Slave " << slaveIndex << " does not exist!";

   302

        else

   303

            err << strerror(errno);

   304

        throw MasterException(err.str());

   305

    }

   306

}

   307

   308

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

   309

   310

void Master::getSync(

   311

        ec_ioctl_sync_t *sync,

   312

        uint16_t slaveIndex,

   313

        uint8_t syncIndex

   314

        )

   315

{

   316

    sync->slave_position = slaveIndex;

   317

    sync->sync_index = syncIndex;

   318

   319

    if (ioctl(fd, EC_IOCTL_SYNC, sync)) {

   320

        stringstream err;

   321

        err << "Failed to get sync manager: ";

   322

        if (errno == EINVAL)

   323

            err << "Either slave " << slaveIndex << " does not exist, "

   324

                << "or contains less than " << (unsigned int) syncIndex + 1

   325

                << " sync managers!";

   326

        else

   327

            err << strerror(errno);

   328

        throw MasterException(err.str());

   329

    }

   330

}

   331

   332

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

   333

   334

void Master::getPdo(

   335

        ec_ioctl_pdo_t *pdo,

   336

        uint16_t slaveIndex,

   337

        uint8_t syncIndex,

   338

        uint8_t pdoPos

   339

        )

   340

{

   341

    pdo->slave_position = slaveIndex;

   342

    pdo->sync_index = syncIndex;

   343

    pdo->pdo_pos = pdoPos;

   344

   345

    if (ioctl(fd, EC_IOCTL_PDO, pdo)) {

   346

        stringstream err;

   347

        err << "Failed to get Pdo: ";

   348

        if (errno == EINVAL)

   349

            err << "Either slave " << slaveIndex << " does not exist, "

   350

                << "or contains less than " << (unsigned int) syncIndex + 1

   351

                << " sync managers, or sync manager "

   352

                << (unsigned int) syncIndex << " contains less than "

   353

                << pdoPos + 1 << " Pdos!" << endl;

   354

        else

   355

            err << strerror(errno);

   356

        throw MasterException(err.str());

   357

    }

   358

}

   359

   360

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

   361

   362

void Master::getPdoEntry(

   363

        ec_ioctl_pdo_entry_t *entry,

   364

        uint16_t slaveIndex,

   365

        uint8_t syncIndex,

   366

        uint8_t pdoPos,

   367

        uint8_t entryPos

   368

        )

   369

{

   370

    entry->slave_position = slaveIndex;

   371

    entry->sync_index = syncIndex;

   372

    entry->pdo_pos = pdoPos;

   373

    entry->entry_pos = entryPos;

   374

   375

    if (ioctl(fd, EC_IOCTL_PDO_ENTRY, entry)) {

   376

        stringstream err;

   377

        err << "Failed to get Pdo entry: ";

   378

        if (errno == EINVAL)

   379

            err << "Either slave " << slaveIndex << " does not exist, "

   380

                << "or contains less than " << (unsigned int) syncIndex + 1

   381

                << " sync managers, or sync manager "

   382

                << (unsigned int) syncIndex << " contains less than "

   383

                << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos

   384

                << " contains less than " << (unsigned int) entryPos + 1

   385

                << " entries!" << endl;

   386

        else

   387

            err << strerror(errno);

   388

        throw MasterException(err.str());

   389

    }

   390

}

   391

   392

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

   393

   394

string Master::slaveState(uint8_t state)

   395

{

   396

    switch (state) {

   397

        case 1: return "INIT";

   398

        case 2: return "PREOP";

   399

        case 4: return "SAFEOP";

   400

        case 8: return "OP";

   401

        default: return "???";

   402

    }

   403

}

   404

   405

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