Fixed SoE opcode determination.
/*****************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License version 2, as
* published by the Free Software Foundation.
*
* The IgH EtherCAT Master is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* ---
*
* The license mentioned above concerns the source code only. Using the
* EtherCAT technology and brand is only permitted in compliance with the
* industrial property and similar rights of Beckhoff Automation GmbH.
*
****************************************************************************/
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <string.h>
#include <unistd.h>
#include <sstream>
#include <iomanip>
using namespace std;
#include "MasterDevice.h"
/****************************************************************************/
MasterDevice::MasterDevice(unsigned int index):
index(index),
masterCount(0U),
fd(-1)
{
}
/****************************************************************************/
MasterDevice::~MasterDevice()
{
close();
}
/****************************************************************************/
void MasterDevice::setIndex(unsigned int i)
{
index = i;
}
/****************************************************************************/
void MasterDevice::open(Permissions perm)
{
stringstream deviceName;
if (fd == -1) { // not already open
ec_ioctl_module_t module_data;
deviceName << "/dev/EtherCAT" << index;
if ((fd = ::open(deviceName.str().c_str(),
perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
stringstream err;
err << "Failed to open master device " << deviceName.str() << ": "
<< strerror(errno);
throw MasterDeviceException(err);
}
getModule(&module_data);
if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {
stringstream err;
err << "ioctl() version magic is differing: "
<< deviceName << ": " << module_data.ioctl_version_magic
<< ", ethercat tool: " << EC_IOCTL_VERSION_MAGIC;
throw MasterDeviceException(err);
}
masterCount = module_data.master_count;
}
}
/****************************************************************************/
void MasterDevice::close()
{
if (fd != -1) {
::close(fd);
fd = -1;
}
}
/****************************************************************************/
void MasterDevice::getModule(ec_ioctl_module_t *data)
{
if (ioctl(fd, EC_IOCTL_MODULE, data) < 0) {
stringstream err;
err << "Failed to get module information: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getMaster(ec_ioctl_master_t *data)
{
if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
stringstream err;
err << "Failed to get master information: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getConfig(ec_ioctl_config_t *data, unsigned int index)
{
data->config_index = index;
if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {
stringstream err;
err << "Failed to get slave configuration: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getConfigPdo(
ec_ioctl_config_pdo_t *data,
unsigned int index,
uint8_t sync_index,
uint16_t pdo_pos
)
{
data->config_index = index;
data->sync_index = sync_index;
data->pdo_pos = pdo_pos;
if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {
stringstream err;
err << "Failed to get slave config PDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getConfigPdoEntry(
ec_ioctl_config_pdo_entry_t *data,
unsigned int index,
uint8_t sync_index,
uint16_t pdo_pos,
uint8_t entry_pos
)
{
data->config_index = index;
data->sync_index = sync_index;
data->pdo_pos = pdo_pos;
data->entry_pos = entry_pos;
if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {
stringstream err;
err << "Failed to get slave config PDO entry: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getConfigSdo(
ec_ioctl_config_sdo_t *data,
unsigned int index,
unsigned int sdo_pos
)
{
data->config_index = index;
data->sdo_pos = sdo_pos;
if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {
stringstream err;
err << "Failed to get slave config SDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getDomain(ec_ioctl_domain_t *data, unsigned int index)
{
data->index = index;
if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
stringstream err;
err << "Failed to get domain: ";
if (errno == EINVAL)
err << "Domain " << index << " does not exist!";
else
err << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getData(ec_ioctl_domain_data_t *data,
unsigned int domainIndex, unsigned int dataSize, unsigned char *mem)
{
data->domain_index = domainIndex;
data->data_size = dataSize;
data->target = mem;
if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {
stringstream err;
err << "Failed to get domain data: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
{
slave->position = slaveIndex;
if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
stringstream err;
err << "Failed to get slave: ";
if (errno == EINVAL)
err << "Slave " << slaveIndex << " does not exist!";
else
err << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getFmmu(
ec_ioctl_domain_fmmu_t *fmmu,
unsigned int domainIndex,
unsigned int fmmuIndex
)
{
fmmu->domain_index = domainIndex;
fmmu->fmmu_index = fmmuIndex;
if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
stringstream err;
err << "Failed to get domain FMMU: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getSync(
ec_ioctl_slave_sync_t *sync,
uint16_t slaveIndex,
uint8_t syncIndex
)
{
sync->slave_position = slaveIndex;
sync->sync_index = syncIndex;
if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) {
stringstream err;
err << "Failed to get sync manager: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getPdo(
ec_ioctl_slave_sync_pdo_t *pdo,
uint16_t slaveIndex,
uint8_t syncIndex,
uint8_t pdoPos
)
{
pdo->slave_position = slaveIndex;
pdo->sync_index = syncIndex;
pdo->pdo_pos = pdoPos;
if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) {
stringstream err;
err << "Failed to get PDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getPdoEntry(
ec_ioctl_slave_sync_pdo_entry_t *entry,
uint16_t slaveIndex,
uint8_t syncIndex,
uint8_t pdoPos,
uint8_t entryPos
)
{
entry->slave_position = slaveIndex;
entry->sync_index = syncIndex;
entry->pdo_pos = pdoPos;
entry->entry_pos = entryPos;
if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) {
stringstream err;
err << "Failed to get PDO entry: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getSdo(
ec_ioctl_slave_sdo_t *sdo,
uint16_t slaveIndex,
uint16_t sdoPosition
)
{
sdo->slave_position = slaveIndex;
sdo->sdo_position = sdoPosition;
if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {
stringstream err;
err << "Failed to get SDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::getSdoEntry(
ec_ioctl_slave_sdo_entry_t *entry,
uint16_t slaveIndex,
int sdoSpec,
uint8_t entrySubindex
)
{
entry->slave_position = slaveIndex;
entry->sdo_spec = sdoSpec;
entry->sdo_entry_subindex = entrySubindex;
if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) {
stringstream err;
err << "Failed to get SDO entry: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::readSii(
ec_ioctl_slave_sii_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, data) < 0) {
stringstream err;
err << "Failed to read SII: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::writeSii(
ec_ioctl_slave_sii_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, data) < 0) {
stringstream err;
err << "Failed to write SII: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::readReg(
ec_ioctl_slave_reg_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_REG_READ, data) < 0) {
stringstream err;
err << "Failed to read register: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::writeReg(
ec_ioctl_slave_reg_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_REG_WRITE, data) < 0) {
stringstream err;
err << "Failed to write register: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::readFoe(
ec_ioctl_slave_foe_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_FOE_READ, data) < 0) {
stringstream err;
err << "Failed to read via FoE: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::writeFoe(
ec_ioctl_slave_foe_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_FOE_WRITE, data) < 0) {
stringstream err;
err << "Failed to write via FoE: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::setDebug(unsigned int debugLevel)
{
if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {
stringstream err;
err << "Failed to set debug level: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, data) < 0) {
stringstream err;
if (errno == EIO && data->abort_code) {
throw MasterDeviceSdoAbortException(data->abort_code);
} else {
err << "Failed to download SDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
}
/****************************************************************************/
void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, data) < 0) {
stringstream err;
if (errno == EIO && data->abort_code) {
throw MasterDeviceSdoAbortException(data->abort_code);
} else {
err << "Failed to upload SDO: " << strerror(errno);
throw MasterDeviceException(err);
}
}
}
/****************************************************************************/
void MasterDevice::requestState(
uint16_t slavePosition,
uint8_t state
)
{
ec_ioctl_slave_state_t data;
data.slave_position = slavePosition;
data.al_state = state;
if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
stringstream err;
err << "Failed to request slave state: ";
if (errno == EINVAL)
err << "Slave " << slavePosition << " does not exist!";
else
err << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
#ifdef EC_EOE
void MasterDevice::getEoeHandler(
ec_ioctl_eoe_handler_t *eoe,
uint16_t eoeHandlerIndex
)
{
eoe->eoe_index = eoeHandlerIndex;
if (ioctl(fd, EC_IOCTL_EOE_HANDLER, eoe)) {
stringstream err;
err << "Failed to get EoE handler: " << strerror(errno);
throw MasterDeviceException(err);
}
}
#endif
/****************************************************************************/
void MasterDevice::readSoe(ec_ioctl_slave_soe_read_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SOE_READ, data) < 0) {
if (errno == EIO && data->error_code) {
throw MasterDeviceSoeException(data->error_code);
} else {
stringstream err;
err << "Failed to read IDN: " << strerror(errno);
throw MasterDeviceException(err);
}
}
}
/****************************************************************************/
void MasterDevice::writeSoe(ec_ioctl_slave_soe_write_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SOE_WRITE, data) < 0) {
if (errno == EIO && data->error_code) {
throw MasterDeviceSoeException(data->error_code);
} else {
stringstream err;
err << "Failed to write IDN: " << strerror(errno);
throw MasterDeviceException(err);
}
}
}
/*****************************************************************************/