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

#include <string.h>

    12

#include <unistd.h>

    13

    14

#include <sstream>

    15

#include <iomanip>

    16

using namespace std;

    17

    18

#include "MasterDevice.h"

    19

    20

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

    21

    22

MasterDevice::MasterDevice()

    23

{

    24

    index = 0;

    25

    fd = -1;

    26

}

    27

    28

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

    29

    30

MasterDevice::~MasterDevice()

    31

{

    32

    close();

    33

}

    34

    35

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

    36

    37

void MasterDevice::setIndex(unsigned int i)

    38

{

    39

    index = i;

    40

}

    41

    42

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

    43

    44

void MasterDevice::open(Permissions perm)

    45

{

    46

    stringstream deviceName;

    47

    48

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

    49

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

    50

    51

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

    52

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

    53

			stringstream err;

    54

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

    55

				<< strerror(errno);

    56

			throw MasterDeviceException(err);

    57

		}

    58

    }

    59

}

    60

    61

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

    62

    63

void MasterDevice::close()

    64

{

    65

    if (fd != -1) {

    66

		::close(fd);

    67

		fd = -1;

    68

	}

    69

}

    70

    71

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

    72

    73

void MasterDevice::getMaster(ec_ioctl_master_t *data)

    74

{

    75

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

    76

        stringstream err;

    77

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

    78

        throw MasterDeviceException(err);

    79

    }

    80

}

    81

    82

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

    83

    84

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

    85

{

    86

    data->config_index = index;

    87

    88

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

    89

        stringstream err;

    90

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

    91

        throw MasterDeviceException(err);

    92

    }

    93

}

    94

    95

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

    96

    97

void MasterDevice::getConfigPdo(

    98

        ec_ioctl_config_pdo_t *data,

    99

        unsigned int index,

   100

        uint8_t sync_index,

   101

        uint16_t pdo_pos

   102

        )

   103

{

   104

    data->config_index = index;

   105

    data->sync_index = sync_index;

   106

    data->pdo_pos = pdo_pos;

   107

   108

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

   109

        stringstream err;

   110

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

   111

        throw MasterDeviceException(err);

   112

    }

   113

}

   114

   115

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

   116

   117

void MasterDevice::getConfigPdoEntry(

   118

        ec_ioctl_config_pdo_entry_t *data,

   119

        unsigned int index,

   120

        uint8_t sync_index,

   121

        uint16_t pdo_pos,

   122

        uint8_t entry_pos

   123

        )

   124

{

   125

    data->config_index = index;

   126

    data->sync_index = sync_index;

   127

    data->pdo_pos = pdo_pos;

   128

    data->entry_pos = entry_pos;

   129

   130

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

   131

        stringstream err;

   132

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

   133

        throw MasterDeviceException(err);

   134

    }

   135

}

   136

   137

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

   138

   139

void MasterDevice::getConfigSdo(

   140

        ec_ioctl_config_sdo_t *data,

   141

        unsigned int index,

   142

        unsigned int sdo_pos

   143

        )

   144

{

   145

    data->config_index = index;

   146

    data->sdo_pos = sdo_pos;

   147

   148

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

   149

        stringstream err;

   150

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

   151

        throw MasterDeviceException(err);

   152

    }

   153

}

   154

   155

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

   156

   157

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

   158

{

   159

    data->index = index;

   160

   161

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

   162

        stringstream err;

   163

        err << "Failed to get domain: ";

   164

        if (errno == EINVAL)

   165

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

   166

        else

   167

            err << strerror(errno);

   168

        throw MasterDeviceException(err);

   169

    }

   170

}

   171

   172

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

   173

   174

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   175

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

   176

{

   177

    data->domain_index = domainIndex;

   178

    data->data_size = dataSize;

   179

    data->target = mem;

   180

   181

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

   182

        stringstream err;

   183

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

   184

        throw MasterDeviceException(err);

   185

    }

   186

}

   187

   188

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

   189

   190

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

   191

{

   192

    slave->position = slaveIndex;

   193

   194

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

   195

        stringstream err;

   196

        err << "Failed to get slave: ";

   197

        if (errno == EINVAL)

   198

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

   199

        else

   200

            err << strerror(errno);

   201

        throw MasterDeviceException(err);

   202

    }

   203

}

   204

   205

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

   206

   207

void MasterDevice::getFmmu(

   208

        ec_ioctl_domain_fmmu_t *fmmu,

   209

        unsigned int domainIndex,

   210

        unsigned int fmmuIndex

   211

        )

   212

{

   213

    fmmu->domain_index = domainIndex;

   214

    fmmu->fmmu_index = fmmuIndex;

   215

   216

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

   217

        stringstream err;

   218

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

   219

        throw MasterDeviceException(err);

   220

    }

   221

}

   222

   223

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

   224

   225

void MasterDevice::getSync(

   226

        ec_ioctl_slave_sync_t *sync,

   227

        uint16_t slaveIndex,

   228

        uint8_t syncIndex

   229

        )

   230

{

   231

    sync->slave_position = slaveIndex;

   232

    sync->sync_index = syncIndex;

   233

   234

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

   235

        stringstream err;

   236

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

   237

        throw MasterDeviceException(err);

   238

    }

   239

}

   240

   241

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

   242

   243

void MasterDevice::getPdo(

   244

        ec_ioctl_slave_sync_pdo_t *pdo,

   245

        uint16_t slaveIndex,

   246

        uint8_t syncIndex,

   247

        uint8_t pdoPos

   248

        )

   249

{

   250

    pdo->slave_position = slaveIndex;

   251

    pdo->sync_index = syncIndex;

   252

    pdo->pdo_pos = pdoPos;

   253

   254

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

   255

        stringstream err;

   256

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

   257

        throw MasterDeviceException(err);

   258

    }

   259

}

   260

   261

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

   262

   263

void MasterDevice::getPdoEntry(

   264

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   265

        uint16_t slaveIndex,

   266

        uint8_t syncIndex,

   267

        uint8_t pdoPos,

   268

        uint8_t entryPos

   269

        )

   270

{

   271

    entry->slave_position = slaveIndex;

   272

    entry->sync_index = syncIndex;

   273

    entry->pdo_pos = pdoPos;

   274

    entry->entry_pos = entryPos;

   275

   276

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

   277

        stringstream err;

   278

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

   279

        throw MasterDeviceException(err);

   280

    }

   281

}

   282

   283

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

   284

   285

void MasterDevice::getSdo(

   286

        ec_ioctl_slave_sdo_t *sdo,

   287

        uint16_t slaveIndex,

   288

        uint16_t sdoPosition

   289

        )

   290

{

   291

    sdo->slave_position = slaveIndex;

   292

    sdo->sdo_position = sdoPosition;

   293

   294

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

   295

        stringstream err;

   296

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

   297

        throw MasterDeviceException(err);

   298

    }

   299

}

   300

   301

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

   302

   303

void MasterDevice::getSdoEntry(

   304

        ec_ioctl_slave_sdo_entry_t *entry,

   305

        uint16_t slaveIndex,

   306

        int sdoSpec,

   307

        uint8_t entrySubindex

   308

        )

   309

{

   310

    entry->slave_position = slaveIndex;

   311

    entry->sdo_spec = sdoSpec;

   312

    entry->sdo_entry_subindex = entrySubindex;

   313

   314

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

   315

        stringstream err;

   316

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

   317

        throw MasterDeviceException(err);

   318

    }

   319

}

   320

   321

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

   322

   323

void MasterDevice::readSii(

   324

        ec_ioctl_slave_sii_t *data

   325

        )

   326

{

   327

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

   328

        stringstream err;

   329

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

   330

        throw MasterDeviceException(err);

   331

    }

   332

}

   333

   334

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

   335

   336

void MasterDevice::writeSii(

   337

        ec_ioctl_slave_sii_t *data

   338

        )

   339

{

   340

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

   341

        stringstream err;

   342

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

   343

        throw MasterDeviceException(err);

   344

    }

   345

}

   346

   347

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

   348

   349

void MasterDevice::setDebug(unsigned int debugLevel)

   350

{

   351

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

   352

        stringstream err;

   353

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

   354

        throw MasterDeviceException(err);

   355

	}

   356

}

   357

   358

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

   359

   360

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   361

{

   362

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

   363

        stringstream err;

   364

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

   365

            throw MasterDeviceSdoAbortException(data->abort_code);

   366

        } else {

   367

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

   368

            throw MasterDeviceException(err);

   369

        }

   370

	}

   371

}

   372

   373

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

   374

   375

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   376

{

   377

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

   378

        stringstream err;

   379

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

   380

            throw MasterDeviceSdoAbortException(data->abort_code);

   381

        } else {

   382

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

   383

            throw MasterDeviceException(err);

   384

        }

   385

    }

   386

}

   387

   388

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

   389

   390

void MasterDevice::requestState(

   391

        uint16_t slavePosition,

   392

        uint8_t state

   393

        )

   394

{

   395

    ec_ioctl_slave_state_t data;

   396

   397

    data.slave_position = slavePosition;

   398

    data.al_state = state;

   399

   400

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

   401

        stringstream err;

   402

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

   403

        if (errno == EINVAL)

   404

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

   405

        else

   406

            err << strerror(errno);

   407

        throw MasterDeviceException(err);

   408

    }

   409

}

   410

   411

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