1

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

     2

 *

     3

 * $Id$

     4

 *

     5

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

     6

     7

#include <sys/stat.h>

     8

#include <fcntl.h>

     9

#include <errno.h>

    10

#include <sys/ioctl.h>

    11

    12

#include <sstream>

    13

#include <iomanip>

    14

using namespace std;

    15

    16

#include "MasterDevice.h"

    17

    18

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

    19

    20

MasterDevice::MasterDevice()

    21

{

    22

    index = 0;

    23

    fd = -1;

    24

}

    25

    26

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

    27

    28

MasterDevice::~MasterDevice()

    29

{

    30

    close();

    31

}

    32

    33

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

    34

    35

void MasterDevice::setIndex(unsigned int i)

    36

{

    37

    index = i;

    38

}

    39

    40

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

    41

    42

void MasterDevice::open(Permissions perm)

    43

{

    44

    stringstream deviceName;

    45

    46

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

    47

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

    48

    49

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

    50

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

    51

			stringstream err;

    52

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

    53

				<< strerror(errno);

    54

			throw MasterDeviceException(err.str());

    55

		}

    56

    }

    57

}

    58

    59

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

    60

    61

void MasterDevice::close()

    62

{

    63

    if (fd != -1) {

    64

		::close(fd);

    65

		fd = -1;

    66

	}

    67

}

    68

    69

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

    70

    71

unsigned int MasterDevice::slaveCount()

    72

{

    73

    ec_ioctl_master_t data;

    74

    75

    getMaster(&data);

    76

    return data.slave_count;

    77

}

    78

    79

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

    80

    81

void MasterDevice::getMaster(ec_ioctl_master_t *data)

    82

{

    83

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

    84

        stringstream err;

    85

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

    86

        throw MasterDeviceException(err.str());

    87

    }

    88

}

    89

    90

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

    91

    92

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

    93

{

    94

    data->config_index = index;

    95

    96

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

    97

        stringstream err;

    98

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

    99

        throw MasterDeviceException(err.str());

   100

    }

   101

}

   102

   103

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

   104

   105

void MasterDevice::getConfigPdo(

   106

        ec_ioctl_config_pdo_t *data,

   107

        unsigned int index,

   108

        uint8_t sync_index,

   109

        uint16_t pdo_pos

   110

        )

   111

{

   112

    data->config_index = index;

   113

    data->sync_index = sync_index;

   114

    data->pdo_pos = pdo_pos;

   115

   116

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

   117

        stringstream err;

   118

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

   119

        throw MasterDeviceException(err.str());

   120

    }

   121

}

   122

   123

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

   124

   125

void MasterDevice::getConfigPdoEntry(

   126

        ec_ioctl_config_pdo_entry_t *data,

   127

        unsigned int index,

   128

        uint8_t sync_index,

   129

        uint16_t pdo_pos,

   130

        uint8_t entry_pos

   131

        )

   132

{

   133

    data->config_index = index;

   134

    data->sync_index = sync_index;

   135

    data->pdo_pos = pdo_pos;

   136

    data->entry_pos = entry_pos;

   137

   138

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

   139

        stringstream err;

   140

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

   141

        throw MasterDeviceException(err.str());

   142

    }

   143

}

   144

   145

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

   146

   147

void MasterDevice::getConfigSdo(

   148

        ec_ioctl_config_sdo_t *data,

   149

        unsigned int index,

   150

        unsigned int sdo_pos

   151

        )

   152

{

   153

    data->config_index = index;

   154

    data->sdo_pos = sdo_pos;

   155

   156

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

   157

        stringstream err;

   158

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

   159

        throw MasterDeviceException(err.str());

   160

    }

   161

}

   162

   163

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

   164

   165

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

   166

{

   167

    data->index = index;

   168

   169

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

   170

        stringstream err;

   171

        err << "Failed to get domain: ";

   172

        if (errno == EINVAL)

   173

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

   174

        else

   175

            err << strerror(errno);

   176

        throw MasterDeviceException(err.str());

   177

    }

   178

}

   179

   180

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

   181

   182

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   183

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

   184

{

   185

    data->domain_index = domainIndex;

   186

    data->data_size = dataSize;

   187

    data->target = mem;

   188

   189

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

   190

        stringstream err;

   191

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

   192

        throw MasterDeviceException(err.str());

   193

    }

   194

}

   195

   196

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

   197

   198

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

   199

{

   200

    slave->position = slaveIndex;

   201

   202

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

   203

        stringstream err;

   204

        err << "Failed to get slave: ";

   205

        if (errno == EINVAL)

   206

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

   207

        else

   208

            err << strerror(errno);

   209

        throw MasterDeviceException(err.str());

   210

    }

   211

}

   212

   213

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

   214

   215

void MasterDevice::getFmmu(

   216

        ec_ioctl_domain_fmmu_t *fmmu,

   217

        unsigned int domainIndex,

   218

        unsigned int fmmuIndex

   219

        )

   220

{

   221

    fmmu->domain_index = domainIndex;

   222

    fmmu->fmmu_index = fmmuIndex;

   223

   224

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

   225

        stringstream err;

   226

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

   227

        throw MasterDeviceException(err.str());

   228

    }

   229

}

   230

   231

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

   232

   233

void MasterDevice::getSync(

   234

        ec_ioctl_slave_sync_t *sync,

   235

        uint16_t slaveIndex,

   236

        uint8_t syncIndex

   237

        )

   238

{

   239

    sync->slave_position = slaveIndex;

   240

    sync->sync_index = syncIndex;

   241

   242

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

   243

        stringstream err;

   244

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

   245

        throw MasterDeviceException(err.str());

   246

    }

   247

}

   248

   249

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

   250

   251

void MasterDevice::getPdo(

   252

        ec_ioctl_slave_sync_pdo_t *pdo,

   253

        uint16_t slaveIndex,

   254

        uint8_t syncIndex,

   255

        uint8_t pdoPos

   256

        )

   257

{

   258

    pdo->slave_position = slaveIndex;

   259

    pdo->sync_index = syncIndex;

   260

    pdo->pdo_pos = pdoPos;

   261

   262

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

   263

        stringstream err;

   264

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

   265

        throw MasterDeviceException(err.str());

   266

    }

   267

}

   268

   269

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

   270

   271

void MasterDevice::getPdoEntry(

   272

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   273

        uint16_t slaveIndex,

   274

        uint8_t syncIndex,

   275

        uint8_t pdoPos,

   276

        uint8_t entryPos

   277

        )

   278

{

   279

    entry->slave_position = slaveIndex;

   280

    entry->sync_index = syncIndex;

   281

    entry->pdo_pos = pdoPos;

   282

    entry->entry_pos = entryPos;

   283

   284

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

   285

        stringstream err;

   286

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

   287

        throw MasterDeviceException(err.str());

   288

    }

   289

}

   290

   291

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

   292

   293

void MasterDevice::getSdo(

   294

        ec_ioctl_slave_sdo_t *sdo,

   295

        uint16_t slaveIndex,

   296

        uint16_t sdoPosition

   297

        )

   298

{

   299

    sdo->slave_position = slaveIndex;

   300

    sdo->sdo_position = sdoPosition;

   301

   302

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

   303

        stringstream err;

   304

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

   305

        throw MasterDeviceException(err.str());

   306

    }

   307

}

   308

   309

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

   310

   311

void MasterDevice::getSdoEntry(

   312

        ec_ioctl_slave_sdo_entry_t *entry,

   313

        uint16_t slaveIndex,

   314

        int sdoSpec,

   315

        uint8_t entrySubindex

   316

        )

   317

{

   318

    entry->slave_position = slaveIndex;

   319

    entry->sdo_spec = sdoSpec;

   320

    entry->sdo_entry_subindex = entrySubindex;

   321

   322

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

   323

        stringstream err;

   324

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

   325

        throw MasterDeviceException(err.str());

   326

    }

   327

}

   328

   329

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

   330

   331

void MasterDevice::readSii(

   332

        ec_ioctl_slave_sii_t *data

   333

        )

   334

{

   335

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

   336

        stringstream err;

   337

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

   338

        throw MasterDeviceException(err.str());

   339

    }

   340

}

   341

   342

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

   343

   344

void MasterDevice::writeSii(

   345

        ec_ioctl_slave_sii_t *data

   346

        )

   347

{

   348

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

   349

        stringstream err;

   350

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

   351

        throw MasterDeviceException(err.str());

   352

    }

   353

}

   354

   355

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

   356

   357

void MasterDevice::setDebug(unsigned int debugLevel)

   358

{

   359

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

   360

        stringstream err;

   361

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

   362

        throw MasterDeviceException(err.str());

   363

	}

   364

}

   365

   366

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

   367

   368

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   369

{

   370

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

   371

        stringstream err;

   372

        err << "Failed to download Sdo: ";

   373

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

   374

            err << "Abort code 0x" << hex << setfill('0')

   375

                << setw(8) << data->abort_code;

   376

        } else {

   377

            err << strerror(errno);

   378

        }

   379

        throw MasterDeviceException(err.str());

   380

	}

   381

}

   382

   383

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

   384

   385

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   386

{

   388

        stringstream err;

   389

        err << "Failed to upload Sdo: ";

   390

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

   391

            err << "Abort code 0x" << hex << setfill('0')

   392

                << setw(8) << data->abort_code;

   393

        } else {

   394

            err << strerror(errno);

   395

        }

   396

        throw MasterDeviceException(err.str());

   397

    }

   398

}

   399

   400

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

   401

   402

void MasterDevice::requestState(

   403

        uint16_t slavePosition,

   404

        uint8_t state

   405

        )

   406

{

   407

    ec_ioctl_slave_state_t data;

   408

   409

    data.slave_position = slavePosition;

   410

    data.requested_state = state;

   411

   412

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

   413

        stringstream err;

   414

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

   415

        if (errno == EINVAL)

   416

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

   417

        else

   418

            err << strerror(errno);

   419

        throw MasterDeviceException(err.str());

   420

    }

   421

}

   422

   423

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