fp@1122: /*****************************************************************************
fp@1122:  *
fp@1122:  * $Id$
fp@1122:  *
fp@1122:  ****************************************************************************/
fp@1122: 
fp@1122: #include <sys/stat.h>
fp@1122: #include <fcntl.h>
fp@1122: #include <errno.h>
fp@1122: #include <sys/ioctl.h>
fp@1636: #include <string.h>
fp@1643: #include <unistd.h>
fp@1122: 
fp@1122: #include <sstream>
fp@1126: #include <iomanip>
fp@1122: using namespace std;
fp@1122: 
fp@1122: #include "MasterDevice.h"
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: MasterDevice::MasterDevice()
fp@1122: {
fp@1122:     index = 0;
fp@1122:     fd = -1;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: MasterDevice::~MasterDevice()
fp@1122: {
fp@1122:     close();
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::setIndex(unsigned int i)
fp@1122: {
fp@1122:     index = i;
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::open(Permissions perm)
fp@1122: {
fp@1122:     stringstream deviceName;
fp@1122: 
fp@1126:     if (fd == -1) { // not already open
fp@1126: 		deviceName << "/dev/EtherCAT" << index;
fp@1126: 
fp@1126: 		if ((fd = ::open(deviceName.str().c_str(),
fp@1126: 						perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1) {
fp@1126: 			stringstream err;
fp@1126: 			err << "Failed to open master device " << deviceName.str() << ": "
fp@1126: 				<< strerror(errno);
fp@1136: 			throw MasterDeviceException(err);
fp@1126: 		}
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::close()
fp@1122: {
fp@1126:     if (fd != -1) {
fp@1126: 		::close(fd);
fp@1126: 		fd = -1;
fp@1126: 	}
fp@1126: }
fp@1126: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getMaster(ec_ioctl_master_t *data)
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_MASTER, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get master information: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfig(ec_ioctl_config_t *data, unsigned int index)
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave configuration: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigPdo(
fp@1122:         ec_ioctl_config_pdo_t *data,
fp@1122:         unsigned int index,
fp@1122:         uint8_t sync_index,
fp@1122:         uint16_t pdo_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sync_index = sync_index;
fp@1122:     data->pdo_pos = pdo_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_PDO, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave config Pdo: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigPdoEntry(
fp@1122:         ec_ioctl_config_pdo_entry_t *data,
fp@1122:         unsigned int index,
fp@1122:         uint8_t sync_index,
fp@1122:         uint16_t pdo_pos,
fp@1122:         uint8_t entry_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sync_index = sync_index;
fp@1122:     data->pdo_pos = pdo_pos;
fp@1122:     data->entry_pos = entry_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_PDO_ENTRY, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave config Pdo entry: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getConfigSdo(
fp@1122:         ec_ioctl_config_sdo_t *data,
fp@1122:         unsigned int index,
fp@1122:         unsigned int sdo_pos
fp@1122:         )
fp@1122: {
fp@1122:     data->config_index = index;
fp@1122:     data->sdo_pos = sdo_pos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_CONFIG_SDO, data) < 0) {
fp@1122:         stringstream err;
fp@1686:         err << "Failed to get slave config SDO: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getDomain(ec_ioctl_domain_t *data, unsigned int index)
fp@1122: {
fp@1122:     data->index = index;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN, data)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get domain: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Domain " << index << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1126: void MasterDevice::getData(ec_ioctl_domain_data_t *data,
fp@1126:         unsigned int domainIndex, unsigned int dataSize, unsigned char *mem)
fp@1122: {
fp@1122:     data->domain_index = domainIndex;
fp@1122:     data->data_size = dataSize;
fp@1122:     data->target = mem;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN_DATA, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get domain data: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
fp@1122: {
fp@1122:     slave->position = slaveIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to get slave: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Slave " << slaveIndex << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getFmmu(
fp@1122:         ec_ioctl_domain_fmmu_t *fmmu,
fp@1122:         unsigned int domainIndex,
fp@1122:         unsigned int fmmuIndex
fp@1122:         )
fp@1122: {
fp@1122:     fmmu->domain_index = domainIndex;
fp@1122:     fmmu->fmmu_index = fmmuIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_DOMAIN_FMMU, fmmu)) {
fp@1122:         stringstream err;
fp@1126:         err << "Failed to get domain FMMU: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSync(
fp@1122:         ec_ioctl_slave_sync_t *sync,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex
fp@1122:         )
fp@1122: {
fp@1122:     sync->slave_position = slaveIndex;
fp@1122:     sync->sync_index = syncIndex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC, sync)) {
fp@1122:         stringstream err;
fp@1126:         err << "Failed to get sync manager: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getPdo(
fp@1122:         ec_ioctl_slave_sync_pdo_t *pdo,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex,
fp@1122:         uint8_t pdoPos
fp@1122:         )
fp@1122: {
fp@1122:     pdo->slave_position = slaveIndex;
fp@1122:     pdo->sync_index = syncIndex;
fp@1122:     pdo->pdo_pos = pdoPos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO, pdo)) {
fp@1122:         stringstream err;
fp@1126:         err << "Failed to get Pdo: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getPdoEntry(
fp@1122:         ec_ioctl_slave_sync_pdo_entry_t *entry,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint8_t syncIndex,
fp@1122:         uint8_t pdoPos,
fp@1122:         uint8_t entryPos
fp@1122:         )
fp@1122: {
fp@1122:     entry->slave_position = slaveIndex;
fp@1122:     entry->sync_index = syncIndex;
fp@1122:     entry->pdo_pos = pdoPos;
fp@1122:     entry->entry_pos = entryPos;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, entry)) {
fp@1122:         stringstream err;
fp@1126:         err << "Failed to get Pdo entry: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSdo(
fp@1122:         ec_ioctl_slave_sdo_t *sdo,
fp@1122:         uint16_t slaveIndex,
fp@1122:         uint16_t sdoPosition
fp@1122:         )
fp@1122: {
fp@1122:     sdo->slave_position = slaveIndex;
fp@1122:     sdo->sdo_position = sdoPosition;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO, sdo)) {
fp@1122:         stringstream err;
fp@1686:         err << "Failed to get SDO: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::getSdoEntry(
fp@1122:         ec_ioctl_slave_sdo_entry_t *entry,
fp@1122:         uint16_t slaveIndex,
fp@1122:         int sdoSpec,
fp@1122:         uint8_t entrySubindex
fp@1122:         )
fp@1122: {
fp@1122:     entry->slave_position = slaveIndex;
fp@1122:     entry->sdo_spec = sdoSpec;
fp@1122:     entry->sdo_entry_subindex = entrySubindex;
fp@1122: 
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_ENTRY, entry)) {
fp@1122:         stringstream err;
fp@1686:         err << "Failed to get SDO entry: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::readSii(
fp@1122:         ec_ioctl_slave_sii_t *data
fp@1122:         )
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_READ, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to read SII: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1122: void MasterDevice::writeSii(
fp@1122:         ec_ioctl_slave_sii_t *data
fp@1122:         )
fp@1122: {
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_SII_WRITE, data) < 0) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to write SII: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /****************************************************************************/
fp@1122: 
fp@1707: void MasterDevice::readFoe(
fp@1707:         ec_ioctl_slave_foe_t *data
fp@1707:         )
fp@1707: {
fp@1707:     if (ioctl(fd, EC_IOCTL_SLAVE_FOE_READ, data) < 0) {
fp@1707:         stringstream err;
fp@1707:         err << "Failed to read via FoE: " << strerror(errno);
fp@1707:         throw MasterDeviceException(err);
fp@1707:     }
fp@1707: }
fp@1707: 
fp@1707: /****************************************************************************/
fp@1707: 
fp@1707: void MasterDevice::writeFoe(
fp@1707:         ec_ioctl_slave_foe_t *data
fp@1707:         )
fp@1707: {
fp@1707:     if (ioctl(fd, EC_IOCTL_SLAVE_FOE_WRITE, data) < 0) {
fp@1707:         stringstream err;
fp@1707:         err << "Failed to write via FoE: " << strerror(errno);
fp@1707:         throw MasterDeviceException(err);
fp@1707:     }
fp@1707: }
fp@1707: 
fp@1707: /****************************************************************************/
fp@1707: 
fp@1126: void MasterDevice::setDebug(unsigned int debugLevel)
fp@1126: {
fp@1126:     if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {
fp@1126:         stringstream err;
fp@1126:         err << "Failed to set debug level: " << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1126: 	}
fp@1126: }
fp@1126: 
fp@1126: /****************************************************************************/
fp@1126: 
fp@1126: void MasterDevice::sdoDownload(ec_ioctl_slave_sdo_download_t *data)
fp@1126: {
fp@1126:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, data) < 0) {
fp@1126:         stringstream err;
fp@1126:         if (errno == EIO && data->abort_code) {
fp@1184:             throw MasterDeviceSdoAbortException(data->abort_code);
fp@1126:         } else {
fp@1686:             err << "Failed to download SDO: " << strerror(errno);
fp@1184:             throw MasterDeviceException(err);
fp@1126:         }
fp@1126: 	}
fp@1126: }
fp@1126: 
fp@1126: /****************************************************************************/
fp@1126: 
fp@1126: void MasterDevice::sdoUpload(ec_ioctl_slave_sdo_upload_t *data)
fp@1126: {
fp@1135:     if (ioctl(fd, EC_IOCTL_SLAVE_SDO_UPLOAD, data) < 0) {
fp@1126:         stringstream err;
fp@1126:         if (errno == EIO && data->abort_code) {
fp@1184:             throw MasterDeviceSdoAbortException(data->abort_code);
fp@1126:         } else {
fp@1686:             err << "Failed to upload SDO: " << strerror(errno);
fp@1184:             throw MasterDeviceException(err);
fp@1126:         }
fp@1126:     }
fp@1126: }
fp@1126: 
fp@1126: /****************************************************************************/
fp@1126: 
fp@1122: void MasterDevice::requestState(
fp@1122:         uint16_t slavePosition,
fp@1122:         uint8_t state
fp@1122:         )
fp@1122: {
fp@1122:     ec_ioctl_slave_state_t data;
fp@1122: 
fp@1122:     data.slave_position = slavePosition;
fp@1148:     data.al_state = state;
fp@1122:     
fp@1122:     if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
fp@1122:         stringstream err;
fp@1122:         err << "Failed to request slave state: ";
fp@1122:         if (errno == EINVAL)
fp@1122:             err << "Slave " << slavePosition << " does not exist!";
fp@1122:         else
fp@1122:             err << strerror(errno);
fp@1136:         throw MasterDeviceException(err);
fp@1122:     }
fp@1122: }
fp@1122: 
fp@1122: /*****************************************************************************/