# HG changeset patch # User Florian Pose # Date 1143560719 0 # Node ID a03a684cac8958cabfa4547ab0c3bdd51c3acaf3 # Parent 80d4939172056f55c477e0d9039724fd60e087f5 Dynamische Mailbox-Kommunikation, auch mit unbekannten Slaves. diff -r 80d493917205 -r a03a684cac89 master/master.c --- a/master/master.c Tue Mar 28 13:42:50 2006 +0000 +++ b/master/master.c Tue Mar 28 15:45:19 2006 +0000 @@ -662,6 +662,26 @@ /*****************************************************************************/ /** + Initialisiert eine Sync-Manager-Konfigurationsseite mit EEPROM-Daten. + + Der mit \a data referenzierte Speicher muss mindestens EC_SYNC_SIZE Bytes + groß sein. +*/ + +void ec_eeprom_sync_config(const ec_eeprom_sync_t *sync, /**< Sync-Manager */ + uint8_t *data /**> Zeiger auf Konfig.-Speicher */ + ) +{ + EC_WRITE_U16(data, sync->physical_start_address); + EC_WRITE_U16(data + 2, sync->length); + EC_WRITE_U8 (data + 4, sync->control_register); + EC_WRITE_U8 (data + 5, 0x00); // status byte (read only) + EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable +} + +/*****************************************************************************/ + +/** Initialisiert eine FMMU-Konfigurationsseite. Der mit \a data referenzierte Speicher muss mindestens EC_FMMU_SIZE Bytes @@ -778,6 +798,7 @@ uint8_t data[256]; uint32_t domain_offset; ec_domain_t *domain; + ec_eeprom_sync_t *eeprom_sync; // Domains erstellen domain_offset = 0; @@ -799,12 +820,8 @@ return -1; // Check if slave was registered... - if (!slave->type) { - EC_WARN("Slave %i has unknown type!\n", i); - continue; - } - type = slave->type; + if (!type) EC_WARN("Slave %i has unknown type!\n", i); // Check and reset CRC fault counters ec_slave_check_crc(slave); @@ -834,18 +851,32 @@ } // Set Sync Managers - for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) - { - sync = type->sync_managers[j]; - - ec_sync_config(sync, data); - ec_command_init_npwr(&command, slave->station_address, - 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, - data); - if (unlikely(ec_master_simple_io(master, &command))) { - EC_ERR("Setting sync manager %i failed on slave %i!\n", - j, slave->ring_position); - return -1; + if (type) { + for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) + { + sync = type->sync_managers[j]; + ec_sync_config(sync, data); + ec_command_init_npwr(&command, slave->station_address, + 0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, + data); + if (unlikely(ec_master_simple_io(master, &command))) { + EC_ERR("Setting sync manager %i failed on slave %i!\n", + j, slave->ring_position); + return -1; + } + } + } + else if (slave->sii_mailbox_protocols) { // Unknown slave, has mailbox + list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) { + ec_eeprom_sync_config(eeprom_sync, data); + ec_command_init_npwr(&command, slave->station_address, + 0x800 + eeprom_sync->index * EC_SYNC_SIZE, + EC_SYNC_SIZE, data); + if (unlikely(ec_master_simple_io(master, &command))) { + EC_ERR("Setting sync manager %i failed on slave %i!\n", + eeprom_sync->index, slave->ring_position); + return -1; + } } } @@ -853,6 +884,9 @@ if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_PREOP))) return -1; + // Stop activation here for slaves without type + if (!type) continue; + // Slaves that are not registered are only brought into PREOP // state -> nice blinking and mailbox comm. possible if (!slave->registered && !slave->type->bus_coupler) { @@ -1169,7 +1203,7 @@ ec_slave_t *slave; if (!(slave = ec_master_slave_address(master, slave_address))) return -1; - return ec_slave_sii_write(slave, 0x0004, alias); + return ec_slave_sii_write16(slave, 0x0004, alias); } /*****************************************************************************/ diff -r 80d493917205 -r a03a684cac89 master/slave.c --- a/master/slave.c Tue Mar 28 13:42:50 2006 +0000 +++ b/master/slave.c Tue Mar 28 15:45:19 2006 +0000 @@ -48,6 +48,10 @@ slave->sii_product_code = 0; slave->sii_revision_number = 0; slave->sii_serial_number = 0; + slave->sii_rx_mailbox_offset = 0; + slave->sii_rx_mailbox_size = 0; + slave->sii_tx_mailbox_offset = 0; + slave->sii_tx_mailbox_size = 0; slave->sii_mailbox_protocols = 0; slave->type = NULL; slave->registered = 0; @@ -143,44 +147,29 @@ if (slave->base_fmmu_count > EC_MAX_FMMUS) slave->base_fmmu_count = EC_MAX_FMMUS; - // Read identification from "Slave Information Interface" (SII) - - if (unlikely(ec_slave_sii_read(slave, 0x0004, - (uint32_t *) &slave->sii_alias))) { - EC_ERR("Could not read SII alias!\n"); - return -1; - } - - if (unlikely(ec_slave_sii_read(slave, 0x0008, &slave->sii_vendor_id))) { - EC_ERR("Could not read SII vendor id!\n"); - return -1; - } - - if (unlikely(ec_slave_sii_read(slave, 0x000A, &slave->sii_product_code))) { - EC_ERR("Could not read SII product code!\n"); - return -1; - } - - if (unlikely(ec_slave_sii_read(slave, 0x000C, - &slave->sii_revision_number))) { - EC_ERR("Could not read SII revision number!\n"); - return -1; - } - - if (unlikely(ec_slave_sii_read(slave, 0x000E, - &slave->sii_serial_number))) { - EC_ERR("Could not read SII serial number!\n"); - return -1; - } - - if (unlikely(ec_slave_sii_read(slave, 0x001C, - &slave->sii_mailbox_protocols))) { - EC_ERR("Could not read SII supported mailbox protocols!\n"); - return -1; - } + if (ec_slave_sii_read16(slave, 0x0004, &slave->sii_alias)) + return -1; + if (ec_slave_sii_read32(slave, 0x0008, &slave->sii_vendor_id)) + return -1; + if (ec_slave_sii_read32(slave, 0x000A, &slave->sii_product_code)) + return -1; + if (ec_slave_sii_read32(slave, 0x000C, &slave->sii_revision_number)) + return -1; + if (ec_slave_sii_read32(slave, 0x000E, &slave->sii_serial_number)) + return -1; + if (ec_slave_sii_read16(slave, 0x0018, &slave->sii_rx_mailbox_offset)) + return -1; + if (ec_slave_sii_read16(slave, 0x0019, &slave->sii_rx_mailbox_size)) + return -1; + if (ec_slave_sii_read16(slave, 0x001A, &slave->sii_tx_mailbox_offset)) + return -1; + if (ec_slave_sii_read16(slave, 0x001B, &slave->sii_tx_mailbox_size)) + return -1; + if (ec_slave_sii_read16(slave, 0x001C, &slave->sii_mailbox_protocols)) + return -1; if (unlikely(ec_slave_fetch_categories(slave))) { - EC_ERR("Could not fetch category data!\n"); + EC_ERR("Failed to fetch category data!\n"); return -1; } @@ -190,20 +179,19 @@ /*****************************************************************************/ /** - Liest Daten aus dem Slave-Information-Interface + Liest 16 Bit aus dem Slave-Information-Interface eines EtherCAT-Slaves. \return 0 bei Erfolg, sonst < 0 */ -int ec_slave_sii_read(ec_slave_t *slave, - /**< EtherCAT-Slave */ - uint16_t offset, - /**< Adresse des zu lesenden SII-Registers */ - uint32_t *target - /**< Zeiger auf einen 4 Byte großen Speicher zum Ablegen - der Daten */ - ) +int ec_slave_sii_read16(ec_slave_t *slave, + /**< EtherCAT-Slave */ + uint16_t offset, + /**< Adresse des zu lesenden SII-Registers */ + uint16_t *target + /**< Speicher für Wert (16-Bit) */ + ) { ec_command_t command; uint8_t data[10]; @@ -242,6 +230,71 @@ end = get_cycles(); if (likely((EC_READ_U8(command.data + 1) & 0x81) == 0)) { + *target = EC_READ_U16(command.data + 6); + return 0; + } + + if (unlikely((end - start) >= timeout)) { + EC_ERR("SII-read. Slave %i timed out!\n", slave->ring_position); + return -1; + } + } +} + +/*****************************************************************************/ + +/** + Liest 32 Bit aus dem Slave-Information-Interface + eines EtherCAT-Slaves. + + \return 0 bei Erfolg, sonst < 0 +*/ + +int ec_slave_sii_read32(ec_slave_t *slave, + /**< EtherCAT-Slave */ + uint16_t offset, + /**< Adresse des zu lesenden SII-Registers */ + uint32_t *target + /**< Speicher für Wert (32-Bit) */ + ) +{ + ec_command_t command; + uint8_t data[10]; + cycles_t start, end, timeout; + + // Initiate read operation + + EC_WRITE_U8 (data, 0x00); // read-only access + EC_WRITE_U8 (data + 1, 0x01); // request read operation + EC_WRITE_U32(data + 2, offset); + + ec_command_init_npwr(&command, slave->station_address, 0x502, 6, data); + if (unlikely(ec_master_simple_io(slave->master, &command))) { + EC_ERR("SII-read failed on slave %i!\n", slave->ring_position); + return -1; + } + + // Der Slave legt die Informationen des Slave-Information-Interface + // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange + // den Status auslesen, bis das Bit weg ist. + + start = get_cycles(); + timeout = (cycles_t) 100 * cpu_khz; // 100ms + + while (1) + { + udelay(10); + + ec_command_init_nprd(&command, slave->station_address, 0x502, 10); + if (unlikely(ec_master_simple_io(slave->master, &command))) { + EC_ERR("Getting SII-read status failed on slave %i!\n", + slave->ring_position); + return -1; + } + + end = get_cycles(); + + if (likely((EC_READ_U8(command.data + 1) & 0x81) == 0)) { *target = EC_READ_U32(command.data + 6); return 0; } @@ -256,19 +309,19 @@ /*****************************************************************************/ /** - Schreibt Daten in das Slave-Information-Interface + Schreibt 16 Bit Daten in das Slave-Information-Interface eines EtherCAT-Slaves. \return 0 bei Erfolg, sonst < 0 */ -int ec_slave_sii_write(ec_slave_t *slave, - /**< EtherCAT-Slave */ - uint16_t offset, - /**< Adresse des zu lesenden SII-Registers */ - uint16_t value - /**< Zu schreibender Wert */ - ) +int ec_slave_sii_write16(ec_slave_t *slave, + /**< EtherCAT-Slave */ + uint16_t offset, + /**< Adresse des zu lesenden SII-Registers */ + uint16_t value + /**< Zu schreibender Wert */ + ) { ec_command_t command; uint8_t data[8]; @@ -352,7 +405,7 @@ while (1) { // read category type - if (ec_slave_sii_read(slave, word_offset, &value)) { + if (ec_slave_sii_read32(slave, word_offset, &value)) { EC_ERR("Unable to read category header.\n"); goto out_free; } @@ -365,7 +418,7 @@ // Fetch category data for (i = 0; i < word_count; i++) { - if (ec_slave_sii_read(slave, word_offset + 2 + i, &value)) { + if (ec_slave_sii_read32(slave, word_offset + 2 + i, &value)) { EC_ERR("Unable to read category data word %i.\n", i); goto out_free; } @@ -824,11 +877,13 @@ EC_INFO("| Supported FMMUs: %i, Sync managers: %i\n", slave->base_fmmu_count, slave->base_sync_count); - EC_INFO("| Supported mailbox protocols: "); - if (!slave->sii_mailbox_protocols) { - printk("(none)"); - } - else { + if (slave->sii_mailbox_protocols) { + EC_INFO("| Mailbox communication:\n"); + EC_INFO("| RX mailbox: 0x%04X/%i, TX mailbox: 0x%04X/%i\n", + slave->sii_rx_mailbox_offset, slave->sii_rx_mailbox_size, + slave->sii_tx_mailbox_offset, slave->sii_tx_mailbox_size); + EC_INFO("| Supported protocols: "); + first = 1; if (slave->sii_mailbox_protocols & EC_MBOX_AOE) { printk("AoE"); @@ -858,8 +913,8 @@ if (!first) printk(", "); printk("VoE"); } - } - printk("\n"); + printk("\n"); + } EC_INFO("| EEPROM data:\n"); @@ -983,29 +1038,45 @@ size_t size /**< Datengröße */ ) { - uint8_t data[0xF6]; + size_t total_size; + uint8_t *data; ec_command_t command; - if (unlikely(size + 6 > 0xF6)) { + if (unlikely(!slave->sii_mailbox_protocols)) { + EC_ERR("Slave %i does not support mailbox communication!\n", + slave->ring_position); + return -1; + } + + total_size = size + 6; + if (unlikely(total_size > slave->sii_rx_mailbox_size)) { EC_ERR("Data size does not fit in mailbox!\n"); return -1; } - memset(data, 0x00, 0xF6); - + if (!(data = kmalloc(slave->sii_rx_mailbox_size, GFP_KERNEL))) { + EC_ERR("Failed to allocate %i bytes of memory for mailbox data!\n", + slave->sii_rx_mailbox_size); + return -1; + } + + memset(data, 0x00, slave->sii_rx_mailbox_size); EC_WRITE_U16(data, size); // Length of the Mailbox service data EC_WRITE_U16(data + 2, slave->station_address); // Station address EC_WRITE_U8 (data + 4, 0x00); // Channel & priority EC_WRITE_U8 (data + 5, type); // Underlying protocol type - memcpy(data + 6, prot_data, size); - ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); + ec_command_init_npwr(&command, slave->station_address, + slave->sii_rx_mailbox_offset, + slave->sii_rx_mailbox_size, data); if (unlikely(ec_master_simple_io(slave->master, &command))) { EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); - return -1; - } - + kfree(data); + return -1; + } + + kfree(data); return 0; } @@ -1032,6 +1103,7 @@ while (1) { + // FIXME: Zweiter Sync-Manager nicht immer TX-Mailbox? ec_command_init_nprd(&command, slave->station_address, 0x808, 8); if (unlikely(ec_master_simple_io(slave->master, &command))) { EC_ERR("Mailbox checking failed on slave %i!\n", @@ -1041,7 +1113,8 @@ end = get_cycles(); - if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high + if (EC_READ_U8(command.data + 5) & 8) + break; // Proceed with received data if ((end - start) >= timeout) { EC_ERR("Mailbox check - Slave %i timed out.\n", @@ -1052,29 +1125,37 @@ udelay(100); } - 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); + ec_command_init_nprd(&command, slave->station_address, + slave->sii_tx_mailbox_offset, + slave->sii_tx_mailbox_size); if (unlikely(ec_master_simple_io(slave->master, &command))) { EC_ERR("Mailbox receiving failed on slave %i!\n", slave->ring_position); return -1; } - if (EC_READ_U8(command.data + 5) != type) { // nicht CoE - EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", + if ((EC_READ_U8(command.data + 5) & 0x0F) != type) { + EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at" + " slave %i!\n", EC_READ_U8(command.data + 5), type, slave->ring_position); return -1; } + if (unlikely(slave->master->debug_level) > 1) + EC_DBG("Mailbox receive took %ius.\n", ((u32) (end - start) * 1000 + / cpu_khz)); + if ((data_size = EC_READ_U16(command.data)) > *size) { - EC_ERR("CoE data does not fit in buffer (%i > %i).\n", + EC_ERR("Mailbox service data does not fit into buffer (%i > %i).\n", data_size, *size); return -1; } + if (data_size > slave->sii_tx_mailbox_size - 6) { + EC_ERR("Currupt mailbox response detected!\n"); + return -1; + } + memcpy(prot_data, command.data + 6, data_size); *size = data_size; return 0; diff -r 80d493917205 -r a03a684cac89 master/slave.h --- a/master/slave.h Tue Mar 28 13:42:50 2006 +0000 +++ b/master/slave.h Tue Mar 28 15:45:19 2006 +0000 @@ -187,7 +187,11 @@ uint32_t sii_product_code; /**< Herstellerspezifischer Produktcode */ uint32_t sii_revision_number; /**< Revisionsnummer */ uint32_t sii_serial_number; /**< Seriennummer der Klemme */ - uint32_t sii_mailbox_protocols; /**< Unterstützte Mailbox-Protokolle */ + uint16_t sii_rx_mailbox_offset; /**< Adresse der Mailbox (Master->Slave) */ + uint16_t sii_rx_mailbox_size; /**< Adresse der Mailbox (Master->Slave) */ + uint16_t sii_tx_mailbox_offset; /**< Adresse der Mailbox (Slave->Master) */ + uint16_t sii_tx_mailbox_size; /**< Adresse der Mailbox (Slave->Master) */ + uint16_t sii_mailbox_protocols; /**< Unterstützte Mailbox-Protokolle */ const ec_slave_type_t *type; /**< Zeiger auf die Beschreibung des Slave-Typs */ @@ -216,8 +220,9 @@ // Slave control int ec_slave_fetch(ec_slave_t *); -int ec_slave_sii_read(ec_slave_t *, uint16_t, uint32_t *); -int ec_slave_sii_write(ec_slave_t *, uint16_t, uint16_t); +int ec_slave_sii_read16(ec_slave_t *, uint16_t, uint16_t *); +int ec_slave_sii_read32(ec_slave_t *, uint16_t, uint32_t *); +int ec_slave_sii_write16(ec_slave_t *, uint16_t, uint16_t); int ec_slave_state_change(ec_slave_t *, uint8_t); int ec_slave_set_fmmu(ec_slave_t *, const ec_domain_t *, const ec_sync_t *); diff -r 80d493917205 -r a03a684cac89 rt/msr_module.c --- a/rt/msr_module.c Tue Mar 28 13:42:50 2006 +0000 +++ b/rt/msr_module.c Tue Mar 28 15:45:19 2006 +0000 @@ -38,6 +38,7 @@ #include "../include/ecrt.h" #define ASYNC +//#define BLOCK1 // Defines/Makros #define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ) @@ -52,30 +53,33 @@ // EtherCAT ec_master_t *master = NULL; ec_domain_t *domain1 = NULL; -ec_domain_t *domain2 = NULL; // Prozessdaten +#ifdef BLOCK1 void *r_ssi, *r_ssi_st; -void *r_ssi2; +#else void *r_inc; - -uint32_t k_angle; +#endif + uint32_t k_ssi_pos; uint32_t k_ssi_status; +uint32_t k_angle; uint32_t k_preio; uint32_t k_postio; uint32_t k_finished; +#ifdef BLOCK1 ec_field_init_t domain1_fields[] = { {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0}, {&r_ssi_st, "1", "Beckhoff", "EL5001", "Status", 0}, {} }; - -ec_field_init_t domain2_fields[] = { - {&r_ssi2, "1", "Beckhoff", "EL5001", "InputValue", 0}, +#else +ec_field_init_t domain1_fields[] = { + {&r_inc, "4", "Beckhoff", "EL5101", "InputValue", 0}, {} }; +#endif /*****************************************************************************/ @@ -98,28 +102,33 @@ // Empfangen ecrt_master_async_receive(master); ecrt_domain_process(domain1); - ecrt_domain_process(domain2); // Prozessdaten verarbeiten +#ifdef BLOCK1 k_ssi_pos = EC_READ_U32(r_ssi); k_ssi_status = EC_READ_U32(r_ssi_st); +#else + k_angle = EC_READ_U16(r_inc); +#endif // Senden ecrt_domain_queue(domain1); - ecrt_domain_queue(domain2); ecrt_master_async_send(master); #else // Senden und empfangen ecrt_domain_queue(domain1); - ecrt_domain_queue(domain2); ecrt_master_sync_io(master); ecrt_domain_process(domain1); - ecrt_domain_process(domain2); // Prozessdaten verarbeiten +#ifdef BLOCK1 k_ssi_pos = EC_READ_U32(r_ssi); k_ssi_status = EC_READ_U32(r_ssi_st); -#endif +#else + k_angle = EC_READ_U16(r_inc); +#endif + +#endif // ASYNC k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; @@ -195,11 +204,6 @@ goto out_release_master; } - if (!(domain2 = ecrt_master_create_domain(master))) { - printk(KERN_ERR "Could not register domain!\n"); - goto out_release_master; - } - printk(KERN_INFO "Registering domain fields...\n"); if (ecrt_domain_register_field_list(domain1, domain1_fields)) { @@ -207,11 +211,6 @@ goto out_release_master; } - if (ecrt_domain_register_field_list(domain2, domain2_fields)) { - printk(KERN_ERR "Failed to register domain fields.\n"); - goto out_release_master; - } - printk(KERN_INFO "Activating master...\n"); if (ecrt_master_activate(master)) { @@ -219,6 +218,8 @@ goto out_release_master; } + ecrt_master_print(master); + //ecrt_master_debug(master, 2); if (ecrt_master_fetch_sdo_lists(master)) { printk(KERN_ERR "Failed to fetch SDO lists!\n"); @@ -228,7 +229,7 @@ ecrt_master_print(master); -#if 1 +#ifdef BLOCK1 if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) { printk(KERN_ERR "Could not read SSI version!\n"); goto out_deactivate;