--- a/include/ecrt.h Tue Mar 28 12:38:05 2006 +0000
+++ b/include/ecrt.h Tue Mar 28 13:42:50 2006 +0000
@@ -52,6 +52,7 @@
ec_domain_t *ecrt_master_create_domain(ec_master_t *master);
int ecrt_master_activate(ec_master_t *master);
void ecrt_master_deactivate(ec_master_t *master);
+int ecrt_master_fetch_sdo_lists(ec_master_t *master);
void ecrt_master_sync_io(ec_master_t *master);
void ecrt_master_async_send(ec_master_t *master);
void ecrt_master_async_receive(ec_master_t *master);
--- a/master/canopen.c Tue Mar 28 12:38:05 2006 +0000
+++ b/master/canopen.c Tue Mar 28 13:42:50 2006 +0000
@@ -30,6 +30,7 @@
const ec_sdo_abort_message_t sdo_abort_messages[];
void ec_canopen_abort_msg(uint32_t);
+int ec_slave_fetch_sdo_descriptions(ec_slave_t *);
/*****************************************************************************/
@@ -205,10 +206,11 @@
{
uint8_t data[0xF0];
size_t rec_size;
-
- //EC_DBG("Fetching SDO list for slave %i...\n", slave->ring_position);
-
- EC_WRITE_U16(data, 0x8000); // Number (0), Service (get OD request)
+ unsigned int i, sdo_count;
+ ec_sdo_t *sdo;
+ uint16_t sdo_index;
+
+ EC_WRITE_U16(data, 0x8000); // Number (0), Service = SDO information
EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request
EC_WRITE_U8 (data + 3, 0x00); // res.
EC_WRITE_U16(data + 4, 0x0000); // fragments left
@@ -216,21 +218,12 @@
if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1;
- do
- {
+ do {
rec_size = 0xF0;
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 transf. req.
- EC_ERR("SDO list download aborted on slave %i.\n",
- slave->ring_position);
- ec_canopen_abort_msg(EC_READ_U32(data + 12));
- return -1;
- }
-
if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information
- (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Get OD List response
+ (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response
EC_ERR("SDO information error response at slave %i!\n",
slave->ring_position);
ec_canopen_abort_msg(EC_READ_U32(data + 6));
@@ -249,12 +242,97 @@
return -1;
}
-#if 0
- for (i = 0; i < (rec_size - 8) / 2; i++)
- EC_INFO("Object 0x%04X\n", EC_READ_U16(data + 8 + i * 2));
-#endif
- }
- while (EC_READ_U8(data + 2) & 0x80);
+ sdo_count = (rec_size - 8) / 2;
+ for (i = 0; i < sdo_count; i++) {
+ sdo_index = EC_READ_U16(data + 8 + i * 2);
+ if (!sdo_index) continue; // Manchmal ist der Index 0... ???
+
+ if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) {
+ EC_ERR("Failed to allocate memory for SDO!\n");
+ return -1;
+ }
+ sdo->index = sdo_index;
+ sdo->name = NULL;
+ list_add_tail(&sdo->list, &slave->sdo_dictionary);
+ }
+ } while (EC_READ_U8(data + 2) & 0x80);
+
+ // Jetzt alle Beschreibungen holen
+ if (ec_slave_fetch_sdo_descriptions(slave)) return -1;
+
+ return 0;
+}
+
+/*****************************************************************************/
+
+/**
+ Holt die Beschreibungen zu allen bereits bekannten SDOs.
+
+ \return 0, wenn alles ok, sonst < 0
+*/
+
+int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave /**< EtherCAT-Slave */)
+{
+ uint8_t data[0xF0];
+ size_t rec_size, name_size;
+ ec_sdo_t *sdo;
+
+ list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
+ EC_WRITE_U16(data, 0x8000); // Number (0), Service = SDO inform.
+ EC_WRITE_U8 (data + 2, 0x03); // Get object description request
+ EC_WRITE_U8 (data + 3, 0x00); // res.
+ EC_WRITE_U16(data + 4, 0x0000); // fragments left
+ EC_WRITE_U16(data + 6, sdo->index); // SDO index
+ if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1;
+
+ rec_size = 0xF0;
+ if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size))
+ return -1;
+
+ if (EC_READ_U16(data) >> 12 == 0x08 && // 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,
+ sdo->index);
+ ec_canopen_abort_msg(EC_READ_U32(data + 6));
+ return -1;
+ }
+
+ if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information
+ (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Obj. desc. resp.
+ 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,
+ sdo->index);
+ return -1;
+ }
+
+ if (rec_size < 12) {
+ EC_ERR("Invalid data size!\n");
+ return -1;
+ }
+
+ sdo->type = EC_READ_U16(data + 8);
+ sdo->max_subindex = EC_READ_U8(data + 10);
+ sdo->features = EC_READ_U8(data + 11);
+
+ name_size = rec_size - 12;
+ if (!name_size) {
+ EC_WARN("Object 0x%04X name size is 0...", sdo->index);
+ 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");
+ }
return 0;
}
--- a/master/master.c Tue Mar 28 12:38:05 2006 +0000
+++ b/master/master.c Tue Mar 28 13:42:50 2006 +0000
@@ -853,14 +853,6 @@
if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_PREOP)))
return -1;
- // Fetch SDO list
- if (slave->sii_mailbox_protocols & EC_MBOX_COE) {
- if (unlikely(ec_slave_fetch_sdo_list(slave))) {
- EC_ERR("Could not fetch SDO list!\n");
- return -1;
- }
- }
-
// Slaves that are not registered are only brought into PREOP
// state -> nice blinking and mailbox comm. possible
if (!slave->registered && !slave->type->bus_coupler) {
@@ -918,6 +910,36 @@
}
}
+
+/*****************************************************************************/
+
+/**
+ Lädt die SDO-Dictionaries aller Slaves.
+
+ Slaves, die kein CoE unterstützen, werden ausgelassen.
+
+ \return 0 wenn alles ok, sonst < 0
+*/
+
+int ecrt_master_fetch_sdo_lists(ec_master_t *master /**< EtherCAT-Master */)
+{
+ ec_slave_t *slave;
+ unsigned int i;
+
+ for (i = 0; i < master->slave_count; i++) {
+ slave = master->slaves + i;
+ if (slave->sii_mailbox_protocols & EC_MBOX_COE) {
+ if (unlikely(ec_slave_fetch_sdo_list(slave))) {
+ EC_ERR("Failed to fetch SDO list on slave %i!\n",
+ slave->ring_position);
+ return -1;
+ }
+ }
+ }
+
+ return 0;
+}
+
/*****************************************************************************/
/**
@@ -1155,6 +1177,7 @@
EXPORT_SYMBOL(ecrt_master_create_domain);
EXPORT_SYMBOL(ecrt_master_activate);
EXPORT_SYMBOL(ecrt_master_deactivate);
+EXPORT_SYMBOL(ecrt_master_fetch_sdo_lists);
EXPORT_SYMBOL(ecrt_master_prepare_async_io);
EXPORT_SYMBOL(ecrt_master_sync_io);
EXPORT_SYMBOL(ecrt_master_async_send);
--- a/master/slave.c Tue Mar 28 12:38:05 2006 +0000
+++ b/master/slave.c Tue Mar 28 13:42:50 2006 +0000
@@ -58,6 +58,7 @@
INIT_LIST_HEAD(&slave->eeprom_strings);
INIT_LIST_HEAD(&slave->eeprom_syncs);
INIT_LIST_HEAD(&slave->eeprom_pdos);
+ INIT_LIST_HEAD(&slave->sdo_dictionary);
}
/*****************************************************************************/
@@ -72,6 +73,7 @@
ec_eeprom_sync_t *sync, *next_sync;
ec_eeprom_pdo_t *pdo, *next_pdo;
ec_eeprom_pdo_entry_t *entry, *next_ent;
+ ec_sdo_t *sdo, *next_sdo;
// Alle Strings freigeben
list_for_each_entry_safe(string, next_str, &slave->eeprom_strings, list) {
@@ -103,6 +105,13 @@
if (slave->eeprom_name) kfree(slave->eeprom_name);
if (slave->eeprom_group) kfree(slave->eeprom_group);
if (slave->eeprom_desc) kfree(slave->eeprom_desc);
+
+ // Alle SDOs freigeben
+ list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
+ list_del(&sdo->list);
+ if (sdo->name) kfree(sdo->name);
+ kfree(sdo);
+ }
}
/*****************************************************************************/
@@ -792,6 +801,7 @@
ec_eeprom_sync_t *sync;
ec_eeprom_pdo_t *pdo;
ec_eeprom_pdo_entry_t *entry;
+ ec_sdo_t *sdo;
int first;
EC_INFO("x-- EtherCAT slave information ---------------\n");
@@ -892,6 +902,16 @@
}
}
+ 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,
+ sdo->name ? sdo->name : "");
+ EC_INFO("| Type 0x%04X, subindices: %i, features: 0x%02X\n",
+ sdo->type, sdo->max_subindex, sdo->features);
+ }
+ }
+
EC_INFO("x---------------------------------------------\n");
}
--- a/master/slave.h Tue Mar 28 12:38:05 2006 +0000
+++ b/master/slave.h Tue Mar 28 13:42:50 2006 +0000
@@ -146,6 +146,23 @@
/*****************************************************************************/
/**
+ CANopen-SDO.
+*/
+
+typedef struct
+{
+ struct list_head list;
+ uint16_t index;
+ uint16_t type;
+ uint8_t max_subindex;
+ uint8_t features;
+ char *name;
+}
+ec_sdo_t;
+
+/*****************************************************************************/
+
+/**
EtherCAT-Slave
*/
@@ -187,6 +204,8 @@
char *eeprom_name; /**< Slave-Name laut Hersteller */
char *eeprom_group; /**< Slave-Beschreibung laut Hersteller */
char *eeprom_desc; /**< Slave-Beschreibung laut Hersteller */
+
+ struct list_head sdo_dictionary; /**< SDO-Verzeichnis des Slaves */
};
/*****************************************************************************/
--- a/mini/mini.c Tue Mar 28 12:38:05 2006 +0000
+++ b/mini/mini.c Tue Mar 28 13:42:50 2006 +0000
@@ -94,7 +94,7 @@
printk(KERN_INFO "=== Starting Minimal EtherCAT environment... ===\n");
if ((master = ecrt_request_master(0)) == NULL) {
- printk(KERN_ERR "Error requesting master 0!\n");
+ printk(KERN_ERR "Requesting master 0 failed!\n");
goto out_return;
}
@@ -104,24 +104,29 @@
if (!(domain1 = ecrt_master_create_domain(master)))
{
- printk(KERN_ERR "EtherCAT: Could not register domain!\n");
+ printk(KERN_ERR "Domain creation failed!\n");
goto out_release_master;
}
printk(KERN_INFO "Registering domain fields...\n");
if (ecrt_domain_register_field_list(domain1, domain1_fields)) {
- printk(KERN_ERR "EtherCAT: Could not register field!\n");
+ printk(KERN_ERR "Field registration failed!\n");
goto out_release_master;
}
printk(KERN_INFO "Activating master...\n");
if (ecrt_master_activate(master)) {
- printk(KERN_ERR "EtherCAT: Could not activate master!\n");
+ printk(KERN_ERR "Failed to activate master!\n");
goto out_release_master;
}
+ if (ecrt_master_fetch_sdo_lists(master)) {
+ printk(KERN_ERR "Failed to fetch SDO lists!\n");
+ goto out_deactivate;
+ }
+
//ecrt_master_debug(master, 2);
#if 0
@@ -134,7 +139,7 @@
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");
+ printk(KERN_ERR "Failed to configure SSI slave!\n");
goto out_deactivate;
}
#endif
@@ -144,7 +149,7 @@
#if 0
printk(KERN_INFO "Writing alias...\n");
if (ecrt_master_write_slave_alias(master, "0", 0xBEEF)) {
- printk(KERN_ERR "EtherCAT: Failed to write alias!\n");
+ printk(KERN_ERR "Failed to write alias!\n");
goto out_deactivate;
}
#endif
@@ -162,17 +167,12 @@
add_timer(&timer);
printk(KERN_INFO "=== Minimal EtherCAT environment started. ===\n");
-
return 0;
-#if 0
out_deactivate:
ecrt_master_deactivate(master);
-#endif
-
out_release_master:
ecrt_release_master(master);
-
out_return:
return -1;
}
@@ -183,12 +183,9 @@
{
printk(KERN_INFO "=== Stopping Minimal EtherCAT environment... ===\n");
- if (master)
- {
+ if (master) {
del_timer_sync(&timer);
-
printk(KERN_INFO "Deactivating master...\n");
-
ecrt_master_deactivate(master);
ecrt_release_master(master);
}
--- a/rt/msr_module.c Tue Mar 28 12:38:05 2006 +0000
+++ b/rt/msr_module.c Tue Mar 28 13:42:50 2006 +0000
@@ -188,8 +188,6 @@
goto out_msr_cleanup;
}
- ecrt_master_print(master);
-
printk(KERN_INFO "Registering domains...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
@@ -216,24 +214,27 @@
printk(KERN_INFO "Activating master...\n");
- //ecrt_master_debug(master, 2);
-
if (ecrt_master_activate(master)) {
printk(KERN_ERR "Could not activate master!\n");
goto out_release_master;
}
+ //ecrt_master_debug(master, 2);
+ if (ecrt_master_fetch_sdo_lists(master)) {
+ printk(KERN_ERR "Failed to fetch SDO lists!\n");
+ goto out_deactivate;
+ }
//ecrt_master_debug(master, 0);
+ ecrt_master_print(master);
+
#if 1
if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) {
printk(KERN_ERR "Could not read SSI version!\n");
goto out_deactivate;
}
printk(KERN_INFO "Software-version: %u\n", version);
-#endif
-
-#if 1
+
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) ||