# HG changeset patch # User Florian Pose # Date 1143645000 0 # Node ID 998215ab9b92de3530ae7c83d618fea83e07fc2f # Parent 7e743a61a9911e6ef81842d3a71c6d0106bdf0d3 Laden der SDO-Subindizes. diff -r 7e743a61a991 -r 998215ab9b92 master/canopen.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; diff -r 7e743a61a991 -r 998215ab9b92 master/slave.c --- 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); + } } } diff -r 7e743a61a991 -r 998215ab9b92 master/slave.h --- 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 */ diff -r 7e743a61a991 -r 998215ab9b92 rt/msr_module.c --- 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) ||