fp@922: /*****************************************************************************
fp@922:  *
fp@922:  * $Id$
fp@922:  *
fp@922:  ****************************************************************************/
fp@922: 
fp@922: #include <sys/types.h>
fp@922: #include <sys/stat.h>
fp@922: #include <fcntl.h>
fp@922: #include <errno.h>
fp@922: #include <sys/ioctl.h>
fp@922: 
fp@922: #include <iostream>
fp@922: #include <iomanip>
fp@922: #include <sstream>
fp@922: using namespace std;
fp@922: 
fp@922: #include "Master.h"
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: Master::Master()
fp@922: {
fp@922:     index = 0;
fp@922:     fd = -1;
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: Master::~Master()
fp@922: {
fp@922:     close();
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: void Master::open(unsigned int index)
fp@922: {
fp@922:     stringstream deviceName;
fp@922:     
fp@922:     Master::index = index;
fp@922: 
fp@922:     deviceName << "/dev/EtherCAT" << index;
fp@922:     
fp@922:     if ((fd = ::open(deviceName.str().c_str(), O_RDONLY)) == -1) {
fp@922:         stringstream err;
fp@922:         err << "Failed to open master device " << deviceName.str() << ": "
fp@922:             << strerror(errno);
fp@922:         throw MasterException(err.str());
fp@922:     }
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: void Master::close()
fp@922: {
fp@922:     if (fd == -1)
fp@922:         return;
fp@922: 
fp@922:     ::close(fd);
fp@922: }
fp@922: 
fp@922: /****************************************************************************/
fp@922: 
fp@922: void Master::listSlaves()
fp@922: {
fp@922:     unsigned int numSlaves = slaveCount(), i;
fp@935:     ec_ioctl_slave_t slave;
fp@922:     uint16_t lastAlias, aliasIndex;
fp@922:     
fp@922:     lastAlias = 0;
fp@922:     aliasIndex = 0;
fp@922:     for (i = 0; i < numSlaves; i++) {
fp@935:         getSlave(&slave, i);
fp@935:         cout << setw(2) << i << "  ";
fp@935: 
fp@935:         if (slave.alias) {
fp@935:             lastAlias = slave.alias;
fp@922:             aliasIndex = 0;
fp@922:         }
fp@922:         if (lastAlias) {
fp@922:             cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;
fp@922:         }
fp@922: 
fp@935:         cout << "  " << slaveState(slave.state) << "  ";
fp@935: 
fp@935:         if (strlen(slave.name)) {
fp@935:             cout << slave.name;
fp@922:         } else {
fp@935:             cout << "0x" << hex << setfill('0') << slave.vendor_id
fp@935:                 << ":0x" << slave.product_code;
fp@922:         }
fp@922: 
fp@922:         cout << endl;
fp@922:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::listPdos(int slavePosition)
fp@935: {
fp@935:     ec_ioctl_slave_t slave;
fp@935:     ec_ioctl_sync_t sync;
fp@935:     ec_ioctl_pdo_t pdo;
fp@935:     ec_ioctl_pdo_entry_t entry;
fp@935:     unsigned int i, j, k;
fp@935:     
fp@935:     getSlave(&slave, slavePosition);
fp@935: 
fp@935:     for (i = 0; i < slave.sync_count; i++) {
fp@935:         getSync(&sync, slavePosition, i);
fp@935: 
fp@935:         cout << "SM" << i << ":"
fp@935:             << " PhysAddr 0x"
fp@935:             << hex << setfill('0') << setw(4) << sync.physical_start_address
fp@935:             << ", DefaultSize "
fp@935:             << dec << setfill(' ') << setw(4) << sync.default_size
fp@935:             << ", ControlRegister 0x"
fp@935:             << hex << setfill('0') << setw(2)
fp@935:             << (unsigned int) sync.control_register
fp@935:             << ", Enable " << dec << (unsigned int) sync.enable
fp@935:             << endl;
fp@935: 
fp@935:         for (j = 0; j < sync.pdo_count; j++) {
fp@935:             getPdo(&pdo, slavePosition, i, j);
fp@935: 
fp@935:             cout << "  " << (pdo.dir ? "T" : "R") << "xPdo 0x"
fp@935:                 << hex << setfill('0') << setw(4) << pdo.index
fp@935:                 << " \"" << pdo.name << "\"" << endl;
fp@935: 
fp@935:             for (k = 0; k < pdo.entry_count; k++) {
fp@935:                 getPdoEntry(&entry, slavePosition, i, j, k);
fp@935: 
fp@935:                 cout << "    Pdo entry 0x"
fp@935:                     << hex << setfill('0') << setw(4) << entry.index
fp@935:                     << ":" << hex << setfill('0') << setw(2)
fp@935:                     << (unsigned int) entry.subindex
fp@935:                     << ", " << dec << (unsigned int) entry.bit_length
fp@935:                     << " bit, \"" << entry.name << "\"" << endl;
fp@935:             }
fp@935:         }
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: unsigned int Master::slaveCount()
fp@935: {
fp@935:     int ret;
fp@935: 
fp@935:     if ((ret = ioctl(fd, EC_IOCTL_SLAVE_COUNT, 0)) < 0) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get slave: " << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: 
fp@935:     return ret;
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex)
fp@935: {
fp@935:     slave->position = slaveIndex;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_SLAVE, slave)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get slave: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Slave " << slaveIndex << " does not exist!";
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getSync(
fp@935:         ec_ioctl_sync_t *sync,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex
fp@935:         )
fp@935: {
fp@935:     sync->slave_position = slaveIndex;
fp@935:     sync->sync_index = syncIndex;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_SYNC, sync)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get sync manager: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@935:                 << "or contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers!";
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getPdo(
fp@935:         ec_ioctl_pdo_t *pdo,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex,
fp@935:         uint8_t pdoPos
fp@935:         )
fp@935: {
fp@935:     pdo->slave_position = slaveIndex;
fp@935:     pdo->sync_index = syncIndex;
fp@935:     pdo->pdo_pos = pdoPos;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_PDO, pdo)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get Pdo: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@935:                 << "or contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers, or sync manager "
fp@935:                 << (unsigned int) syncIndex << " contains less than "
fp@935:                 << pdoPos + 1 << " Pdos!" << endl;
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: void Master::getPdoEntry(
fp@935:         ec_ioctl_pdo_entry_t *entry,
fp@935:         uint16_t slaveIndex,
fp@935:         uint8_t syncIndex,
fp@935:         uint8_t pdoPos,
fp@935:         uint8_t entryPos
fp@935:         )
fp@935: {
fp@935:     entry->slave_position = slaveIndex;
fp@935:     entry->sync_index = syncIndex;
fp@935:     entry->pdo_pos = pdoPos;
fp@935:     entry->entry_pos = entryPos;
fp@935: 
fp@935:     if (ioctl(fd, EC_IOCTL_PDO_ENTRY, entry)) {
fp@935:         stringstream err;
fp@935:         err << "Failed to get Pdo entry: ";
fp@935:         if (errno == EINVAL)
fp@935:             err << "Either slave " << slaveIndex << " does not exist, "
fp@935:                 << "or contains less than " << (unsigned int) syncIndex + 1
fp@935:                 << " sync managers, or sync manager "
fp@935:                 << (unsigned int) syncIndex << " contains less than "
fp@935:                 << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos
fp@935:                 << " contains less than " << (unsigned int) entryPos + 1
fp@935:                 << " entries!" << endl;
fp@935:         else
fp@935:             err << strerror(errno);
fp@935:         throw MasterException(err.str());
fp@935:     }
fp@935: }
fp@935: 
fp@935: /****************************************************************************/
fp@935: 
fp@935: string Master::slaveState(uint8_t state)
fp@922: {
fp@922:     switch (state) {
fp@922:         case 1: return "INIT";
fp@922:         case 2: return "PREOP";
fp@922:         case 4: return "SAFEOP";
fp@922:         case 8: return "OP";
fp@922:         default: return "???";
fp@922:     }
fp@922: }
fp@922: 
fp@922: /****************************************************************************/