1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

#include <sys/stat.h>

    31

#include <fcntl.h>

    32

#include <errno.h>

    33

#include <sys/ioctl.h>

    34

#include <string.h>

    35

#include <unistd.h>

    36

    37

#include <sstream>

    38

#include <iomanip>

    39

using namespace std;

    40

    41

#include "MasterDevice.h"

    42

    43

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

    44

    45

MasterDevice::MasterDevice(unsigned int index):

    46

    index(index),

    47

    masterCount(0U),

    48

    fd(-1)

    49

{

    50

}

    51

    52

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

    53

    54

MasterDevice::~MasterDevice()

    55

{

    56

    close();

    57

}

    58

    59

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

    60

    61

void MasterDevice::setIndex(unsigned int i)

    62

{

    63

    index = i;

    64

}

    65

    66

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

    67

    68

void MasterDevice::open(Permissions perm)

    69

{

    70

    stringstream deviceName;

    71

    72

    if (fd == -1) { // not already open

    73

        ec_ioctl_module_t module_data;

    74

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

    75

    76

        if ((fd = ::open(deviceName.str().c_str(),

    77

                        perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {

    78

            stringstream err;

    79

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

    80

                << strerror(errno);

    81

            throw MasterDeviceException(err);

    82

        }

    83

    84

        getModule(&module_data);

    85

        if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {

    86

            stringstream err;

    87

            err << "ioctl() version magic is differing: "

    88

                << deviceName.str() << ": " << module_data.ioctl_version_magic

    89

                << ", ethercat tool: " << EC_IOCTL_VERSION_MAGIC;

    90

            throw MasterDeviceException(err);

    91

        }

    92

        masterCount = module_data.master_count;

    93

    }

    94

}

    95

    96

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

    97

    98

void MasterDevice::close()

    99

{

   100

    if (fd != -1) {

   101

        ::close(fd);

   102

        fd = -1;

   103

    }

   104

}

   105

   106

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

   107

   108

void MasterDevice::getModule(ec_ioctl_module_t *data)

   109

{

   110

    if (ioctl(fd, EC_IOCTL_MODULE, data) < 0) {

   111

        stringstream err;

   112

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

   113

        throw MasterDeviceException(err);

   114

    }

   115

}

   116

   117

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

   118

   119

void MasterDevice::getMaster(ec_ioctl_master_t *data)

   120

{

   121

    if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {

   122

        stringstream err;

   123

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

   124

        throw MasterDeviceException(err);

   125

    }

   126

}

   127

   128

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

   129

   130

void MasterDevice::getConfig(ec_ioctl_config_t *data, unsigned int index)

   131

{

   132

    data->config_index = index;

   133

   134

    if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {

   135

        stringstream err;

   136

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

   137

        throw MasterDeviceException(err);

   138

    }

   139

}

   140

   141

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

   142

   143

void MasterDevice::getConfigPdo(

   144

        ec_ioctl_config_pdo_t *data,

   145

        unsigned int index,

   146

        uint8_t sync_index,

   147

        uint16_t pdo_pos

   148

        )

   149

{

   150

    data->config_index = index;

   151

    data->sync_index = sync_index;

   152

    data->pdo_pos = pdo_pos;

   153

   154

    if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {

   155

        stringstream err;

   156

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

   157

        throw MasterDeviceException(err);

   158

    }

   159

}

   160

   161

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

   162

   163

void MasterDevice::getConfigPdoEntry(

   164

        ec_ioctl_config_pdo_entry_t *data,

   165

        unsigned int index,

   166

        uint8_t sync_index,

   167

        uint16_t pdo_pos,

   168

        uint8_t entry_pos

   169

        )

   170

{

   171

    data->config_index = index;

   172

    data->sync_index = sync_index;

   173

    data->pdo_pos = pdo_pos;

   174

    data->entry_pos = entry_pos;

   175

   176

    if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {

   177

        stringstream err;

   178

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

   179

        throw MasterDeviceException(err);

   180

    }

   181

}

   182

   183

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

   184

   185

void MasterDevice::getConfigSdo(

   186

        ec_ioctl_config_sdo_t *data,

   187

        unsigned int index,

   188

        unsigned int sdo_pos

   189

        )

   190

{

   191

    data->config_index = index;

   192

    data->sdo_pos = sdo_pos;

   193

   194

    if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {

   195

        stringstream err;

   196

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

   197

        throw MasterDeviceException(err);

   198

    }

   199

}

   200

   201

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

   202

   203

void MasterDevice::getConfigIdn(

   204

        ec_ioctl_config_idn_t *data,

   205

        unsigned int index,

   206

        unsigned int pos

   207

        )

   208

{

   209

    data->config_index = index;

   210

    data->idn_pos = pos;

   211

   212

    if (ioctl(fd, EC_IOCTL_CONFIG_IDN, data) < 0) {

   213

        stringstream err;

   214

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

   215

        throw MasterDeviceException(err);

   216

    }

   217

}

   218

   219

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

   220

   221

void MasterDevice::getDomain(ec_ioctl_domain_t *data, unsigned int index)

   222

{

   223

    data->index = index;

   224

   225

    if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {

   226

        stringstream err;

   227

        err << "Failed to get domain: ";

   228

        if (errno == EINVAL)

   229

            err << "Domain " << index << " does not exist!";

   230

        else

   231

            err << strerror(errno);

   232

        throw MasterDeviceException(err);

   233

    }

   234

}

   235

   236

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

   237

   238

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   239

        unsigned int domainIndex, unsigned int dataSize, unsigned char *mem)

   240

{

   241

    data->domain_index = domainIndex;

   242

    data->data_size = dataSize;

   243

    data->target = mem;

   244

   245

    if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {

   246

        stringstream err;

   247

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

   248

        throw MasterDeviceException(err);

   249

    }

   250

}

   251

   252

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

   253

   254

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

   255

{

   256

    slave->position = slaveIndex;

   257

   258

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

   259

        stringstream err;

   260

        err << "Failed to get slave: ";

   261

        if (errno == EINVAL)

   262

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

   263

        else

   264

            err << strerror(errno);

   265

        throw MasterDeviceException(err);

   266

    }

   267

}

   268

   269

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

   270

   271

void MasterDevice::getFmmu(

   272

        ec_ioctl_domain_fmmu_t *fmmu,

   273

        unsigned int domainIndex,

   274

        unsigned int fmmuIndex

   275

        )

   276

{

   277

    fmmu->domain_index = domainIndex;

   278

    fmmu->fmmu_index = fmmuIndex;

   279

   280

    if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {

   281

        stringstream err;

   282

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

   283

        throw MasterDeviceException(err);

   284

    }

   285

}

   286

   287

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

   288

   289

void MasterDevice::getSync(

   290

        ec_ioctl_slave_sync_t *sync,

   291

        uint16_t slaveIndex,

   292

        uint8_t syncIndex

   293

        )

   294

{

   295

    sync->slave_position = slaveIndex;

   296

    sync->sync_index = syncIndex;

   297

   298

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

   299

        stringstream err;

   300

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

   301

        throw MasterDeviceException(err);

   302

    }

   303

}

   304

   305

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

   306

   307

void MasterDevice::getPdo(

   308

        ec_ioctl_slave_sync_pdo_t *pdo,

   309

        uint16_t slaveIndex,

   310

        uint8_t syncIndex,

   311

        uint8_t pdoPos

   312

        )

   313

{

   314

    pdo->slave_position = slaveIndex;

   315

    pdo->sync_index = syncIndex;

   316

    pdo->pdo_pos = pdoPos;

   317

   318

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

   319

        stringstream err;

   320

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

   321

        throw MasterDeviceException(err);

   322

    }

   323

}

   324

   325

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

   326

   327

void MasterDevice::getPdoEntry(

   328

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   329

        uint16_t slaveIndex,

   330

        uint8_t syncIndex,

   331

        uint8_t pdoPos,

   332

        uint8_t entryPos

   333

        )

   334

{

   335

    entry->slave_position = slaveIndex;

   336

    entry->sync_index = syncIndex;

   337

    entry->pdo_pos = pdoPos;

   338

    entry->entry_pos = entryPos;

   339

   340

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

   341

        stringstream err;

   342

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

   343

        throw MasterDeviceException(err);

   344

    }

   345

}

   346

   347

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

   348

   349

void MasterDevice::getSdo(

   350

        ec_ioctl_slave_sdo_t *sdo,

   351

        uint16_t slaveIndex,

   352

        uint16_t sdoPosition

   353

        )

   354

{

   355

    sdo->slave_position = slaveIndex;

   356

    sdo->sdo_position = sdoPosition;

   357

   358

    if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {

   359

        stringstream err;

   360

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

   361

        throw MasterDeviceException(err);

   362

    }

   363

}

   364

   365

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

   366

   367

void MasterDevice::getSdoEntry(

   368

        ec_ioctl_slave_sdo_entry_t *entry,

   369

        uint16_t slaveIndex,

   370

        int sdoSpec,

   371

        uint8_t entrySubindex

   372

        )

   373

{

   374

    entry->slave_position = slaveIndex;

   375

    entry->sdo_spec = sdoSpec;

   376

    entry->sdo_entry_subindex = entrySubindex;

   377

   378

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

   379

        stringstream err;

   380

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

   381

        throw MasterDeviceException(err);

   382

    }

   383

}

   384

   385

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

   386

   387

void MasterDevice::readSii(

   388

        ec_ioctl_slave_sii_t *data

   389

        )

   390

{

   391

    if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, data) < 0) {

   392

        stringstream err;

   393

        err << "Failed to read SII: " << strerror(errno);

   394

        throw MasterDeviceException(err);

   395

    }

   396

}

   397

   398

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

   399

   400

void MasterDevice::writeSii(

   401

        ec_ioctl_slave_sii_t *data

   402

        )

   403

{

   404

    if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, data) < 0) {

   405

        stringstream err;

   406

        err << "Failed to write SII: " << strerror(errno);

   407

        throw MasterDeviceException(err);

   408

    }

   409

}

   410

   411

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

   412

   413

void MasterDevice::readReg(

   414

        ec_ioctl_slave_reg_t *data

   415

        )

   416

{

   417

    if (ioctl(fd, EC_IOCTL_SLAVE_REG_READ, data) < 0) {

   418

        stringstream err;

   419

        err << "Failed to read register: " << strerror(errno);

   420

        throw MasterDeviceException(err);

   421

    }

   422

}

   423

   424

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

   425

   426

void MasterDevice::writeReg(

   427

        ec_ioctl_slave_reg_t *data

   428

        )

   429

{

   430

    if (ioctl(fd, EC_IOCTL_SLAVE_REG_WRITE, data) < 0) {

   431

        stringstream err;

   432

        err << "Failed to write register: " << strerror(errno);

   433

        throw MasterDeviceException(err);

   434

    }

   435

}

   436

   437

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

   438

   439

void MasterDevice::readFoe(

   440

        ec_ioctl_slave_foe_t *data

   441

        )

   442

{

   443

    if (ioctl(fd, EC_IOCTL_SLAVE_FOE_READ, data) < 0) {

   444

        stringstream err;

   445

        err << "Failed to read via FoE: " << strerror(errno);

   446

        throw MasterDeviceException(err);

   447

    }

   448

}

   449

   450

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

   451

   452

void MasterDevice::writeFoe(

   453

        ec_ioctl_slave_foe_t *data

   454

        )

   455

{

   456

    if (ioctl(fd, EC_IOCTL_SLAVE_FOE_WRITE, data) < 0) {

   457

        stringstream err;

   458

        err << "Failed to write via FoE: " << strerror(errno);

   459

        throw MasterDeviceException(err);

   460

    }

   461

}

   462

   463

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

   464

   465

void MasterDevice::setDebug(unsigned int debugLevel)

   466

{

   467

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

   468

        stringstream err;

   469

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

   470

        throw MasterDeviceException(err);

   471

    }

   472

}

   473

   474

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

   475

   476

void MasterDevice::rescan()

   477

{

   478

    if (ioctl(fd, EC_IOCTL_MASTER_RESCAN, 0) < 0) {

   479

        stringstream err;

   480

        err << "Failed to command rescan: " << strerror(errno);

   481

        throw MasterDeviceException(err);

   482

    }

   483

}

   484

   485

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

   486

   487

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   488

{

   489

    if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, data) < 0) {

   490

        stringstream err;

   491

        if (errno == EIO && data->abort_code) {

   492

            throw MasterDeviceSdoAbortException(data->abort_code);

   493

        } else {

   494

            err << "Failed to download SDO: " << strerror(errno);

   495

            throw MasterDeviceException(err);

   496

        }

   497

    }

   498

}

   499

   500

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

   501

   502

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   503

{

   504

    if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, data) < 0) {

   505

        stringstream err;

   506

        if (errno == EIO && data->abort_code) {

   507

            throw MasterDeviceSdoAbortException(data->abort_code);

   508

        } else {

   509

            err << "Failed to upload SDO: " << strerror(errno);