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