# HG changeset patch # User Florian Pose # Date 1143553370 0 # Node ID 80d4939172056f55c477e0d9039724fd60e087f5 # Parent aecc8cb720979be0b492b8d60023b02f01fb4ba4 Laden der SDO Dictionaries funktioniert. diff -r aecc8cb72097 -r 80d493917205 include/ecrt.h --- 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); diff -r aecc8cb72097 -r 80d493917205 master/canopen.c --- 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; } diff -r aecc8cb72097 -r 80d493917205 master/master.c --- 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); diff -r aecc8cb72097 -r 80d493917205 master/slave.c --- 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"); } diff -r aecc8cb72097 -r 80d493917205 master/slave.h --- 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 */ }; /*****************************************************************************/ diff -r aecc8cb72097 -r 80d493917205 mini/mini.c --- 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); } diff -r aecc8cb72097 -r 80d493917205 rt/msr_module.c --- 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) ||