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()

    46

{

    47

    index = 0;

    48

    fd = -1;

    49

}

    50

    51

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

    52

    53

MasterDevice::~MasterDevice()

    54

{

    55

    close();

    56

}

    57

    58

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

    59

    60

void MasterDevice::setIndex(unsigned int i)

    61

{

    62

    index = i;

    63

}

    64

    65

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

    66

    67

void MasterDevice::open(Permissions perm)

    68

{

    69

    stringstream deviceName;

    70

    71

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

    72

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

    73

    74

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

    75

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

    76

			stringstream err;

    77

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

    78

				<< strerror(errno);

    79

			throw MasterDeviceException(err);

    80

		}

    81

    }

    82

}

    83

    84

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

    85

    86

void MasterDevice::close()

    87

{

    88

    if (fd != -1) {

    89

		::close(fd);

    90

		fd = -1;

    91

	}

    92

}

    93

    94

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

    95

    96

void MasterDevice::getMaster(ec_ioctl_master_t *data)

    97

{

    98

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

    99

        stringstream err;

   100

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

   101

        throw MasterDeviceException(err);

   102

    }

   103

}

   104

   105

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

   106

   107

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

   108

{

   109

    data->config_index = index;

   110

   111

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

   112

        stringstream err;

   113

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

   114

        throw MasterDeviceException(err);

   115

    }

   116

}

   117

   118

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

   119

   120

void MasterDevice::getConfigPdo(

   121

        ec_ioctl_config_pdo_t *data,

   122

        unsigned int index,

   123

        uint8_t sync_index,

   124

        uint16_t pdo_pos

   125

        )

   126

{

   127

    data->config_index = index;

   128

    data->sync_index = sync_index;

   129

    data->pdo_pos = pdo_pos;

   130

   131

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

   132

        stringstream err;

   133

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

   134

        throw MasterDeviceException(err);

   135

    }

   136

}

   137

   138

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

   139

   140

void MasterDevice::getConfigPdoEntry(

   141

        ec_ioctl_config_pdo_entry_t *data,

   142

        unsigned int index,

   143

        uint8_t sync_index,

   144

        uint16_t pdo_pos,

   145

        uint8_t entry_pos

   146

        )

   147

{

   148

    data->config_index = index;

   149

    data->sync_index = sync_index;

   150

    data->pdo_pos = pdo_pos;

   151

    data->entry_pos = entry_pos;

   152

   153

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

   154

        stringstream err;

   155

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

   156

        throw MasterDeviceException(err);

   157

    }

   158

}

   159

   160

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

   161

   162

void MasterDevice::getConfigSdo(

   163

        ec_ioctl_config_sdo_t *data,

   164

        unsigned int index,

   165

        unsigned int sdo_pos

   166

        )

   167

{

   168

    data->config_index = index;

   169

    data->sdo_pos = sdo_pos;

   170

   171

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

   172

        stringstream err;

   173

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

   174

        throw MasterDeviceException(err);

   175

    }

   176

}

   177

   178

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

   179

   180

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

   181

{

   182

    data->index = index;

   183

   184

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

   185

        stringstream err;

   186

        err << "Failed to get domain: ";

   187

        if (errno == EINVAL)

   188

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

   189

        else

   190

            err << strerror(errno);

   191

        throw MasterDeviceException(err);

   192

    }

   193

}

   194

   195

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

   196

   197

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   198

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

   199

{

   200

    data->domain_index = domainIndex;

   201

    data->data_size = dataSize;

   202

    data->target = mem;

   203

   204

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

   205

        stringstream err;

   206

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

   207

        throw MasterDeviceException(err);

   208

    }

   209

}

   210

   211

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

   212

   213

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

   214

{

   215

    slave->position = slaveIndex;

   216

   217

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

   218

        stringstream err;

   219

        err << "Failed to get slave: ";

   220

        if (errno == EINVAL)

   221

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

   222

        else

   223

            err << strerror(errno);

   224

        throw MasterDeviceException(err);

   225

    }

   226

}

   227

   228

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

   229

   230

void MasterDevice::getFmmu(

   231

        ec_ioctl_domain_fmmu_t *fmmu,

   232

        unsigned int domainIndex,

   233

        unsigned int fmmuIndex

   234

        )

   235

{

   236

    fmmu->domain_index = domainIndex;

   237

    fmmu->fmmu_index = fmmuIndex;

   238

   239

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

   240

        stringstream err;

   241

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

   242

        throw MasterDeviceException(err);

   243

    }

   244

}

   245

   246

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

   247

   248

void MasterDevice::getSync(

   249

        ec_ioctl_slave_sync_t *sync,

   250

        uint16_t slaveIndex,

   251

        uint8_t syncIndex

   252

        )

   253

{

   254

    sync->slave_position = slaveIndex;

   255

    sync->sync_index = syncIndex;

   256

   257

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

   258

        stringstream err;

   259

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

   260

        throw MasterDeviceException(err);

   261

    }

   262

}

   263

   264

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

   265

   266

void MasterDevice::getPdo(

   267

        ec_ioctl_slave_sync_pdo_t *pdo,

   268

        uint16_t slaveIndex,

   269

        uint8_t syncIndex,

   270

        uint8_t pdoPos

   271

        )

   272

{

   273

    pdo->slave_position = slaveIndex;

   274

    pdo->sync_index = syncIndex;

   275

    pdo->pdo_pos = pdoPos;

   276

   277

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

   278

        stringstream err;

   279

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

   280

        throw MasterDeviceException(err);

   281

    }

   282

}

   283

   284

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

   285

   286

void MasterDevice::getPdoEntry(

   287

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   288

        uint16_t slaveIndex,

   289

        uint8_t syncIndex,

   290

        uint8_t pdoPos,

   291

        uint8_t entryPos

   292

        )

   293

{

   294

    entry->slave_position = slaveIndex;

   295

    entry->sync_index = syncIndex;

   296

    entry->pdo_pos = pdoPos;

   297

    entry->entry_pos = entryPos;

   298

   299

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

   300

        stringstream err;

   301

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

   302

        throw MasterDeviceException(err);

   303

    }

   304

}

   305

   306

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

   307

   308

void MasterDevice::getSdo(

   309

        ec_ioctl_slave_sdo_t *sdo,

   310

        uint16_t slaveIndex,

   311

        uint16_t sdoPosition

   312

        )

   313

{

   314

    sdo->slave_position = slaveIndex;

   315

    sdo->sdo_position = sdoPosition;

   316

   317

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

   318

        stringstream err;

   319

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

   320

        throw MasterDeviceException(err);

   321

    }

   322

}

   323

   324

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

   325

   326

void MasterDevice::getSdoEntry(

   327

        ec_ioctl_slave_sdo_entry_t *entry,

   328

        uint16_t slaveIndex,

   329

        int sdoSpec,

   330

        uint8_t entrySubindex

   331

        )

   332

{

   333

    entry->slave_position = slaveIndex;

   334

    entry->sdo_spec = sdoSpec;

   335

    entry->sdo_entry_subindex = entrySubindex;

   336

   337

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

   338

        stringstream err;

   339

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

   340

        throw MasterDeviceException(err);

   341

    }

   342

}

   343

   344

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

   345

   346

void MasterDevice::readSii(

   347

        ec_ioctl_slave_sii_t *data

   348

        )

   349

{

   350

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

   351

        stringstream err;

   352

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

   353

        throw MasterDeviceException(err);

   354

    }

   355

}

   356

   357

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

   358

   359

void MasterDevice::writeSii(

   360

        ec_ioctl_slave_sii_t *data

   361

        )

   362

{

   363

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

   364

        stringstream err;

   365

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

   366

        throw MasterDeviceException(err);

   367

    }

   368

}

   369

   370

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

   371

   372

void MasterDevice::readReg(

   373

        ec_ioctl_slave_reg_t *data

   374

        )

   375

{

   376

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

   377

        stringstream err;

   378

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

   379

        throw MasterDeviceException(err);

   380

    }

   381

}

   382

   383

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

   384

   385

void MasterDevice::writeReg(

   386

        ec_ioctl_slave_reg_t *data

   387

        )

   388

{

   389

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

   390

        stringstream err;

   391

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

   392

        throw MasterDeviceException(err);

   393

    }

   394

}

   395

   396

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

   397

   398

void MasterDevice::readFoe(

   399

        ec_ioctl_slave_foe_t *data

   400

        )

   401

{

   402

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

   403

        stringstream err;

   404

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

   405

        throw MasterDeviceException(err);

   406

    }

   407

}

   408

   409

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

   410

   411

void MasterDevice::writeFoe(

   412

        ec_ioctl_slave_foe_t *data

   413

        )

   414

{

   415

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

   416

        stringstream err;

   417

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

   418

        throw MasterDeviceException(err);

   419

    }

   420

}

   421

   422

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

   423

   424

void MasterDevice::setDebug(unsigned int debugLevel)

   425

{

   426

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

   427

        stringstream err;

   428

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

   429

        throw MasterDeviceException(err);

   430

	}

   431

}

   432

   433

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

   434

   435

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   436

{

   437

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

   438

        stringstream err;

   439

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

   440

            throw MasterDeviceSdoAbortException(data->abort_code);

   441

        } else {

   442

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

   443

            throw MasterDeviceException(err);

   444

        }

   445

	}

   446

}

   447

   448

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

   449

   450

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   451

{

   452

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

   453

        stringstream err;

   454

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

   455

            throw MasterDeviceSdoAbortException(data->abort_code);

   456

        } else {

   457

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

   458

            throw MasterDeviceException(err);

   459

        }

   460

    }

   461

}

   462

   463

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

   464

   465

void MasterDevice::requestState(

   466

        uint16_t slavePosition,

   467

        uint8_t state

   468

        )

   469

{

   470

    ec_ioctl_slave_state_t data;

   471

   472

    data.slave_position = slavePosition;

   473

    data.al_state = state;

   474

   475

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

   476

        stringstream err;

   477

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

   478

        if (errno == EINVAL)

   479

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

   480

        else

   481

            err << strerror(errno);

   482

        throw MasterDeviceException(err);

   483

    }

   484

}

   485

   486

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