Bugfix: SDO download. SDO abort codes ausgeben.
--- 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;
}
/*****************************************************************************/