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

{

   117

    ec_ioctl_slave_t slave;

   118

    ec_ioctl_sync_t sync;

   119

    ec_ioctl_pdo_t pdo;

   120

    ec_ioctl_pdo_entry_t entry;

   121

    unsigned int i, j, k;

   122

   123

    getSlave(&slave, slavePosition);

   124

   128

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

   129

        getSync(&sync, slavePosition, i);

   130

   131

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

   132

            << " PhysAddr 0x"

   133

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

   134

            << ", DefaultSize "

   135

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

   136

            << ", ControlRegister 0x"

   137

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

   138

            << (unsigned int) sync.control_register

   139

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

   140

            << endl;

   141

   142

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

   143

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

   144

   145

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

   146

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

   147

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

   148

   149

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

   150

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

   151

   152

                cout << "    Pdo entry 0x"

   153

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

   154

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

   155

                    << (unsigned int) entry.subindex

   156

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

   157

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

   158

            }

   159

        }

   160

    }

   161

}

   162

   163

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

   164

   165

unsigned int Master::slaveCount()

   166

{

   167

    int ret;

   168

   169

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

   170

        stringstream err;

   171

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

   172

        throw MasterException(err.str());

   173

    }

   174

   175

    return ret;

   176

}

   177

   178

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

   179

   180

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

   181

{

   182

    slave->position = slaveIndex;

   183

   184

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

   185

        stringstream err;

   186

        err << "Failed to get slave: ";

   187

        if (errno == EINVAL)

   188

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

   189

        else

   190

            err << strerror(errno);

   191

        throw MasterException(err.str());

   192

    }

   193

}

   194

   195

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

   196

   197

void Master::getSync(

   198

        ec_ioctl_sync_t *sync,

   199

        uint16_t slaveIndex,

   200

        uint8_t syncIndex

   201

        )

   202

{

   203

    sync->slave_position = slaveIndex;

   204

    sync->sync_index = syncIndex;

   205

   206

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

   207

        stringstream err;

   208

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

   209

        if (errno == EINVAL)

   210

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

   211

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

   212

                << " sync managers!";

   213

        else

   214

            err << strerror(errno);

   215

        throw MasterException(err.str());

   216

    }

   217

}

   218

   219

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

   220

   221

void Master::getPdo(

   222

        ec_ioctl_pdo_t *pdo,

   223

        uint16_t slaveIndex,

   224

        uint8_t syncIndex,

   225

        uint8_t pdoPos

   226

        )

   227

{

   228

    pdo->slave_position = slaveIndex;

   229

    pdo->sync_index = syncIndex;

   230

    pdo->pdo_pos = pdoPos;

   231

   232

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

   233

        stringstream err;

   234

        err << "Failed to get Pdo: ";

   235

        if (errno == EINVAL)

   236

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

   237

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

   238

                << " sync managers, or sync manager "

   239

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

   240

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

   241

        else

   242

            err << strerror(errno);

   243

        throw MasterException(err.str());

   244

    }

   245

}

   246

   247

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

   248

   249

void Master::getPdoEntry(

   250

        ec_ioctl_pdo_entry_t *entry,

   251

        uint16_t slaveIndex,

   252

        uint8_t syncIndex,

   253

        uint8_t pdoPos,

   254

        uint8_t entryPos

   255

        )

   256

{

   257

    entry->slave_position = slaveIndex;

   258

    entry->sync_index = syncIndex;

   259

    entry->pdo_pos = pdoPos;

   260

    entry->entry_pos = entryPos;

   261

   262

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

   263

        stringstream err;

   264

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

   265

        if (errno == EINVAL)

   266

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

   267

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

   268

                << " sync managers, or sync manager "

   269

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

   270

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

   271

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

   272

                << " entries!" << endl;

   273

        else

   274

            err << strerror(errno);

   275

        throw MasterException(err.str());

   276

    }

   277

}

   278

   279

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

   280

   281

string Master::slaveState(uint8_t state)

   282

{

   283

    switch (state) {

   284

        case 1: return "INIT";

   285

        case 2: return "PREOP";

   286

        case 4: return "SAFEOP";

   287

        case 8: return "OP";

   288

        default: return "???";

   289

    }

   290

}

   291

   292

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