bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Dynamische Mailbox-Kommunikation, auch mit unbekannten Slaves.

This commit is contained in:
Florian Pose 2006-03-28 15:45:19 +00:00
parent 1e150e4553
commit 3b8f2472a5
4 changed files with 232 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
master/master.c
View File

@ -661,6 +661,26 @@ void ec_sync_config(const ec_sync_t *sync, /**< Sync-Manager */
/*****************************************************************************/
/**
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.
@ -778,6 +798,7 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT-Master */)
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 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT-Master */)
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 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT-Master */)
}
// 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 @@ int ecrt_master_activate(ec_master_t *master /**< EtherCAT-Master */)
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 @@ int ecrt_master_write_slave_alias(ec_master_t *master,
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);
}
/*****************************************************************************/

219
master/slave.c
View File

@ -48,6 +48,10 @@ void ec_slave_init(ec_slave_t *slave, /**< EtherCAT-Slave */
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 @@ int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */)
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");
if (ec_slave_sii_read16(slave, 0x0004, &slave->sii_alias))
return -1;
}
if (unlikely(ec_slave_sii_read(slave, 0x0008, &slave->sii_vendor_id))) {
EC_ERR("Could not read SII vendor id!\n");
if (ec_slave_sii_read32(slave, 0x0008, &slave->sii_vendor_id))
return -1;
}
if (unlikely(ec_slave_sii_read(slave, 0x000A, &slave->sii_product_code))) {
EC_ERR("Could not read SII product code!\n");
if (ec_slave_sii_read32(slave, 0x000A, &slave->sii_product_code))
return -1;
}
if (unlikely(ec_slave_sii_read(slave, 0x000C,
&slave->sii_revision_number))) {
EC_ERR("Could not read SII revision number!\n");
if (ec_slave_sii_read32(slave, 0x000C, &slave->sii_revision_number))
return -1;
}
if (unlikely(ec_slave_sii_read(slave, 0x000E,
&slave->sii_serial_number))) {
EC_ERR("Could not read SII serial number!\n");
if (ec_slave_sii_read32(slave, 0x000E, &slave->sii_serial_number))
return -1;
}
if (unlikely(ec_slave_sii_read(slave, 0x001C,
&slave->sii_mailbox_protocols))) {
EC_ERR("Could not read SII supported mailbox protocols!\n");
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,84 @@ int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */)
/*****************************************************************************/
/**
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];
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_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];
@ -256,19 +309,19 @@ int ec_slave_sii_read(ec_slave_t *slave,
/*****************************************************************************/
/**
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 @@ int ec_slave_fetch_categories(ec_slave_t *slave /**< EtherCAT-Slave */)
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 @@ int ec_slave_fetch_categories(ec_slave_t *slave /**< EtherCAT-Slave */)
// 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 @@ void ec_slave_print(const ec_slave_t *slave /**< EtherCAT-Slave */)
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 @@ void ec_slave_print(const ec_slave_t *slave /**< EtherCAT-Slave */)
if (!first) printk(", ");
printk("VoE");
}
printk("\n");
}
printk("\n");
EC_INFO("| EEPROM data:\n");
@ -983,29 +1038,45 @@ int ec_slave_mailbox_send(ec_slave_t *slave, /**< EtherCAT-Slave */
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);
kfree(data);
return -1;
}
kfree(data);
return 0;
}
@ -1032,6 +1103,7 @@ int ec_slave_mailbox_receive(ec_slave_t *slave, /**< EtherCAT-Slave */
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 @@ int ec_slave_mailbox_receive(ec_slave_t *slave, /**< EtherCAT-Slave */
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 @@ int ec_slave_mailbox_receive(ec_slave_t *slave, /**< EtherCAT-Slave */
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;

11
master/slave.h
View File

@ -187,7 +187,11 @@ struct ec_slave
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 @@ void ec_slave_clear(ec_slave_t *);
// 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 *);

43
rt/msr_module.c
View File

@ -38,6 +38,7 @@
#include "../include/ecrt.h"
#define ASYNC
//#define BLOCK1
// Defines/Makros
#define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ)
@ -52,30 +53,33 @@ static struct ipipe_sysinfo sys_info;
// 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;
#endif
uint32_t k_angle;
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,29 +102,34 @@ static void msr_controller_run(void)
// 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);
#else
k_angle = EC_READ_U16(r_inc);
#endif
#endif // ASYNC
k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz;
//ecrt_master_debug(master, 0);
@ -195,11 +204,6 @@ int __init init_rt_module(void)
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 @@ int __init init_rt_module(void)
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 @@ int __init init_rt_module(void)
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 @@ int __init init_rt_module(void)
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;