/*****************************************************************************
*
* $Id$
*
****************************************************************************/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cctype> // toupper()
using namespace std;
#include "Master.h"
/****************************************************************************/
Master::Master()
{
index = 0;
fd = -1;
}
/****************************************************************************/
Master::~Master()
{
close();
}
/****************************************************************************/
void Master::open(unsigned int index)
{
stringstream deviceName;
Master::index = index;
deviceName << "/dev/EtherCAT" << index;
if ((fd = ::open(deviceName.str().c_str(), O_RDONLY)) == -1) {
stringstream err;
err << "Failed to open master device " << deviceName.str() << ": "
<< strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::close()
{
if (fd == -1)
return;
::close(fd);
}
/****************************************************************************/
void Master::outputData(int domainIndex)
{
if (domainIndex == -1) {
unsigned int numDomains = domainCount(), i;
for (i = 0; i < numDomains; i++) {
outputDomainData(i);
}
} else {
outputDomainData(domainIndex);
}
}
/****************************************************************************/
void Master::setDebug(const vector<string> &commandArgs)
{
stringstream str;
int debugLevel;
if (commandArgs.size() != 1) {
stringstream err;
err << "'debug' takes exactly one argument!";
throw MasterException(err.str());
}
str << commandArgs[0];
str >> debugLevel;
if (str.fail()) {
stringstream err;
err << "Invalid debug level '" << commandArgs[0] << "'!";
throw MasterException(err.str());
}
if (ioctl(fd, EC_IOCTL_SET_DEBUG, debugLevel) < 0) {
stringstream err;
err << "Failed to set debug level: " << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::showDomains(int domainIndex)
{
if (domainIndex == -1) {
unsigned int numDomains = domainCount(), i;
for (i = 0; i < numDomains; i++) {
showDomain(i);
}
} else {
showDomain(domainIndex);
}
}
/****************************************************************************/
void Master::listSlaves()
{
unsigned int numSlaves = slaveCount(), i;
ec_ioctl_slave_t slave;
uint16_t lastAlias, aliasIndex;
lastAlias = 0;
aliasIndex = 0;
for (i = 0; i < numSlaves; i++) {
getSlave(&slave, i);
cout << setw(2) << i << " ";
if (slave.alias) {
lastAlias = slave.alias;
aliasIndex = 0;
}
if (lastAlias) {
cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;
}
cout << " " << slaveState(slave.state) << " ";
if (strlen(slave.name)) {
cout << slave.name;
} else {
cout << "0x" << hex << setfill('0') << slave.vendor_id
<< ":0x" << slave.product_code;
}
cout << endl;
}
}
/****************************************************************************/
void Master::showMaster()
{
ec_ioctl_master_t data;
stringstream err;
unsigned int i;
getMaster(&data);
cout
<< "Master" << index << endl
<< " State: ";
switch (data.mode) {
case 0: cout << "Waiting for device..."; break;
case 1: cout << "Idle"; break;
case 2: cout << "Operation"; break;
default:
err << "Invalid master state " << data.mode;
throw MasterException(err.str());
}
cout << endl
<< " Slaves: " << data.slave_count << endl;
for (i = 0; i < 2; i++) {
cout << " Device" << i << ": ";
if (data.devices[i].address[0] == 0x00
&& data.devices[i].address[1] == 0x00
&& data.devices[i].address[2] == 0x00
&& data.devices[i].address[3] == 0x00
&& data.devices[i].address[4] == 0x00
&& data.devices[i].address[5] == 0x00) {
cout << "None.";
} else {
cout << hex << setfill('0')
<< setw(2) << (unsigned int) data.devices[i].address[0] << ":"
<< setw(2) << (unsigned int) data.devices[i].address[1] << ":"
<< setw(2) << (unsigned int) data.devices[i].address[2] << ":"
<< setw(2) << (unsigned int) data.devices[i].address[3] << ":"
<< setw(2) << (unsigned int) data.devices[i].address[4] << ":"
<< setw(2) << (unsigned int) data.devices[i].address[5] << " ("
<< (data.devices[i].attached ? "attached" : "waiting...")
<< ")" << endl << dec
<< " Tx count: " << data.devices[i].tx_count << endl
<< " Rx count: " << data.devices[i].rx_count;
}
cout << endl;
}
}
/****************************************************************************/
void Master::listPdos(int slavePosition)
{
if (slavePosition == -1) {
unsigned int numSlaves = slaveCount(), i;
for (i = 0; i < numSlaves; i++) {
listSlavePdos(i, true);
}
} else {
listSlavePdos(slavePosition, false);
}
}
/****************************************************************************/
void Master::requestStates(
int slavePosition,
const vector<string> &commandArgs
)
{
string stateStr;
uint8_t state;
if (commandArgs.size() != 1) {
stringstream err;
err << "'state' takes exactly one argument!";
throw MasterException(err.str());
}
stateStr = commandArgs[0];
transform(stateStr.begin(), stateStr.end(),
stateStr.begin(), (int (*) (int)) std::toupper);
if (stateStr == "INIT") {
state = 0x01;
} else if (stateStr == "PREOP") {
state = 0x02;
} else if (stateStr == "SAFEOP") {
state = 0x04;
} else if (stateStr == "OP") {
state = 0x08;
} else {
stringstream err;
err << "Invalid state '" << commandArgs[0] << "'!";
throw MasterException(err.str());
}
if (slavePosition == -1) {
unsigned int i, numSlaves = slaveCount();
for (i = 0; i < numSlaves; i++)
requestState(i, state);
} else {
requestState(slavePosition, state);
}
}
/****************************************************************************/
void Master::generateXml(int slavePosition)
{
if (slavePosition == -1) {
unsigned int numSlaves = slaveCount(), i;
for (i = 0; i < numSlaves; i++) {
generateSlaveXml(i);
}
} else {
generateSlaveXml(slavePosition);
}
}
/****************************************************************************/
void Master::outputDomainData(unsigned int domainIndex)
{
ec_ioctl_domain_t domain;
ec_ioctl_data_t data;
unsigned char *processData;
unsigned int i;
getDomain(&domain, domainIndex);
if (!domain.data_size)
return;
processData = new unsigned char[domain.data_size];
try {
getData(&data, domainIndex, domain.data_size, processData);
} catch (MasterException &e) {
delete [] processData;
throw e;
}
for (i = 0; i < data.data_size; i++)
cout << processData[i];
cout.flush();
delete [] processData;
}
/****************************************************************************/
void Master::showDomain(unsigned int domainIndex)
{
ec_ioctl_domain_t domain;
unsigned char *processData;
ec_ioctl_data_t data;
unsigned int i, j;
ec_ioctl_domain_fmmu_t fmmu;
unsigned int dataOffset;
getDomain(&domain, domainIndex);
cout << "Domain" << domainIndex << ":"
<< " LogBaseAddr 0x"
<< hex << setfill('0') << setw(8) << domain.logical_base_address
<< ", Size " << dec << setfill(' ') << setw(3) << domain.data_size
<< ", WorkingCounter "
<< dec << domain.working_counter << "/"
<< domain.expected_working_counter << endl;
if (!domain.data_size)
return;
processData = new unsigned char[domain.data_size];
try {
getData(&data, domainIndex, domain.data_size, processData);
} catch (MasterException &e) {
delete [] processData;
throw e;
}
for (i = 0; i < domain.fmmu_count; i++) {
getFmmu(&fmmu, domainIndex, i);
cout << " SlaveConfig "
<< fmmu.slave_config_alias << ":" << fmmu.slave_config_position
<< ", Dir "
<< setfill(' ') << setw(3) << (fmmu.fmmu_dir ? "In" : "Out")
<< ", LogAddr 0x"
<< hex << setfill('0') << setw(8) << fmmu.logical_address
<< ", Size " << dec << fmmu.data_size << endl;
dataOffset = fmmu.logical_address - domain.logical_base_address;
if (dataOffset + fmmu.data_size > domain.data_size) {
stringstream err;
err << "Fmmu information corrupted!";
delete [] processData;
throw MasterException(err.str());
}
cout << " " << hex << setfill('0');
for (j = 0; j < fmmu.data_size; j++) {
cout << setw(2)
<< (unsigned int) *(processData + dataOffset + j) << " ";
}
cout << endl;
}
delete [] processData;
}
/****************************************************************************/
void Master::listSlavePdos(uint16_t slavePosition, bool printSlave)
{
ec_ioctl_slave_t slave;
ec_ioctl_sync_t sync;
ec_ioctl_pdo_t pdo;
ec_ioctl_pdo_entry_t entry;
unsigned int i, j, k;
getSlave(&slave, slavePosition);
if (printSlave)
cout << "=== Slave " << slavePosition << " ===" << endl;
for (i = 0; i < slave.sync_count; i++) {
getSync(&sync, slavePosition, i);
cout << "SM" << i << ":"
<< " PhysAddr 0x"
<< hex << setfill('0') << setw(4) << sync.physical_start_address
<< ", DefaultSize "
<< dec << setfill(' ') << setw(4) << sync.default_size
<< ", ControlRegister 0x"
<< hex << setfill('0') << setw(2)
<< (unsigned int) sync.control_register
<< ", Enable " << dec << (unsigned int) sync.enable
<< endl;
for (j = 0; j < sync.pdo_count; j++) {
getPdo(&pdo, slavePosition, i, j);
cout << " " << (pdo.dir ? "T" : "R") << "xPdo 0x"
<< hex << setfill('0') << setw(4) << pdo.index
<< " \"" << pdo.name << "\"" << endl;
for (k = 0; k < pdo.entry_count; k++) {
getPdoEntry(&entry, slavePosition, i, j, k);
cout << " Pdo entry 0x"
<< hex << setfill('0') << setw(4) << entry.index
<< ":" << hex << setfill('0') << setw(2)
<< (unsigned int) entry.subindex
<< ", " << dec << (unsigned int) entry.bit_length
<< " bit, \"" << entry.name << "\"" << endl;
}
}
}
}
/****************************************************************************/
void Master::generateSlaveXml(uint16_t slavePosition)
{
ec_ioctl_slave_t slave;
ec_ioctl_sync_t sync;
ec_ioctl_pdo_t pdo;
ec_ioctl_pdo_entry_t entry;
unsigned int i, j, k;
getSlave(&slave, slavePosition);
cout
<< "<?xml version=\"1.0\" ?>" << endl
<< " <EtherCATInfo>" << endl
<< " <!-- Slave " << slave.position << " -->" << endl
<< " <Vendor>" << endl
<< " <Id>" << slave.vendor_id << "</Id>" << endl
<< " </Vendor>" << endl
<< " <Descriptions>" << endl
<< " <Devices>" << endl
<< " <Device>" << endl
<< " <Type ProductCode=\"#x"
<< hex << setfill('0') << setw(8) << slave.product_code
<< "\" RevisionNo=\"#x"
<< hex << setfill('0') << setw(8) << slave.revision_number
<< "\"/>" << endl;
for (i = 0; i < slave.sync_count; i++) {
getSync(&sync, slavePosition, i);
for (j = 0; j < sync.pdo_count; j++) {
getPdo(&pdo, slavePosition, i, j);
cout
<< " <" << (pdo.dir ? "T" : "R") << "xPdo>" << endl
<< " <Index>#x"
<< hex << setfill('0') << setw(4) << pdo.index
<< "</Index>" << endl
<< " <Name>" << pdo.name << "</Name>" << endl;
for (k = 0; k < pdo.entry_count; k++) {
getPdoEntry(&entry, slavePosition, i, j, k);
cout
<< " <Entry>" << endl
<< " <Index>#x"
<< hex << setfill('0') << setw(4) << entry.index
<< "</Index>" << endl;
if (entry.index)
cout
<< " <SubIndex>"
<< dec << (unsigned int) entry.subindex
<< "</SubIndex>" << endl;
cout
<< " <BitLen>"
<< (unsigned int) entry.bit_length
<< "</BitLen>" << endl;
if (entry.index) {
cout
<< " <Name>" << entry.name
<< "</Name>" << endl
<< " <DataType>";
if (entry.bit_length == 1) {
cout << "BOOL";
} else if (!(entry.bit_length % 8)) {
if (entry.bit_length <= 64)
cout << "UINT" << (unsigned int) entry.bit_length;
else
cout << "STRING("
<< (unsigned int) (entry.bit_length / 8)
<< ")";
} else {
cerr << "Invalid bit length "
<< (unsigned int) entry.bit_length << endl;
}
cout << "</DataType>" << endl;
}
cout << " </Entry>" << endl;
}
cout
<< " </" << (pdo.dir ? "T" : "R") << "xPdo>" << endl;
}
}
cout
<< " </Device>" << endl
<< " </Devices>" << endl
<< " </Descriptions>" << endl
<< "</EtherCATInfo>" << endl;
}
/****************************************************************************/
unsigned int Master::domainCount()
{
int ret;
if ((ret = ioctl(fd, EC_IOCTL_DOMAIN_COUNT, 0)) < 0) {
stringstream err;
err << "Failed to get number of domains: " << strerror(errno);
throw MasterException(err.str());
}
return ret;
}
/****************************************************************************/
unsigned int Master::slaveCount()
{
ec_ioctl_master_t data;
getMaster(&data);
return data.slave_count;
}
/****************************************************************************/
void Master::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 MasterException(err.str());
}
}
/****************************************************************************/
void Master::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 MasterException(err.str());
}
}
/****************************************************************************/
void Master::getData(ec_ioctl_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_DATA, data) < 0) {
stringstream err;
err << "Failed to get domain data: " << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::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 MasterException(err.str());
}
}
/****************************************************************************/
void Master::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: ";
if (errno == EINVAL)
err << "Either domain " << domainIndex << " does not exist, "
<< "or it contains less than " << (unsigned int) fmmuIndex + 1
<< " FMMus!";
else
err << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::getSync(
ec_ioctl_sync_t *sync,
uint16_t slaveIndex,
uint8_t syncIndex
)
{
sync->slave_position = slaveIndex;
sync->sync_index = syncIndex;
if (ioctl(fd, EC_IOCTL_SYNC, sync)) {
stringstream err;
err << "Failed to get sync manager: ";
if (errno == EINVAL)
err << "Either slave " << slaveIndex << " does not exist, "
<< "or it contains less than " << (unsigned int) syncIndex + 1
<< " sync managers!";
else
err << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::getPdo(
ec_ioctl_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_PDO, pdo)) {
stringstream err;
err << "Failed to get Pdo: ";
if (errno == EINVAL)
err << "Either slave " << slaveIndex << " does not exist, "
<< "or it contains less than " << (unsigned int) syncIndex + 1
<< " sync managers, or sync manager "
<< (unsigned int) syncIndex << " contains less than "
<< pdoPos + 1 << " Pdos!" << endl;
else
err << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::getPdoEntry(
ec_ioctl_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_PDO_ENTRY, entry)) {
stringstream err;
err << "Failed to get Pdo entry: ";
if (errno == EINVAL)
err << "Either slave " << slaveIndex << " does not exist, "
<< "or it contains less than " << (unsigned int) syncIndex + 1
<< " sync managers, or sync manager "
<< (unsigned int) syncIndex << " contains less than "
<< pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
<< " contains less than " << (unsigned int) entryPos + 1
<< " entries!" << endl;
else
err << strerror(errno);
throw MasterException(err.str());
}
}
/****************************************************************************/
void Master::requestState(
uint16_t slavePosition,
uint8_t state
)
{
ec_ioctl_slave_state_t data;
data.slave_position = slavePosition;
data.requested_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 MasterException(err.str());
}
}
/****************************************************************************/
string Master::slaveState(uint8_t state)
{
switch (state) {
case 1: return "INIT";
case 2: return "PREOP";
case 4: return "SAFEOP";
case 8: return "OP";
default: return "???";
}
}
/****************************************************************************/