tools/Master.cpp
changeset 935 b954e9d91ea5
parent 922 fede1d8f5b71
child 936 30fddfce8db6
--- a/tools/Master.cpp	Mon Jun 02 10:49:44 2008 +0000
+++ b/tools/Master.cpp	Mon Jun 02 10:51:31 2008 +0000
@@ -16,7 +16,6 @@
 using namespace std;
 
 #include "Master.h"
-#include "../master/ioctl.h"
 
 /****************************************************************************/
 
@@ -63,70 +62,205 @@
 
 /****************************************************************************/
 
-unsigned int Master::slaveCount()
-{
-    unsigned int numSlaves;
-
-    if (ioctl(fd, EC_IOCTL_SLAVE_COUNT, &numSlaves)) {
-        stringstream err;
-        err << "Failed to get number of slaves: " << strerror(errno);
-        throw MasterException(err.str());
-    }
-
-    return numSlaves;
-}
-
-/****************************************************************************/
-
 void Master::listSlaves()
 {
     unsigned int numSlaves = slaveCount(), i;
-    struct ec_ioctl_slave_info *infos, *info;
+    ec_ioctl_slave_t slave;
     uint16_t lastAlias, aliasIndex;
     
-    if (!numSlaves)
-        return;
-    
-    infos = new struct ec_ioctl_slave_info[numSlaves];
-
-    if (ioctl(fd, EC_IOCTL_SLAVE_INFO, infos)) {
-        stringstream err;
-        err << "Failed to get slave information: " << strerror(errno);
-        throw MasterException(err.str());
-    }
-
     lastAlias = 0;
     aliasIndex = 0;
     for (i = 0; i < numSlaves; i++) {
-        info = &infos[i];
-        cout << setw(2) << info->ring_position << "  ";
-
-        if (info->alias) {
-            lastAlias = info->alias;
+        getSlave(&slave, i);
+        cout << setw(2) << i << "  ";
+
+        if (slave.alias) {
+            lastAlias = slave.alias;
             aliasIndex = 0;
         }
         if (lastAlias) {
             cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;
         }
 
-        cout << "  " << slaveState(info->state) << "  ";
-
-        if (strlen(info->description)) {
-            cout << info->description;
+        cout << "  " << slaveState(slave.state) << "  ";
+
+        if (strlen(slave.name)) {
+            cout << slave.name;
         } else {
-            cout << "0x" << hex << setfill('0') << info->vendor_id
-                << ":0x" << info->product_code;
+            cout << "0x" << hex << setfill('0') << slave.vendor_id
+                << ":0x" << slave.product_code;
         }
 
         cout << endl;
     }
-
-    delete [] infos;
-}
-
-/****************************************************************************/
-
-string Master::slaveState(uint8_t state) const
+}
+
+/****************************************************************************/
+
+void Master::listPdos(int 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);
+
+    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;
+            }
+        }
+    }
+}
+
+/****************************************************************************/
+
+unsigned int Master::slaveCount()
+{
+    int ret;
+
+    if ((ret = ioctl(fd, EC_IOCTL_SLAVE_COUNT, 0)) < 0) {
+        stringstream err;
+        err << "Failed to get slave: " << strerror(errno);
+        throw MasterException(err.str());
+    }
+
+    return ret;
+}
+
+/****************************************************************************/
+
+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::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 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 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 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());
+    }
+}
+
+/****************************************************************************/
+
+string Master::slaveState(uint8_t state)
 {
     switch (state) {
         case 1: return "INIT";