Bugfix: SDO download. SDO abort codes ausgeben.
authorFlorian Pose <fp@igh-essen.com>
Mon, 20 Mar 2006 11:02:39 +0000
changeset 111 9963537ec776
parent 110 2346e2963087
child 112 eec7639c7be9
Bugfix: SDO download. SDO abort codes ausgeben.
master/canopen.c
master/master.c
master/module.c
mini/mini.c
--- a/master/canopen.c	Mon Mar 20 08:36:08 2006 +0000
+++ b/master/canopen.c	Mon Mar 20 11:02:39 2006 +0000
@@ -17,6 +17,21 @@
 /*****************************************************************************/
 
 /**
+   SDO Abort Code Messages
+*/
+
+typedef struct
+{
+    uint32_t code;
+    const char *message;
+}
+ec_sdo_abort_message_t;
+
+const ec_sdo_abort_message_t sdo_abort_messages[];
+
+/*****************************************************************************/
+
+/**
    Schreibt ein CANopen-SDO (service data object).
  */
 
@@ -32,13 +47,15 @@
     unsigned int i;
     ec_master_t *master;
     cycles_t start, end, timeout;
+    uint32_t abort_code;
+    const ec_sdo_abort_message_t *abort_msg;
 
     memset(data, 0x00, 0xF6);
 
     master = slave->master;
 
     if (size == 0 || size > 4) {
-        EC_ERR("Illegal SDO data size: %i!\n", size);
+        EC_ERR("Invalid SDO data size: %i!\n", size);
         return -1;
     }
 
@@ -46,8 +63,8 @@
     EC_WRITE_U16(data + 2,  slave->station_address); // Station address
     EC_WRITE_U8 (data + 4,  0x00); // Channel & priority
     EC_WRITE_U8 (data + 5,  0x03); // CANopen over EtherCAT
-    EC_WRITE_U16(data + 6,  0x2000); // Number (0), Service (SDO request)
-    EC_WRITE_U8 (data + 8,  0x13 | ((4 - size) << 2)); // Spec., exp., init.
+    EC_WRITE_U16(data + 6,  0x02 << 12); // Number (0), Service (SDO request)
+    EC_WRITE_U8 (data + 8,  0x23 | ((4 - size) << 2)); // Spec., exp., init.
     EC_WRITE_U16(data + 9,  sdo_index);
     EC_WRITE_U8 (data + 11, sdo_subindex);
 
@@ -64,10 +81,12 @@
 
     // Read "written bit" of Sync-Manager
     start = get_cycles();
-    timeout = cpu_khz; // 1ms
-
-    do
+    timeout = (cycles_t) 10 * cpu_khz; // 10ms
+
+    while (1)
     {
+        udelay(100);
+
         ec_command_init_nprd(&command, slave->station_address, 0x808, 8);
         if (unlikely(ec_master_simple_io(master, &command))) {
             EC_ERR("Mailbox checking failed on slave %i!\n",
@@ -78,13 +97,17 @@
         end = get_cycles();
 
         if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high
-    }
-    while ((end - start) < timeout);
-
-    if ((end - start) >= timeout) {
-        EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position);
-        return -1;
-    }
+
+        if ((end - start) >= timeout) {
+            EC_ERR("Mailbox check - Slave %i timed out.\n",
+                   slave->ring_position);
+            return -1;
+        }
+    }
+
+    if (unlikely(slave->master->debug_level) > 1)
+        EC_DBG("SDO download took %ius.\n", ((u32) (end - start) * 1000
+                                             / cpu_khz));
 
     ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6);
     if (unlikely(ec_master_simple_io(master, &command))) {
@@ -93,13 +116,35 @@
         return -1;
     }
 
-    if (EC_READ_U8 (command.data + 5) != 0x03 || // COE
-        EC_READ_U16(command.data + 6) != 0x3000 || // SDO response
+    if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE
+        EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n",
+               slave->ring_position);
+        return -1;
+    }
+
+    if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request
+        EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req.
+        EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave"
+               " %i.\n", sdo_index, sdo_subindex, value, size,
+               slave->ring_position);
+        abort_code = EC_READ_U32(command.data + 12);
+        for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
+            if (abort_msg->code == abort_code) {
+                EC_ERR("SDO abort message 0x%08X: \"%s\".\n",
+                       abort_msg->code, abort_msg->message);
+                return -1;
+            }
+        }
+        EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);
+        return -1;
+    }
+
+    if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response
         EC_READ_U8 (command.data + 8) >> 5 != 0x03 || // Download response
         EC_READ_U16(command.data + 9) != sdo_index || // Index
         EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex
     {
-        EC_ERR("Illegal mailbox response at slave %i!\n",
+        EC_ERR("Invalid SDO download response at slave %i!\n",
                slave->ring_position);
         return -1;
     }
@@ -123,6 +168,8 @@
     ec_command_t command;
     ec_master_t *master;
     cycles_t start, end, timeout;
+    uint32_t abort_code;
+    const ec_sdo_abort_message_t *abort_msg;
 
     memset(data, 0x00, 0xF6);
     master = slave->master;
@@ -165,7 +212,7 @@
     while (likely((end - start) < timeout));
 
     if (unlikely((end - start) >= timeout)) {
-        EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position);
+        EC_ERR("Mailbox check on slave %i timed out.\n", slave->ring_position);
         return -1;
     }
 
@@ -176,13 +223,34 @@
         return -1;
     }
 
-    if (EC_READ_U8 (command.data + 5) != 0x03 || // COE
-        EC_READ_U16(command.data + 6) != 0x3000 || // SDO response
+    if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE
+        EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n",
+               slave->ring_position);
+        return -1;
+    }
+
+    if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request
+        EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req.
+        EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n",
+               sdo_index, sdo_subindex, slave->ring_position);
+        abort_code = EC_READ_U32(command.data + 12);
+        for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
+            if (abort_msg->code == abort_code) {
+                EC_ERR("SDO abort message 0x%08X: \"%s\".\n",
+                       abort_msg->code, abort_msg->message);
+                return -1;
+            }
+        }
+        EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);
+        return -1;
+    }
+
+    if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response
         EC_READ_U8 (command.data + 8) >> 5 != 0x02 || // Upload response
         EC_READ_U16(command.data + 9) != sdo_index || // Index
         EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex
     {
-        EC_ERR("Illegal mailbox response at slave %i!\n",
+        EC_ERR("Invalid SDO upload response at slave %i!\n",
                slave->ring_position);
         return -1;
     }
@@ -250,6 +318,45 @@
 
 /*****************************************************************************/
 
+const ec_sdo_abort_message_t sdo_abort_messages[] = {
+    {0x05030000, "Toggle bit not changed"},
+    {0x05040000, "SDO protocol timeout"},
+    {0x05040001, "Client/Server command specifier not valid or unknown"},
+    {0x05040005, "Out of memory"},
+    {0x06010000, "Unsupported access to an object"},
+    {0x06010001, "Attempt to read a write-only object"},
+    {0x06010002, "Attempt to write a read-only object"},
+    {0x06020000, "This object does not exist in the object directory"},
+    {0x06040041, "The object cannot be mapped into the PDO"},
+    {0x06040042, "The number and length of the objects to be mapped would"
+     " exceed the PDO length"},
+    {0x06040043, "General parameter incompatibility reason"},
+    {0x06040047, "Gerneral internal incompatibility in device"},
+    {0x06060000, "Access failure due to a hardware error"},
+    {0x06070010, "Data type does not match, length of service parameter does"
+     " not match"},
+    {0x06070012, "Data type does not match, length of service parameter too"
+     " high"},
+    {0x06070013, "Data type does not match, length of service parameter too"
+     " low"},
+    {0x06090011, "Subindex does not exist"},
+    {0x06090030, "Value range of parameter exceeded"},
+    {0x06090031, "Value of parameter written too high"},
+    {0x06090032, "Value of parameter written too low"},
+    {0x06090036, "Maximum value is less than minimum value"},
+    {0x08000000, "General error"},
+    {0x08000020, "Data cannot be transferred or stored to the application"},
+    {0x08000021, "Data cannot be transferred or stored to the application"
+     " because of local control"},
+    {0x08000022, "Data cannot be transferred or stored to the application"
+     " because of the present device state"},
+    {0x08000023, "Object dictionary dynamic generation fails or no object"
+     " dictionary is present"},
+    {}
+};
+
+/*****************************************************************************/
+
 EXPORT_SYMBOL(ecrt_slave_sdo_write);
 EXPORT_SYMBOL(ecrt_slave_sdo_read);
 EXPORT_SYMBOL(ecrt_master_sdo_write);
--- a/master/master.c	Mon Mar 20 08:36:08 2006 +0000
+++ b/master/master.c	Mon Mar 20 11:02:39 2006 +0000
@@ -564,7 +564,7 @@
             return master->slaves + first;
         }
 
-        EC_ERR("Bus ID \"%s\" - Absolute position illegal!\n", address);
+        EC_ERR("Bus ID \"%s\" - Absolute position invalid!\n", address);
     }
 
     else if (remainder[0] == ':') { // field position
@@ -578,7 +578,7 @@
         }
 
         if (remainder2[0]) {
-            EC_ERR("Bus ID \"%s\" - Illegal trailer (2)!\n", address);
+            EC_ERR("Bus ID \"%s\" - Invalid trailer (2)!\n", address);
             return NULL;
         }
 
@@ -598,7 +598,7 @@
     }
 
     else
-        EC_ERR("Bus ID \"%s\" - Illegal trailer!\n", address);
+        EC_ERR("Bus ID \"%s\" - Invalid trailer!\n", address);
 
     // FIXME ???
 
--- a/master/module.c	Mon Mar 20 08:36:08 2006 +0000
+++ b/master/module.c	Mon Mar 20 11:02:39 2006 +0000
@@ -74,7 +74,7 @@
     EC_INFO("Master driver, %s\n", COMPILE_INFO);
 
     if (ec_master_count < 1) {
-        EC_ERR("Error - Illegal ec_master_count: %i\n", ec_master_count);
+        EC_ERR("Error - Invalid ec_master_count: %i\n", ec_master_count);
         return -1;
     }
 
--- a/mini/mini.c	Mon Mar 20 08:36:08 2006 +0000
+++ b/mini/mini.c	Mon Mar 20 11:02:39 2006 +0000
@@ -29,19 +29,22 @@
 ec_domain_t *domain1 = NULL;
 
 // Datenfelder
+void *r_ssi_input, *r_ssi_status;
 void *r_field[9];
 void *r_4102[3];
 
 // Kanäle
 uint32_t k_pos;
+uint8_t k_stat;
 
 ec_field_init_t domain1_fields[] = {
-    {&r_field[0],   "1", "Beckhoff", "EL5001", "InputValue", 0, 1},
+    {&r_ssi_input,  "1", "Beckhoff", "EL5001", "InputValue",  0, 1},
+    {&r_ssi_status, "1", "Beckhoff", "EL5001", "Status",      0, 1},
     {&r_field[1],   "2", "Beckhoff", "EL4132", "OutputValue", 0, 1},
-    {&r_field[2],   "3", "Beckhoff", "EL3162", "InputValue", 0, 1},
-    {r_4102,        "4", "Beckhoff", "EL4102", "OutputValue", 0, 3},
-    {&r_field[4],   "5", "Beckhoff", "EL5001", "InputValue", 0, 1},
-    {&r_field[5],   "6", "Beckhoff", "EL1014", "InputValue", 0, 1},
+    {&r_field[2],   "3", "Beckhoff", "EL3162", "InputValue",  0, 1},
+    {r_4102,        "4", "Beckhoff", "EL4102", "OutputValue", 0, 2},
+    {&r_field[4],   "5", "Beckhoff", "EL5001", "InputValue",  0, 1},
+    {&r_field[5],   "6", "Beckhoff", "EL1014", "InputValue",  0, 1},
     {&r_field[6],   "7", "Beckhoff", "EL2004", "OutputValue", 0, 1},
     {&r_field[7],   "8", "Beckhoff", "EL4132", "OutputValue", 0, 1},
     {&r_field[8],   "9", "Beckhoff", "EL4132", "OutputValue", 0, 1},
@@ -60,7 +63,8 @@
     ecrt_domain_process(domain1);
 
     // Prozessdaten verarbeiten
-    //  k_pos   = EC_READ_U32(r_ssi);
+    k_pos   = EC_READ_U32(r_ssi_input);
+    k_stat  = EC_READ_U8(r_ssi_status);
 
     // Prozessdaten senden
     ecrt_domain_queue(domain1);
@@ -80,7 +84,8 @@
     }
     else {
         counter = ABTASTFREQUENZ;
-        printk(KERN_INFO "k_pos   = %i\n", k_pos);
+        printk(KERN_INFO "k_pos    = %i\n", k_pos);
+        printk(KERN_INFO "k_stat   = 0x%02X\n", k_stat);
     }
 
     // Timer neu starten
@@ -123,6 +128,23 @@
         goto out_release_master;
     }
 
+    //ecrt_master_debug(master, 2);
+
+    if (ecrt_master_sdo_write(master, "1", 0x4061, 1,  0, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4061, 2,  1, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4061, 3,  1, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4066, 0,  0, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4067, 0,  4, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4068, 0,  0, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x4069, 0, 25, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x406A, 0, 25, 1) ||
+        ecrt_master_sdo_write(master, "1", 0x406B, 0, 50, 1)) {
+        printk(KERN_ERR "EtherCAT: Failed to configure SSI!\n");
+        goto out_deactivate;
+    }
+
+    //ecrt_master_debug(master, 0);
+
 #ifdef ASYNC
     // Einmal senden und warten...
     ecrt_master_prepare_async_io(master);
@@ -139,11 +161,14 @@
 
     return 0;
 
+ out_deactivate:
+    ecrt_master_deactivate(master);
+
  out_release_master:
-  ecrt_release_master(master);
+    ecrt_release_master(master);
 
  out_return:
-  return -1;
+    return -1;
 }
 
 /*****************************************************************************/