Laden der SDO-Subindizes.
authorFlorian Pose <fp@igh-essen.com>
Wed, 29 Mar 2006 15:10:00 +0000
changeset 139 998215ab9b92
parent 138 7e743a61a991
child 140 b09658e50d6f
Laden der SDO-Subindizes.
master/canopen.c
master/slave.c
master/slave.h
rt/msr_module.c
--- a/master/canopen.c	Wed Mar 29 10:30:13 2006 +0000
+++ b/master/canopen.c	Wed Mar 29 15:10:00 2006 +0000
@@ -18,6 +18,7 @@
 
 void ec_canopen_abort_msg(uint32_t);
 int ec_slave_fetch_sdo_descriptions(ec_slave_t *);
+int ec_slave_fetch_sdo_entries(ec_slave_t *, ec_sdo_t *, uint8_t);
 
 /*****************************************************************************/
 
@@ -37,6 +38,54 @@
 /*****************************************************************************/
 
 /**
+   Liest 32 Bit eines CANopen-SDOs im Expedited-Modus aus einem Slave.
+   \return 0 wenn alles ok, < 0 bei Fehler
+ */
+
+int ec_slave_sdo_read_exp(ec_slave_t *slave, /**< EtherCAT-Slave */
+                          uint16_t sdo_index, /**< SDO-Index */
+                          uint8_t sdo_subindex, /**< SDO-Subindex */
+                          uint8_t *target /**< Speicher für 4 Bytes */
+                          )
+{
+    uint8_t data[0x20];
+    size_t rec_size;
+
+    EC_WRITE_U16(data, 0x2 << 12); // SDO request
+    EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer
+                            | 0x2 << 5));  // initiate upload request
+    EC_WRITE_U16(data + 3, sdo_index);
+    EC_WRITE_U8 (data + 5, sdo_subindex);
+
+    if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1;
+
+    rec_size = 0x20;
+    if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;
+
+    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+        EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request
+        EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n",
+               sdo_index, sdo_subindex, slave->ring_position);
+        ec_canopen_abort_msg(EC_READ_U32(data + 6));
+        return -1;
+    }
+
+    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+        EC_READ_U8 (data + 2) >> 5 != 0x2 || // Upload response
+        EC_READ_U16(data + 3) != sdo_index || // Index
+        EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex
+        EC_ERR("Invalid SDO upload response at slave %i!\n",
+               slave->ring_position);
+        return -1;
+    }
+
+    memcpy(target, data + 6, 4);
+    return 0;
+}
+
+/*****************************************************************************/
+
+/**
    Beschreibt ein CANopen-SDO eines Slaves im Expedited-Modus.
    \return 0 wenn alles ok, < 0 bei Fehler
  */
@@ -56,8 +105,11 @@
         return -1;
     }
 
-    EC_WRITE_U16(data,     0x2000); // SDO request
-    EC_WRITE_U8 (data + 2, 0x23 | (4 - size) << 2); // expedited, size specif.
+    EC_WRITE_U16(data, 0x2 << 12); // SDO request
+    EC_WRITE_U8 (data + 2, (0x1 // size specified
+                            | 0x1 << 1 // expedited transfer
+                            | (4 - size) << 2 // data set size
+                            | 0x1 << 5)); // initiate download request
     EC_WRITE_U16(data + 3, sdo_index);
     EC_WRITE_U8 (data + 5, sdo_subindex);
 
@@ -70,8 +122,8 @@
     rec_size = 0x0A;
     if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;
 
-    if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request
-        EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request
+    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+        EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request
         EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!"
                " slave %i.\n", sdo_index, sdo_subindex, size,
                slave->ring_position);
@@ -79,8 +131,8 @@
         return -1;
     }
 
-    if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response
-        EC_READ_U8 (data + 2) >> 5 != 0x03 || // Download response
+    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+        EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
         EC_READ_U16(data + 3) != sdo_index || // Index
         EC_READ_U8 (data + 5) != sdo_subindex) // Subindex
     {
@@ -95,54 +147,7 @@
 /*****************************************************************************/
 
 /**
-   Liest 32-Bit eines CANopen-SDOs im Expedited-Modus aus einem Slave.
-   \return 0 wenn alles ok, < 0 bei Fehler
- */
-
-int ec_slave_sdo_read_exp(ec_slave_t *slave, /**< EtherCAT-Slave */
-                          uint16_t sdo_index, /**< SDO-Index */
-                          uint8_t sdo_subindex, /**< SDO-Subindex */
-                          uint8_t *target /**< Speicher für 4 Bytes */
-                          )
-{
-    uint8_t data[0x20];
-    size_t rec_size;
-
-    EC_WRITE_U16(data,     0x2000); // SDO request
-    EC_WRITE_U8 (data + 2, 0x42); // Upload request, expedited
-    EC_WRITE_U16(data + 3, sdo_index);
-    EC_WRITE_U8 (data + 5, sdo_subindex);
-
-    if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1;
-
-    rec_size = 0x20;
-    if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;
-
-    if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request
-        EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request
-        EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n",
-               sdo_index, sdo_subindex, slave->ring_position);
-        ec_canopen_abort_msg(EC_READ_U32(data + 6));
-        return -1;
-    }
-
-    if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response
-        EC_READ_U8 (data + 2) >> 5 != 0x02 || // Upload response
-        EC_READ_U16(data + 3) != sdo_index || // Index
-        EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex
-        EC_ERR("Invalid SDO upload response at slave %i!\n",
-               slave->ring_position);
-        return -1;
-    }
-
-    memcpy(target, data + 6, 4);
-    return 0;
-}
-
-/*****************************************************************************/
-
-/**
-   Liest ein CANopen-SDO aus einem Slave.
+   Liest ein CANopen-SDO im "Normal-Modus" aus einem Slave.
    \return 0 wenn alles ok, < 0 bei Fehler
  */
 
@@ -157,8 +162,8 @@
     size_t rec_size, data_size;
     uint32_t complete_size;
 
-    EC_WRITE_U16(data,     0x2000); // SDO request
-    EC_WRITE_U8 (data + 2, 0x2 << 5); // Initiate upload request
+    EC_WRITE_U16(data, 0x2 << 12); // SDO request
+    EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request
     EC_WRITE_U16(data + 3, sdo_index);
     EC_WRITE_U8 (data + 5, sdo_subindex);
 
@@ -167,16 +172,16 @@
     rec_size = 0x20;
     if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;
 
-    if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request
-        EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request
+    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
+        EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request
         EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n",
                sdo_index, sdo_subindex, slave->ring_position);
         ec_canopen_abort_msg(EC_READ_U32(data + 6));
         return -1;
     }
 
-    if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response
-        EC_READ_U8 (data + 2) >> 5 != 0x02 || // Initiate upload response
+    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
+        EC_READ_U8 (data + 2) >> 5 != 0x2 || // Initiate upload response
         EC_READ_U16(data + 3) != sdo_index || // Index
         EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex
         EC_ERR("Invalid SDO upload response at slave %i!\n",
@@ -211,7 +216,7 @@
 /*****************************************************************************/
 
 /**
-   Holt das Object-Dictionary aus dem Slave.
+   Lädt das gesamte SDO-Dictionary aus einem Slave.
    \return 0, wenn alles ok, sonst < 0
 */
 
@@ -223,9 +228,9 @@
     ec_sdo_t *sdo;
     uint16_t sdo_index;
 
-    EC_WRITE_U16(data,     0x8000); // SDO information
-    EC_WRITE_U8 (data + 2,   0x01); // Get OD List Request
-    EC_WRITE_U8 (data + 3,   0x00);
+    EC_WRITE_U16(data, 0x8 << 12); // SDO information
+    EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
+    EC_WRITE_U8 (data + 3, 0x00);
     EC_WRITE_U16(data + 4, 0x0000);
     EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs!
 
@@ -235,7 +240,7 @@
         rec_size = 0xF0;
         if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1;
 
-        if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information
+        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
             (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response
             EC_ERR("SDO information error response at slave %i!\n",
                    slave->ring_position);
@@ -243,7 +248,7 @@
             return -1;
         }
 
-        if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information
+        if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
             (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
             EC_ERR("Invalid SDO list response at slave %i!\n",
                    slave->ring_position);
@@ -270,7 +275,7 @@
         }
     } while (EC_READ_U8(data + 2) & 0x80);
 
-    // Jetzt alle Beschreibungen holen
+    // Alle Beschreibungen laden
     if (ec_slave_fetch_sdo_descriptions(slave)) return -1;
 
     return 0;
@@ -280,7 +285,6 @@
 
 /**
    Holt die Beschreibungen zu allen bereits bekannten SDOs.
-
    \return 0, wenn alles ok, sonst < 0
 */
 
@@ -291,9 +295,9 @@
     ec_sdo_t *sdo;
 
     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
-        EC_WRITE_U16(data,     0x8000); // SDO information
-        EC_WRITE_U8 (data + 2,   0x03); // Get object description request
-        EC_WRITE_U8 (data + 3,   0x00);
+        EC_WRITE_U16(data, 0x8 << 12); // SDO information
+        EC_WRITE_U8 (data + 2, 0x03); // Get object description request
+        EC_WRITE_U8 (data + 3, 0x00);
         EC_WRITE_U16(data + 4, 0x0000);
         EC_WRITE_U16(data + 6, sdo->index); // SDO index
 
@@ -303,7 +307,7 @@
         if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size))
             return -1;
 
-        if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information
+        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
             (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response
             EC_ERR("SDO information error response at slave %i while"
                    " fetching SDO 0x%04X!\n", slave->ring_position,
@@ -312,8 +316,8 @@
             return -1;
         }
 
-        if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information
-            (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Obj. desc. resp.
+        if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
+            (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response
             EC_READ_U16(data + 6) != sdo->index) { // SDO index
             EC_ERR("Invalid object description response at slave %i while"
                    " fetching SDO 0x%04X!\n", slave->ring_position,
@@ -327,25 +331,113 @@
         }
 
         sdo->type = EC_READ_U16(data + 8);
-        sdo->max_subindex = EC_READ_U8(data + 10);
         sdo->features = EC_READ_U8(data + 11);
+        INIT_LIST_HEAD(&sdo->entries);
 
         name_size = rec_size - 12;
-        if (!name_size) {
-            EC_WARN("Object 0x%04X name size is 0...", sdo->index);
+        if (name_size) {
+            if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) {
+                EC_ERR("Failed to allocate SDO name!\n");
+                return -1;
+            }
+
+            memcpy(sdo->name, data + 12, name_size);
+            sdo->name[name_size] = 0;
+        }
+
+        if (EC_READ_U8(data + 2) & 0x80) {
+            EC_ERR("Fragment follows (not implemented)!\n");
+            return -1;
+        }
+
+        // Alle Entries (Subindizes) laden
+        if (ec_slave_fetch_sdo_entries(slave, sdo, EC_READ_U8(data + 10)))
+            return -1;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************/
+
+/**
+   Lädt alle Entries (Subindizes) zu einem SDO.
+   \return 0, wenn alles ok, sonst < 0
+*/
+
+int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT-Slave */
+                               ec_sdo_t *sdo, /**< SDO */
+                               uint8_t subindices /**< Anzahl Subindizes */
+                               )
+{
+    uint8_t data[0xF0];
+    size_t rec_size, data_size;
+    uint8_t i;
+    ec_sdo_entry_t *entry;
+
+    for (i = 1; i <= subindices; i++) {
+        EC_WRITE_U16(data, 0x8 << 12); // SDO information
+        EC_WRITE_U8 (data + 2, 0x05); // Get entry description request
+        EC_WRITE_U8 (data + 3, 0x00);
+        EC_WRITE_U16(data + 4, 0x0000);
+        EC_WRITE_U16(data + 6, sdo->index); // SDO index
+        EC_WRITE_U8 (data + 8, i); // SDO subindex
+        EC_WRITE_U8 (data + 9, 0x00); // value info (no values)
+
+        if (ec_slave_mailbox_send(slave, 0x03, data, 10)) return -1;
+
+        rec_size = 0xF0;
+        if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size))
+            return -1;
+
+        if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
+            (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response
+            EC_ERR("SDO information error response at slave %i while"
+                   " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,
+                   sdo->index, i);
+            ec_canopen_abort_msg(EC_READ_U32(data + 6));
+            return -1;
+        }
+
+        if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
+            (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // entry descr. response
+            EC_READ_U16(data + 6) != sdo->index || // SDO index
+            EC_READ_U8(data + 8) != i) { // SDO subindex
+            EC_ERR("Invalid entry description response at slave %i while"
+                   " fetching SDO 0x%04X:%i!\n", slave->ring_position,
+                   sdo->index, i);
+            return -1;
+        }
+
+        if (rec_size < 16) {
+            EC_ERR("Invalid data size!\n");
+            return -1;
+        }
+
+        if (!EC_READ_U16(data + 12)) // bit length = 0
             continue;
-        }
-
-        if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) {
-            EC_ERR("Failed to allocate SDO name!\n");
-            return -1;
-        }
-
-        memcpy(sdo->name, data + 12, name_size);
-        sdo->name[name_size] = 0;
-
-        if (EC_READ_U8(data + 2) & 0x80)
-            EC_WARN("Fragment follows in object description!\n");
+
+        data_size = rec_size - 16;
+
+        if (!(entry = (ec_sdo_entry_t *)
+              kmalloc(sizeof(ec_sdo_entry_t) + data_size + 1, GFP_KERNEL))) {
+            EC_ERR("Failed to allocate entry memory!\n");
+            return -1;
+        }
+
+        entry->subindex = i;
+        entry->data_type = EC_READ_U16(data + 10);
+        entry->bit_length = EC_READ_U16(data + 12);
+        if (!data_size) {
+            entry->name = NULL;
+        }
+        else {
+            entry->name = (uint8_t *) entry + sizeof(ec_sdo_entry_t);
+            memcpy(entry->name, data + 16, data_size);
+            entry->name[data_size] = 0;
+        }
+
+        list_add_tail(&entry->list, &sdo->entries);
     }
 
     return 0;
--- a/master/slave.c	Wed Mar 29 10:30:13 2006 +0000
+++ b/master/slave.c	Wed Mar 29 15:10:00 2006 +0000
@@ -78,6 +78,7 @@
     ec_eeprom_pdo_t *pdo, *next_pdo;
     ec_eeprom_pdo_entry_t *entry, *next_ent;
     ec_sdo_t *sdo, *next_sdo;
+    ec_sdo_entry_t *en, *next_en;
 
     // Alle Strings freigeben
     list_for_each_entry_safe(string, next_str, &slave->eeprom_strings, list) {
@@ -114,6 +115,11 @@
     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
         list_del(&sdo->list);
         if (sdo->name) kfree(sdo->name);
+        // Alle Entries freigeben
+        list_for_each_entry_safe(en, next_en, &sdo->entries, list) {
+            list_del(&en->list);
+            kfree(en);
+        }
         kfree(sdo);
     }
 }
@@ -853,8 +859,9 @@
 {
     ec_eeprom_sync_t *sync;
     ec_eeprom_pdo_t *pdo;
-    ec_eeprom_pdo_entry_t *entry;
+    ec_eeprom_pdo_entry_t *pdo_entry;
     ec_sdo_t *sdo;
+    ec_sdo_entry_t *sdo_entry;
     int first;
 
     EC_INFO("x-- EtherCAT slave information ---------------\n");
@@ -950,20 +957,27 @@
                 pdo->name ? pdo->name : "???",
                 pdo->index, pdo->sync_manager);
 
-        list_for_each_entry(entry, &pdo->entries, list) {
+        list_for_each_entry(pdo_entry, &pdo->entries, list) {
             EC_INFO("|     \"%s\" 0x%04X:%X, %i Bit\n",
-                    entry->name ? entry->name : "???",
-                    entry->index, entry->subindex, entry->bit_length);
+                    pdo_entry->name ? pdo_entry->name : "???",
+                    pdo_entry->index, pdo_entry->subindex,
+                    pdo_entry->bit_length);
         }
     }
 
     if (!list_empty(&slave->sdo_dictionary)) {
         EC_INFO("|   SDO-Dictionary:\n");
         list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
-            EC_INFO("|     0x%04X: \"%s\"\n", sdo->index,
+            EC_INFO("|     0x%04X \"%s\"\n", sdo->index,
                     sdo->name ? sdo->name : "");
-            EC_INFO("|       Type 0x%04X, subindices: %i, features: 0x%02X\n",
-                    sdo->type, sdo->max_subindex, sdo->features);
+            EC_INFO("|       Type 0x%04X, features: 0x%02X\n",
+                    sdo->type, sdo->features);
+            list_for_each_entry(sdo_entry, &sdo->entries, list) {
+                EC_INFO("|       0x%04X:%i \"%s\", type 0x%04X, %i bits\n",
+                        sdo->index, sdo_entry->subindex,
+                        sdo_entry->name ? sdo_entry->name : "",
+                        sdo_entry->data_type, sdo_entry->bit_length);
+            }
         }
     }
 
--- a/master/slave.h	Wed Mar 29 10:30:13 2006 +0000
+++ b/master/slave.h	Wed Mar 29 15:10:00 2006 +0000
@@ -154,15 +154,31 @@
     struct list_head list;
     uint16_t index;
     uint16_t type;
-    uint8_t max_subindex;
     uint8_t features;
     char *name;
+    struct list_head entries;
 }
 ec_sdo_t;
 
 /*****************************************************************************/
 
 /**
+   CANopen-SDO-Entry.
+*/
+
+typedef struct
+{
+    struct list_head list;
+    uint8_t subindex;
+    uint16_t data_type;
+    uint16_t bit_length;
+    char *name;
+}
+ec_sdo_entry_t;
+
+/*****************************************************************************/
+
+/**
    EtherCAT-Slave
 */
 
--- a/rt/msr_module.c	Wed Mar 29 10:30:13 2006 +0000
+++ b/rt/msr_module.c	Wed Mar 29 15:10:00 2006 +0000
@@ -184,7 +184,7 @@
 int __init init_rt_module(void)
 {
     struct ipipe_domain_attr attr; //ipipe
-    uint8_t string[10];
+    uint8_t string[20];
     size_t size;
     ec_slave_t *slave;
 
@@ -242,7 +242,6 @@
         printk(KERN_ERR "Could not read SSI version!\n");
         goto out_deactivate;
     }
-    string[size] = 0;
     printk(KERN_INFO "Software-version 1: %s\n", string);
 
     if (!(slave = ecrt_master_get_slave(master, "5"))) {
@@ -255,9 +254,15 @@
         printk(KERN_ERR "Could not read SSI version!\n");
         goto out_deactivate;
     }
-    string[size] = 0;
     printk(KERN_INFO "Software-version 5: %s\n", string);
 
+    size = 20;
+    if (ecrt_slave_sdo_read(slave, 0x1008, 0, string, &size)) {
+        printk(KERN_ERR "Could not read string!\n");
+        goto out_deactivate;
+    }
+    printk(KERN_INFO "String: %s\n", string);
+
     if (ecrt_slave_sdo_write_exp8(slave, 0x4061, 1,  0) ||
         ecrt_slave_sdo_write_exp8(slave, 0x4061, 2,  1) ||
         ecrt_slave_sdo_write_exp8(slave, 0x4061, 3,  1) ||