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

    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

void MasterDevice::getMaster(ec_ioctl_master_t *data)

    72

{

    73

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

    74

        stringstream err;

    75

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

    76

        throw MasterDeviceException(err);

    77

    }

    78

}

    79

    80

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

    81

    82

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

    83

{

    84

    data->config_index = index;

    85

    86

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

    87

        stringstream err;

    88

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

    89

        throw MasterDeviceException(err);

    90

    }

    91

}

    92

    93

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

    94

    95

void MasterDevice::getConfigPdo(

    96

        ec_ioctl_config_pdo_t *data,

    97

        unsigned int index,

    98

        uint8_t sync_index,

    99

        uint16_t pdo_pos

   100

        )

   101

{

   102

    data->config_index = index;

   103

    data->sync_index = sync_index;

   104

    data->pdo_pos = pdo_pos;

   105

   106

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

   107

        stringstream err;

   108

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

   109

        throw MasterDeviceException(err);

   110

    }

   111

}

   112

   113

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

   114

   115

void MasterDevice::getConfigPdoEntry(

   116

        ec_ioctl_config_pdo_entry_t *data,

   117

        unsigned int index,

   118

        uint8_t sync_index,

   119

        uint16_t pdo_pos,

   120

        uint8_t entry_pos

   121

        )

   122

{

   123

    data->config_index = index;

   124

    data->sync_index = sync_index;

   125

    data->pdo_pos = pdo_pos;

   126

    data->entry_pos = entry_pos;

   127

   128

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

   129

        stringstream err;

   130

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

   131

        throw MasterDeviceException(err);

   132

    }

   133

}

   134

   135

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

   136

   137

void MasterDevice::getConfigSdo(

   138

        ec_ioctl_config_sdo_t *data,

   139

        unsigned int index,

   140

        unsigned int sdo_pos

   141

        )

   142

{

   143

    data->config_index = index;

   144

    data->sdo_pos = sdo_pos;

   145

   146

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

   147

        stringstream err;

   148

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

   149

        throw MasterDeviceException(err);

   150

    }

   151

}

   152

   153

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

   154

   155

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

   156

{

   157

    data->index = index;

   158

   159

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

   160

        stringstream err;

   161

        err << "Failed to get domain: ";

   162

        if (errno == EINVAL)

   163

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

   164

        else

   165

            err << strerror(errno);

   166

        throw MasterDeviceException(err);

   167

    }

   168

}

   169

   170

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

   171

   172

void MasterDevice::getData(ec_ioctl_domain_data_t *data,

   173

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

   174

{

   175

    data->domain_index = domainIndex;

   176

    data->data_size = dataSize;

   177

    data->target = mem;

   178

   179

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

   180

        stringstream err;

   181

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

   182

        throw MasterDeviceException(err);

   183

    }

   184

}

   185

   186

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

   187

   188

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

   189

{

   190

    slave->position = slaveIndex;

   191

   192

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

   193

        stringstream err;

   194

        err << "Failed to get slave: ";

   195

        if (errno == EINVAL)

   196

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

   197

        else

   198

            err << strerror(errno);

   199

        throw MasterDeviceException(err);

   200

    }

   201

}

   202

   203

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

   204

   205

void MasterDevice::getFmmu(

   206

        ec_ioctl_domain_fmmu_t *fmmu,

   207

        unsigned int domainIndex,

   208

        unsigned int fmmuIndex

   209

        )

   210

{

   211

    fmmu->domain_index = domainIndex;

   212

    fmmu->fmmu_index = fmmuIndex;

   213

   214

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

   215

        stringstream err;

   216

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

   217

        throw MasterDeviceException(err);

   218

    }

   219

}

   220

   221

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

   222

   223

void MasterDevice::getSync(

   224

        ec_ioctl_slave_sync_t *sync,

   225

        uint16_t slaveIndex,

   226

        uint8_t syncIndex

   227

        )

   228

{

   229

    sync->slave_position = slaveIndex;

   230

    sync->sync_index = syncIndex;

   231

   232

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

   233

        stringstream err;

   234

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

   235

        throw MasterDeviceException(err);

   236

    }

   237

}

   238

   239

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

   240

   241

void MasterDevice::getPdo(

   242

        ec_ioctl_slave_sync_pdo_t *pdo,

   243

        uint16_t slaveIndex,

   244

        uint8_t syncIndex,

   245

        uint8_t pdoPos

   246

        )

   247

{

   248

    pdo->slave_position = slaveIndex;

   249

    pdo->sync_index = syncIndex;

   250

    pdo->pdo_pos = pdoPos;

   251

   252

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

   253

        stringstream err;

   254

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

   255

        throw MasterDeviceException(err);

   256

    }

   257

}

   258

   259

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

   260

   261

void MasterDevice::getPdoEntry(

   262

        ec_ioctl_slave_sync_pdo_entry_t *entry,

   263

        uint16_t slaveIndex,

   264

        uint8_t syncIndex,

   265

        uint8_t pdoPos,

   266

        uint8_t entryPos

   267

        )

   268

{

   269

    entry->slave_position = slaveIndex;

   270

    entry->sync_index = syncIndex;

   271

    entry->pdo_pos = pdoPos;

   272

    entry->entry_pos = entryPos;

   273

   274

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

   275

        stringstream err;

   276

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

   277

        throw MasterDeviceException(err);

   278

    }

   279

}

   280

   281

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

   282

   283

void MasterDevice::getSdo(

   284

        ec_ioctl_slave_sdo_t *sdo,

   285

        uint16_t slaveIndex,

   286

        uint16_t sdoPosition

   287

        )

   288

{

   289

    sdo->slave_position = slaveIndex;

   290

    sdo->sdo_position = sdoPosition;

   291

   292

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

   293

        stringstream err;

   294

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

   295

        throw MasterDeviceException(err);

   296

    }

   297

}

   298

   299

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

   300

   301

void MasterDevice::getSdoEntry(

   302

        ec_ioctl_slave_sdo_entry_t *entry,

   303

        uint16_t slaveIndex,

   304

        int sdoSpec,

   305

        uint8_t entrySubindex

   306

        )

   307

{

   308

    entry->slave_position = slaveIndex;

   309

    entry->sdo_spec = sdoSpec;

   310

    entry->sdo_entry_subindex = entrySubindex;

   311

   312

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

   313

        stringstream err;

   314

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

   315

        throw MasterDeviceException(err);

   316

    }

   317

}

   318

   319

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

   320

   321

void MasterDevice::readSii(

   322

        ec_ioctl_slave_sii_t *data

   323

        )

   324

{

   325

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

   326

        stringstream err;

   327

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

   328

        throw MasterDeviceException(err);

   329

    }

   330

}

   331

   332

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

   333

   334

void MasterDevice::writeSii(

   335

        ec_ioctl_slave_sii_t *data

   336

        )

   337

{

   338

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

   339

        stringstream err;

   340

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

   341

        throw MasterDeviceException(err);

   342

    }

   343

}

   344

   345

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

   346

   347

void MasterDevice::setDebug(unsigned int debugLevel)

   348

{

   349

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

   350

        stringstream err;

   351

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

   352

        throw MasterDeviceException(err);

   353

	}

   354

}

   355

   356

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

   357

   358

void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)

   359

{

   360

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

   361

        stringstream err;

   362

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

   363

            throw MasterDeviceSdoAbortException(data->abort_code);

   364

        } else {

   365

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

   366

            throw MasterDeviceException(err);

   367

        }

   368

	}

   369

}

   370

   371

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

   372

   373

void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)

   374

{

   375

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

   376

        stringstream err;

   377

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

   378

            throw MasterDeviceSdoAbortException(data->abort_code);

   379

        } else {

   380

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

   381

            throw MasterDeviceException(err);

   382

        }

   383

    }

   384

}

   385

   386

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

   387

   388

void MasterDevice::requestState(

   389

        uint16_t slavePosition,

   390

        uint8_t state

   391

        )

   392

{

   393

    ec_ioctl_slave_state_t data;

   394

   395

    data.slave_position = slavePosition;

   396

    data.al_state = state;

   397

   398

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

   399

        stringstream err;

   400

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

   401

        if (errno == EINVAL)

   402

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

   403

        else

   404

            err << strerror(errno);

   405

        throw MasterDeviceException(err);

   406

    }

   407

}

   408

   409

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