diff -r 6b3b8acb71b5 -r 7506e67dd122 drivers/ec_master.c --- a/drivers/ec_master.c Fri Jan 13 15:47:44 2006 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1126 +0,0 @@ -/****************************************************************************** - * - * e c _ m a s t e r . c - * - * Methoden für einen EtherCAT-Master. - * - * $Id$ - * - *****************************************************************************/ - -#include -#include -#include -#include -#include - -#include "ec_globals.h" -#include "ec_master.h" - -/*****************************************************************************/ - -/** - Konstruktor des EtherCAT-Masters. - - @param master Zeiger auf den zu initialisierenden EtherCAT-Master -*/ - -void EtherCAT_master_init(EtherCAT_master_t *master) -{ - master->bus_slaves = NULL; - master->bus_slaves_count = 0; - master->dev = NULL; - master->command_index = 0x00; - master->tx_data_length = 0; - master->rx_data_length = 0; - master->domain_count = 0; - master->debug_level = 0; - master->bus_time = 0; - master->frames_lost = 0; - master->t_lost_output = 0; -} - -/*****************************************************************************/ - -/** - Destruktor eines EtherCAT-Masters. - - Entfernt alle Kommandos aus der Liste, löscht den Zeiger - auf das Slave-Array und gibt die Prozessdaten frei. - - @param master Zeiger auf den zu löschenden Master -*/ - -void EtherCAT_master_clear(EtherCAT_master_t *master) -{ - if (master->bus_slaves) { - kfree(master->bus_slaves); - master->bus_slaves = NULL; - } - - master->domain_count = 0; -} - -/*****************************************************************************/ - -/** - Öffnet ein EtherCAT-Geraet für den Master. - - Registriert das Geraet beim Master, der es daraufhin oeffnet. - - @param master Der EtherCAT-Master - @param device Das EtherCAT-Geraet - @return 0, wenn alles o.k., - < 0, wenn bereits ein Geraet registriert - oder das Geraet nicht geoeffnet werden konnte. -*/ - -int EtherCAT_master_open(EtherCAT_master_t *master, - EtherCAT_device_t *device) -{ - if (!master || !device) { - printk(KERN_ERR "EtherCAT: Illegal parameters for master_open()!\n"); - return -1; - } - - if (master->dev) { - printk(KERN_ERR "EtherCAT: Master already has a device.\n"); - return -1; - } - - if (EtherCAT_device_open(device) < 0) { - printk(KERN_ERR "EtherCAT: Could not open device %X!\n", - (unsigned int) master->dev); - return -1; - } - - master->dev = device; - - return 0; -} - -/*****************************************************************************/ - -/** - Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet. - - @param master Der EtherCAT-Master - @param device Das EtherCAT-Geraet -*/ - -void EtherCAT_master_close(EtherCAT_master_t *master, - EtherCAT_device_t *device) -{ - if (master->dev != device) { - printk(KERN_WARNING "EtherCAT: Warning -" - " Trying to close an unknown device!\n"); - return; - } - - if (EtherCAT_device_close(master->dev) < 0) { - printk(KERN_WARNING "EtherCAT: Warning -" - " Could not close device!\n"); - } - - master->dev = NULL; -} - -/*****************************************************************************/ - -/** - Sendet ein einzelnes Kommando in einem Frame und - wartet auf dessen Empfang. - - @param master EtherCAT-Master - @param cmd Kommando zum Senden/Empfangen - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_simple_send_receive(EtherCAT_master_t *master, - EtherCAT_command_t *cmd) -{ - unsigned int tries_left; - - if (unlikely(EtherCAT_simple_send(master, cmd) < 0)) - return -1; - - udelay(3); - - EtherCAT_device_call_isr(master->dev); - - tries_left = 20; - while (unlikely(master->dev->state == ECAT_DS_SENT - && tries_left)) { - udelay(1); - EtherCAT_device_call_isr(master->dev); - tries_left--; - } - - if (unlikely(EtherCAT_simple_receive(master, cmd) < 0)) - return -1; - - return 0; -} - -/*****************************************************************************/ - -/** - Sendet ein einzelnes Kommando in einem Frame. - - @param master EtherCAT-Master - @param cmd Kommando zum Senden - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_simple_send(EtherCAT_master_t *master, - EtherCAT_command_t *cmd) -{ - unsigned int length, framelength, i; - - if (unlikely(master->debug_level > 0)) { - printk(KERN_DEBUG "EtherCAT_send_receive_command\n"); - } - - if (unlikely(cmd->state != ECAT_CS_READY)) { - printk(KERN_WARNING "EtherCAT_send_receive_command:" - "Command not in ready state!\n"); - } - - length = cmd->data_length + 12; - framelength = length + 2; - - if (unlikely(framelength > ECAT_FRAME_BUFFER_SIZE)) { - printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength); - return -1; - } - - if (framelength < 46) framelength = 46; - - if (unlikely(master->debug_level > 0)) { - printk(KERN_DEBUG "Frame length: %i\n", framelength); - } - - master->tx_data[0] = length & 0xFF; - master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; - - cmd->index = master->command_index; - master->command_index = (master->command_index + 1) % 0x0100; - - if (unlikely(master->debug_level > 0)) { - printk(KERN_DEBUG "Sending command index %i\n", cmd->index); - } - - cmd->state = ECAT_CS_SENT; - - master->tx_data[2 + 0] = cmd->type; - master->tx_data[2 + 1] = cmd->index; - master->tx_data[2 + 2] = cmd->address.raw[0]; - master->tx_data[2 + 3] = cmd->address.raw[1]; - master->tx_data[2 + 4] = cmd->address.raw[2]; - master->tx_data[2 + 5] = cmd->address.raw[3]; - master->tx_data[2 + 6] = cmd->data_length & 0xFF; - master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8; - master->tx_data[2 + 8] = 0x00; - master->tx_data[2 + 9] = 0x00; - - if (likely(cmd->type == ECAT_CMD_APWR - || cmd->type == ECAT_CMD_NPWR - || cmd->type == ECAT_CMD_BWR - || cmd->type == ECAT_CMD_LRW)) // Write commands - { - for (i = 0; i < cmd->data_length; i++) - master->tx_data[2 + 10 + i] = cmd->data[i]; - } - else // Read commands - { - for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00; - } - - master->tx_data[2 + 10 + cmd->data_length] = 0x00; - master->tx_data[2 + 11 + cmd->data_length] = 0x00; - - // Pad with zeros - for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00; - - master->tx_data_length = framelength; - - if (unlikely(master->debug_level > 0)) { - printk(KERN_DEBUG "device send...\n"); - } - - // Send frame - if (unlikely(EtherCAT_device_send(master->dev, - master->tx_data, - framelength) != 0)) { - printk(KERN_ERR "EtherCAT: Could not send!\n"); - return -1; - } - - if (unlikely(master->debug_level > 0)) { - printk(KERN_DEBUG "EtherCAT_send done.\n"); - } - - return 0; -} - -/*****************************************************************************/ - -/** - Wartet auf den Empfang eines einzeln gesendeten - Kommandos. - - @param master EtherCAT-Master - @param cmd Gesendetes Kommando - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_simple_receive(EtherCAT_master_t *master, - EtherCAT_command_t *cmd) -{ - unsigned int length; - int ret; - unsigned char command_type, command_index; - - if (unlikely((ret = EtherCAT_device_receive(master->dev, - master->rx_data)) < 0)) - return -1; - - master->rx_data_length = (unsigned int) ret; - - if (unlikely(master->rx_data_length < 2)) { - printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT" - " header!\n"); - output_debug_data(master); - return -1; - } - - // Länge des gesamten Frames prüfen - length = ((master->rx_data[1] & 0x07) << 8) - | (master->rx_data[0] & 0xFF); - - if (unlikely(length > master->rx_data_length)) { - printk(KERN_ERR "EtherCAT: Received corrupted frame (length does" - " not match)!\n"); - output_debug_data(master); - return -1; - } - - command_type = master->rx_data[2]; - command_index = master->rx_data[2 + 1]; - length = (master->rx_data[2 + 6] & 0xFF) - | ((master->rx_data[2 + 7] & 0x07) << 8); - - if (unlikely(master->rx_data_length - 2 < length + 12)) { - printk(KERN_ERR "EtherCAT: Received frame with" - " incomplete command data!\n"); - output_debug_data(master); - return -1; - } - - if (likely(cmd->state == ECAT_CS_SENT - && cmd->type == command_type - && cmd->index == command_index - && cmd->data_length == length)) - { - cmd->state = ECAT_CS_RECEIVED; - - // Empfangene Daten in Kommandodatenspeicher kopieren - memcpy(cmd->data, master->rx_data + 2 + 10, length); - - // Working-Counter setzen - cmd->working_counter - = ((master->rx_data[length + 2 + 10] & 0xFF) - | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); - - if (unlikely(master->debug_level > 1)) { - output_debug_data(master); - } - } - else - { - printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); - output_debug_data(master); - } - - master->dev->state = ECAT_DS_READY; - - return 0; -} - -/*****************************************************************************/ - -/** - Durchsucht den Bus nach Slaves. - - @param master Der EtherCAT-Master - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_scan_for_slaves(EtherCAT_master_t *master) -{ - EtherCAT_command_t cmd; - EtherCAT_slave_t *cur; - unsigned int i, j; - unsigned char data[2]; - - // Determine number of slaves on bus - - EtherCAT_command_broadcast_read(&cmd, 0x0000, 4); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - master->bus_slaves_count = cmd.working_counter; - printk("EtherCAT: Found %i slaves on bus.\n", master->bus_slaves_count); - - if (!master->bus_slaves_count) return 0; - - if (!(master->bus_slaves = - (EtherCAT_slave_t *) kmalloc(master->bus_slaves_count * - sizeof(EtherCAT_slave_t), GFP_KERNEL))) { - printk(KERN_ERR "EtherCAT: Could not allocate memory for bus slaves!\n"); - return -1; - } - - // For every slave in the list - for (i = 0; i < master->bus_slaves_count; i++) - { - cur = master->bus_slaves + i; - - EtherCAT_slave_init(cur); - - // Read base data - - EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Slave %i did not respond" - " while reading base data!\n", i); - return -1; - } - - // Get base data - - cur->type = cmd.data[0]; - cur->revision = cmd.data[1]; - cur->build = cmd.data[2] | (cmd.data[3] << 8); - - // Read identification from "Slave Information Interface" (SII) - - if (unlikely(EtherCAT_read_slave_information(master, - cur->station_address, - 0x0008, - &cur->vendor_id) != 0)) - { - printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n"); - return -1; - } - - if (unlikely(EtherCAT_read_slave_information(master, - cur->station_address, - 0x000A, - &cur->product_code) != 0)) - { - printk(KERN_ERR "EtherCAT: Could not read SII product code!\n"); - return -1; - } - - if (unlikely(EtherCAT_read_slave_information(master, - cur->station_address, - 0x000C, - &cur->revision_number) != 0)) - { - printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n"); - return -1; - } - - if (unlikely(EtherCAT_read_slave_information(master, - cur->station_address, - 0x000E, - &cur->serial_number) != 0)) - { - printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n"); - return -1; - } - - // Search for identification in "database" - - for (j = 0; j < slave_ident_count; j++) - { - if (unlikely(slave_idents[j].vendor_id == cur->vendor_id - && slave_idents[j].product_code == cur->product_code)) - { - cur->desc = slave_idents[j].desc; - break; - } - } - - if (unlikely(!cur->desc)) { - printk(KERN_ERR "EtherCAT: Unknown slave device (vendor %X, code %X) at " - " position %i.\n", cur->vendor_id, cur->product_code, i); - return -1; - } - - // Set ring position - cur->ring_position = -i; - cur->station_address = i + 1; - - // Write station address - - data[0] = cur->station_address & 0x00FF; - data[1] = (cur->station_address & 0xFF00) >> 8; - - EtherCAT_command_position_write(&cmd, cur->ring_position, - 0x0010, 2, data); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Slave %i did not repond" - " while writing station address!\n", i); - return -1; - } - } - - return 0; -} - -/*****************************************************************************/ - -/** - Registriert einen Slave beim Master. - - @param master Der EtherCAT-Master - - @return 0 bei Erfolg, sonst < 0 -*/ - -void *EtherCAT_register_slave(EtherCAT_master_t *master, - unsigned int bus_index, - const char *vendor_name, - const char *product_name, - unsigned int domain) -{ - EtherCAT_slave_t *slave; - EtherCAT_domain_t *dom; - unsigned int j; - - if (bus_index >= master->bus_slaves_count) { - printk(KERN_ERR "EtherCAT: Illegal bus index! (%i / %i)\n", bus_index, - master->bus_slaves_count); - return NULL; - } - - slave = master->bus_slaves + bus_index; - - if (slave->process_data) { - printk(KERN_ERR "EtherCAT: Slave %i is already registered!\n", bus_index); - return NULL; - } - - if (strcmp(vendor_name, slave->desc->vendor_name) || - strcmp(product_name, slave->desc->product_name)) { - printk(KERN_ERR "Invalid Slave Type! Requested: \"%s %s\", present: \"%s" - "%s\".\n", vendor_name, product_name, slave->desc->vendor_name, - slave->desc->product_name); - return NULL; - } - - // Check, if process data domain already exists... - dom = NULL; - for (j = 0; j < master->domain_count; j++) { - if (domain == master->domains[j].number) { - dom = master->domains + j; - } - } - - // Create process data domain - if (!dom) { - if (master->domain_count > ECAT_MAX_DOMAINS - 1) { - printk(KERN_ERR "EtherCAT: Too many domains!\n"); - return NULL; - } - - dom = master->domains + master->domain_count; - EtherCAT_domain_init(dom); - dom->number = domain; - dom->logical_offset = master->domain_count * ECAT_FRAME_BUFFER_SIZE; - master->domain_count++; - } - - if (dom->data_size + slave->desc->process_data_size - > ECAT_FRAME_BUFFER_SIZE - 14) { - printk(KERN_ERR "EtherCAT: Oversized domain %i: %i / %i Bytes!\n", - dom->number, dom->data_size + slave->desc->process_data_size, - ECAT_FRAME_BUFFER_SIZE - 14); - return NULL; - } - - slave->process_data = dom->data + dom->data_size; - dom->data_size += slave->desc->process_data_size; - - return slave->process_data; -} - -/*****************************************************************************/ - -/** - Liest Daten aus dem Slave-Information-Interface - eines EtherCAT-Slaves. - - @param master EtherCAT-Master - @param node_address Knotenadresse des Slaves - @param offset Adresse des zu lesenden SII-Registers - @param target Zeiger auf einen 4 Byte großen Speicher - zum Ablegen der Daten - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_read_slave_information(EtherCAT_master_t *master, - unsigned short int node_address, - unsigned short int offset, - unsigned int *target) -{ - EtherCAT_command_t cmd; - unsigned char data[10]; - unsigned int tries_left; - - // Initiate read operation - - data[0] = 0x00; - data[1] = 0x01; - data[2] = offset & 0xFF; - data[3] = (offset & 0xFF00) >> 8; - data[4] = 0x00; - data[5] = 0x00; - - EtherCAT_command_write(&cmd, node_address, 0x502, 6, data); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n", - node_address); - 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. - - tries_left = 100; - while (likely(tries_left)) - { - udelay(10); - - EtherCAT_command_read(&cmd, node_address, 0x502, 10); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: SII-read status -" - " Slave %04X did not respond!\n", node_address); - return -1; - } - - if (likely((cmd.data[1] & 0x81) == 0)) { - memcpy(target, cmd.data + 6, 4); - break; - } - - tries_left--; - } - - if (unlikely(!tries_left)) { - printk(KERN_WARNING "EtherCAT: SSI-read. Slave %04X timed out!\n", - node_address); - return -1; - } - - return 0; -} - -/*****************************************************************************/ - -/** - Ändert den Zustand eines Slaves (asynchron). - - Führt eine (asynchrone) Zustandsänderung bei einem Slave durch. - - @param master EtherCAT-Master - @param slave Slave, dessen Zustand geändert werden soll - @param state_and_ack Neuer Zustand, evtl. mit gesetztem - Acknowledge-Flag - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_state_change(EtherCAT_master_t *master, - EtherCAT_slave_t *slave, - unsigned char state_and_ack) -{ - EtherCAT_command_t cmd; - unsigned char data[2]; - unsigned int tries_left; - - data[0] = state_and_ack; - data[1] = 0x00; - - EtherCAT_command_write(&cmd, slave->station_address, - 0x0120, 2, data); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) { - printk(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", - state_and_ack); - return -1; - } - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Could not set state %02X - Device \"%s %s\"" - " (%d) did not respond!\n", state_and_ack, slave->desc->vendor_name, - slave->desc->product_name, slave->ring_position * (-1)); - return -1; - } - - slave->requested_state = state_and_ack & 0x0F; - - tries_left = 100; - while (likely(tries_left)) - { - udelay(10); - - EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) { - printk(KERN_ERR "EtherCAT: Could not check state %02X - Unable to" - " send!\n", state_and_ack); - return -1; - } - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Could not check state %02X - Device did not" - " respond!\n", state_and_ack); - return -1; - } - - if (unlikely(cmd.data[0] & 0x10)) { // State change error - printk(KERN_ERR "EtherCAT: Could not set state %02X - Device refused" - " state change (code %02X)!\n", state_and_ack, cmd.data[0]); - return -1; - } - - if (likely(cmd.data[0] == (state_and_ack & 0x0F))) { - // State change successful - break; - } - - tries_left--; - } - - if (unlikely(!tries_left)) { - printk(KERN_ERR "EtherCAT: Could not check state %02X - Timeout while" - " checking!\n", state_and_ack); - return -1; - } - - slave->current_state = state_and_ack & 0x0F; - - return 0; -} - -/*****************************************************************************/ - -/** - Konfiguriert einen Slave und setzt den Operational-Zustand. - - Führt eine komplette Konfiguration eines Slaves durch, - setzt Sync-Manager und FMMU's, führt die entsprechenden - Zustandsübergänge durch, bis der Slave betriebsbereit ist. - - @param master EtherCAT-Master - @param slave Zu aktivierender Slave - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_activate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - EtherCAT_command_t cmd; - const EtherCAT_slave_desc_t *desc; - unsigned char fmmu[16]; - unsigned char data[256]; - - desc = slave->desc; - - if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0)) - return -1; - - // Resetting FMMU's - - memset(data, 0x00, 256); - - EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 256, data); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - - // Resetting Sync Manager channels - - if (desc->type != ECAT_ST_SIMPLE_NOSYNC) - { - memset(data, 0x00, 256); - - EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - } - - // Init Mailbox communication - - if (desc->type == ECAT_ST_MAILBOX) - { - if (desc->sm0) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, - desc->sm0); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - } - - if (desc->sm1) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, - desc->sm1); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM1 -" - " Slave %04X did not respond!\n", - slave->station_address); - return -1; - } - } - } - - // Change state to PREOP - - if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_PREOP) != 0)) - return -1; - - // Set FMMU's - - if (desc->fmmu0) - { - if (unlikely(!slave->process_data)) { - printk(KERN_ERR "EtherCAT: Warning - Slave %04X is not assigned to any" - " process data object!\n", slave->station_address); - return -1; - } - - memcpy(fmmu, desc->fmmu0, 16); - - fmmu[0] = slave->logical_address & 0x000000FF; - fmmu[1] = (slave->logical_address & 0x0000FF00) >> 8; - fmmu[2] = (slave->logical_address & 0x00FF0000) >> 16; - fmmu[3] = (slave->logical_address & 0xFF000000) >> 24; - - EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - } - - // Set Sync Managers - - if (desc->type != ECAT_ST_MAILBOX) - { - if (desc->sm0) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, - desc->sm0); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - } - - if (desc->sm1) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, - desc->sm1); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not" - " respond!\n", slave->station_address); - return -1; - } - } - } - - if (desc->sm2) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n", - slave->station_address); - return -1; - } - } - - if (desc->sm3) - { - EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3); - - if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0)) - return -1; - - if (unlikely(cmd.working_counter != 1)) { - printk(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n", - slave->station_address); - return -1; - } - } - - // Change state to SAVEOP - if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0)) - return -1; - - // Change state to OP - if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_OP) != 0)) - return -1; - - return 0; -} - -/*****************************************************************************/ - -/** - Setzt einen Slave zurück in den Init-Zustand. - - @param master EtherCAT-Master - @param slave Zu deaktivierender Slave - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_deactivate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - if (unlikely(EtherCAT_state_change(master, slave, - ECAT_STATE_INIT) != 0)) - return -1; - - return 0; -} - -/*****************************************************************************/ - -/** - Sendet und empfängt Prozessdaten der angegebenen Domäne - - @param master EtherCAT-Master - domain Domäne - timeout_us Timeout in Mikrosekunden - - @return 0 bei Erfolg, sonst < 0 -*/ - -int EtherCAT_process_data_cycle(EtherCAT_master_t *master, unsigned int domain, - unsigned int timeout_us) -{ - unsigned int i; - EtherCAT_domain_t *dom; - unsigned long start_ticks, end_ticks, timeout_ticks; - - ecat_output_lost_frames(master); // Evtl. verlorene Frames ausgeben - - // Domäne bestimmen - dom = NULL; - for (i = 0; i < master->domain_count; i++) { - if (master->domains[i].number == domain) { - dom = master->domains + i; - break; - } - } - - if (unlikely(!dom)) { - printk(KERN_ERR "EtherCAT: No such domain: %i!\n", domain); - return -1; - } - - EtherCAT_command_logical_read_write(&dom->command, - dom->logical_offset, dom->data_size, - dom->data); - - rdtscl(start_ticks); // Sendezeit nehmen - - if (unlikely(EtherCAT_simple_send(master, &dom->command) < 0)) { - printk(KERN_ERR "EtherCAT: Could not send process data command!\n"); - return -1; - } - - timeout_ticks = timeout_us * cpu_khz / 1000; - - // Warten - do { - EtherCAT_device_call_isr(master->dev); - rdtscl(end_ticks); // Empfangszeit nehmen - } - while (unlikely(master->dev->state == ECAT_DS_SENT - && end_ticks - start_ticks < timeout_ticks)); - - master->bus_time = (end_ticks - start_ticks) * 1000 / cpu_khz; - - if (unlikely(end_ticks - start_ticks >= timeout_ticks)) { - master->dev->state = ECAT_DS_READY; - master->frames_lost++; - ecat_output_lost_frames(master); - return -1; - } - - if (unlikely(EtherCAT_simple_receive(master, &dom->command) < 0)) { - printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); - return -1; - } - - if (unlikely(dom->command.state != ECAT_CS_RECEIVED)) { - printk(KERN_WARNING "EtherCAT: Process data command not received!\n"); - return -1; - } - - if (dom->command.working_counter != dom->response_count) { - dom->response_count = dom->command.working_counter; - printk(KERN_INFO "EtherCAT: Domain %i State change - %i slaves" - " responding.\n", dom->number, dom->response_count); - } - - // Daten vom Kommando in den Prozessdatenspeicher kopieren - memcpy(dom->data, dom->command.data, dom->data_size); - - return 0; -} - -/*****************************************************************************/ - -/** - Gibt Frame-Inhalte zwecks Debugging aus. - - @param master EtherCAT-Master -*/ - -void output_debug_data(const EtherCAT_master_t *master) -{ - unsigned int i; - - printk(KERN_DEBUG "EtherCAT: tx_data content (%i Bytes):\n", - master->tx_data_length); - - printk(KERN_DEBUG); - for (i = 0; i < master->tx_data_length; i++) - { - printk("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG); - } - printk("\n"); - - printk(KERN_DEBUG "EtherCAT: rx_data content (%i Bytes):\n", - master->rx_data_length); - - printk(KERN_DEBUG); - for (i = 0; i < master->rx_data_length; i++) - { - printk("%02X ", master->rx_data[i]); - if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG); - } - printk("\n"); -} - -/*****************************************************************************/ - -/** - Gibt von Zeit zu Zeit die Anzahl verlorener Frames aus. - - @param master EtherCAT-Master -*/ - -void ecat_output_lost_frames(EtherCAT_master_t *master) -{ - unsigned long int t; - - if (master->frames_lost) { - rdtscl(t); - if ((t - master->t_lost_output) / cpu_khz > 1000) { - printk(KERN_ERR "EtherCAT: %u frame(s) LOST!\n", master->frames_lost); - master->frames_lost = 0; - master->t_lost_output = t; - } - } -} - -/*****************************************************************************/ - -EXPORT_SYMBOL(EtherCAT_master_open); -EXPORT_SYMBOL(EtherCAT_master_close); -EXPORT_SYMBOL(EtherCAT_activate_slave); -EXPORT_SYMBOL(EtherCAT_deactivate_slave); -EXPORT_SYMBOL(EtherCAT_process_data_cycle); - -/*****************************************************************************/ - -/* Emacs-Konfiguration -;;; Local Variables: *** -;;; c-basic-offset:2 *** -;;; End: *** -*/