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::getDomain(ec_ioctl_domain_t *data, unsigned int index)

   204

{

   205

    data->index = index;

   206

   207

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

   208

        stringstream err;

   209

        err << "Failed to get domain: ";

   210

        if (errno == EINVAL)

   211

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

   212

        else

   213

            err << strerror(errno);

   214

        throw MasterDeviceException(err);

   215

    }

   216

}

   217

   218

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

   219

   220

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   221

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

   222

{

   223

    data->domain_index = domainIndex;

   224

    data->data_size = dataSize;

   225

    data->target = mem;

   226

   227

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

   228

        stringstream err;

   229

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

   230

        throw MasterDeviceException(err);

   231

    }

   232

}

   233

   234

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

   235

   236

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

   237

{

   238

    slave->position = slaveIndex;

   239

   240

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

   241

        stringstream err;

   242

        err << "Failed to get slave: ";

   243

        if (errno == EINVAL)

   244

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

   245

        else

   246

            err << strerror(errno);

   247

        throw MasterDeviceException(err);

   248

    }

   249

}

   250

   251

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

   252

   253

void MasterDevice::getFmmu(

   254

        ec_ioctl_domain_fmmu_t *fmmu,

   255

        unsigned int domainIndex,

   256

        unsigned int fmmuIndex

   257

        )

   258

{

   259

    fmmu->domain_index = domainIndex;

   260

    fmmu->fmmu_index = fmmuIndex;

   261

   262

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

   263

        stringstream err;

   264

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

   265

        throw MasterDeviceException(err);

   266

    }

   267

}

   268

   269

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

   270

   271

void MasterDevice::getSync(

   272

        ec_ioctl_slave_sync_t *sync,

   273

        uint16_t slaveIndex,

   274

        uint8_t syncIndex

   275

        )

   276

{

   277

    sync->slave_position = slaveIndex;

   278

    sync->sync_index = syncIndex;

   279

   280

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

   281

        stringstream err;

   282

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

   283

        throw MasterDeviceException(err);

   284

    }

   285

}

   286

   287

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

   288

   289

void MasterDevice::getPdo(

   290

        ec_ioctl_slave_sync_pdo_t *pdo,

   291

        uint16_t slaveIndex,

   292

        uint8_t syncIndex,

   293

        uint8_t pdoPos

   294

        )

   295

{

   296

    pdo->slave_position = slaveIndex;

   297

    pdo->sync_index = syncIndex;

   298

    pdo->pdo_pos = pdoPos;

   299

   300

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

   301

        stringstream err;

   302

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

   303

        throw MasterDeviceException(err);

   304

    }

   305

}

   306

   307

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

   308

   309

void MasterDevice::getPdoEntry(

   310

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   311

        uint16_t slaveIndex,

   312

        uint8_t syncIndex,

   313

        uint8_t pdoPos,

   314

        uint8_t entryPos

   315

        )

   316

{

   317

    entry->slave_position = slaveIndex;

   318

    entry->sync_index = syncIndex;

   319

    entry->pdo_pos = pdoPos;

   320

    entry->entry_pos = entryPos;

   321

   322

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

   323

        stringstream err;

   324

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

   325

        throw MasterDeviceException(err);

   326

    }

   327

}

   328

   329

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

   330

   331

void MasterDevice::getSdo(

   332

        ec_ioctl_slave_sdo_t *sdo,

   333

        uint16_t slaveIndex,

   334

        uint16_t sdoPosition

   335

        )

   336

{

   337

    sdo->slave_position = slaveIndex;

   338

    sdo->sdo_position = sdoPosition;

   339

   340

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

   341

        stringstream err;

   342

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

   343

        throw MasterDeviceException(err);

   344

    }

   345

}

   346

   347

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

   348

   349

void MasterDevice::getSdoEntry(

   350

        ec_ioctl_slave_sdo_entry_t *entry,

   351

        uint16_t slaveIndex,

   352

        int sdoSpec,

   353

        uint8_t entrySubindex

   354

        )

   355

{

   356

    entry->slave_position = slaveIndex;

   357

    entry->sdo_spec = sdoSpec;

   358

    entry->sdo_entry_subindex = entrySubindex;

   359

   360

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

   361

        stringstream err;

   362

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

   363

        throw MasterDeviceException(err);

   364

    }

   365

}

   366

   367

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

   368

   369

void MasterDevice::readSii(

   370

        ec_ioctl_slave_sii_t *data

   371

        )

   372

{

   373

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

   374

        stringstream err;

   375

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

   376

        throw MasterDeviceException(err);

   377

    }

   378

}

   379

   380

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

   381

   382

void MasterDevice::writeSii(

   383

        ec_ioctl_slave_sii_t *data

   384

        )

   385

{

   386

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

   387

        stringstream err;

   388

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

   389

        throw MasterDeviceException(err);

   390

    }

   391

}

   392

   393

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

   394

   395

void MasterDevice::readReg(

   396

        ec_ioctl_slave_reg_t *data

   397

        )

   398

{

   399

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

   400

        stringstream err;

   401

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

   402

        throw MasterDeviceException(err);

   403

    }

   404

}

   405

   406

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

   407

   408

void MasterDevice::writeReg(

   409

        ec_ioctl_slave_reg_t *data

   410

        )

   411

{

   412

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

   413

        stringstream err;

   414

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

   415

        throw MasterDeviceException(err);

   416

    }

   417

}

   418

   419

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

   420

   421

void MasterDevice::readFoe(

   422

        ec_ioctl_slave_foe_t *data

   423

        )

   424

{

   425

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

   426

        stringstream err;

   427

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

   428

        throw MasterDeviceException(err);

   429

    }

   430

}

   431

   432

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

   433

   434

void MasterDevice::writeFoe(

   435

        ec_ioctl_slave_foe_t *data

   436

        )

   437

{

   438

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

   439

        stringstream err;

   440

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

   441

        throw MasterDeviceException(err);

   442

    }

   443

}

   444

   445

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

   446

   447

void MasterDevice::setDebug(unsigned int debugLevel)

   448

{

   449

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

   450

        stringstream err;

   451

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

   452

        throw MasterDeviceException(err);

   453

    }

   454

}

   455

   456

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

   457

   458

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   459

{

   460

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

   461

        stringstream err;

   462

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

   463

            throw MasterDeviceSdoAbortException(data->abort_code);

   464

        } else {

   465

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

   466

            throw MasterDeviceException(err);

   467

        }

   468

    }

   469

}

   470

   471

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

   472

   473

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   474

{

   475

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

   476

        stringstream err;

   477

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

   478

            throw MasterDeviceSdoAbortException(data->abort_code);

   479

        } else {

   480

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

   481

            throw MasterDeviceException(err);

   482

        }

   483

    }

   484

}

   485

   486

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

   487

   488

void MasterDevice::requestState(

   489

        uint16_t slavePosition,

   490

        uint8_t state

   491

        )

   492

{

   493

    ec_ioctl_slave_state_t data;

   494

   495

    data.slave_position = slavePosition;

   496

    data.al_state = state;

   497

   498

    if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {

   499

        stringstream err;

   500

        err << "Failed to request slave state: ";

   501

        if (errno == EINVAL)

   502

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

   503

        else

   504

            err << strerror(errno);

   505

        throw MasterDeviceException(err);

   506

    }

   507

}

   508

   509

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

   510