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@0:    @param master Zeiger auf den zu initialisierenden
fp@39:    EtherCAT-Master
fp@0: 
fp@0:    @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)
fp@0: */
fp@0: 
fp@27: void EtherCAT_master_init(EtherCAT_master_t *master)
fp@27: {
fp@0:   master->slaves = NULL;
fp@0:   master->slave_count = 0;
fp@27:   master->dev = NULL;
fp@0:   master->command_index = 0x00;
fp@0:   master->tx_data_length = 0;
fp@0:   master->process_data = NULL;
fp@0:   master->process_data_length = 0;
fp@2:   master->debug_level = 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@0:   // Remove all slaves
fp@0:   EtherCAT_clear_slaves(master);
fp@0: 
fp@39:   if (master->process_data) {
fp@0:     kfree(master->process_data);
fp@0:     master->process_data = NULL;
fp@0:   }
fp@0: 
fp@0:   master->process_data_length = 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@39:     printk(KERN_ERR "EtherCAT: Illegal parameters"
fp@39:            " for master_open()!\n");
fp@39:     return -1;
fp@39:   }
fp@39: 
fp@39:   if (master->dev) {
fp@39:     printk(KERN_ERR "EtherCAT: Master already"
fp@39:            " 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@39:     printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n",
fp@39:            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@39:     printk(KERN_ERR "EtherCAT: Received frame with"
fp@39:            " incomplete EtherCAT 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@39:     printk(KERN_ERR "EtherCAT: Received corrupted"
fp@39:            " frame (length does 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@11:     cmd->working_counter = ((master->rx_data[length + 2 + 10] & 0xFF)
fp@11:                             | ((master->rx_data[length + 2 + 11] & 0xFF) << 8));
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@17:    Überprüft die angeschlossenen Slaves.
fp@17: 
fp@17:    Vergleicht die an den Bus angeschlossenen Slaves mit
fp@17:    den im statischen-Slave-Array vorgegebenen Konfigurationen.
fp@17:    Stimmen Anzahl oder Typen nicht überein, gibt diese
fp@17:    Methode einen Fehler aus.
fp@17: 
fp@17:    @param master Der EtherCAT-Master
fp@17:    @param slaves Zeiger auf ein statisches Slave-Array
fp@17:    @param slave_count Anzahl der Slaves im Array
fp@17: 
fp@17:    @return 0 bei Erfolg, sonst < 0
fp@17: */
fp@17: 
fp@17: int EtherCAT_check_slaves(EtherCAT_master_t *master,
fp@17:                           EtherCAT_slave_t *slaves,
fp@17:                           unsigned int slave_count)
fp@17: {
fp@17:   EtherCAT_command_t cmd;
fp@17:   EtherCAT_slave_t *cur;
fp@17:   unsigned int i, j, found, length, pos;
fp@17:   unsigned char data[2];
fp@17: 
fp@39:   if (unlikely(!slave_count)) {
fp@27:     printk(KERN_ERR "EtherCAT: No slaves in list!\n");
fp@17:     return -1;
fp@17:   }
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@39:   if (unlikely(cmd.working_counter != slave_count)) {
fp@26:     printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",
fp@17:            cmd.working_counter, slave_count);
fp@17:     return -1;
fp@17:   }
fp@17:   else
fp@26:     printk("EtherCAT: Found all %i slaves.\n", slave_count);
fp@17: 
fp@17:   // For every slave in the list
fp@17:   for (i = 0; i < slave_count; i++)
fp@17:   {
fp@17:     cur = &slaves[i];
fp@17: 
fp@39:     if (unlikely(!cur->desc)) {
fp@39:       printk(KERN_ERR "EtherCAT: Slave %i has"
fp@39:              " no description.\n", i);
fp@17:       return -1;
fp@17:     }
fp@17: 
fp@17:     // Set ring position
fp@17:     cur->ring_position = -i;
fp@17:     cur->station_address = i + 1;
fp@17: 
fp@17:     // Write station address
fp@17: 
fp@17:     data[0] = cur->station_address & 0x00FF;
fp@17:     data[1] = (cur->station_address & 0xFF00) >> 8;
fp@17: 
fp@39:     EtherCAT_command_position_write(&cmd, cur->ring_position,
fp@39:                                     0x0010, 2, 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@39:       printk(KERN_ERR "EtherCAT: Slave %i did not repond"
fp@39:              " while writing station address!\n", i);
fp@17:       return -1;
fp@17:     }
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@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@17:     found = 0;
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@17:         found = 1;
fp@17: 
fp@39:         if (unlikely(cur->desc != slave_idents[j].desc)) {
fp@39:           printk(KERN_ERR "EtherCAT: Unexpected slave device"
fp@39:                  " \"%s %s\" at position %i. Expected: \"%s %s\"\n",
fp@17:                  slave_idents[j].desc->vendor_name,
fp@17:                  slave_idents[j].desc->product_name, i,
fp@17:                  cur->desc->vendor_name, cur->desc->product_name);
fp@17:           return -1;
fp@17:         }
fp@17: 
fp@17:         break;
fp@2:       }
fp@17:     }
fp@17: 
fp@39:     if (unlikely(!found)) {
fp@26:       printk(KERN_ERR "EtherCAT: Unknown slave device"
fp@17:              " (vendor %X, code %X) at position %i.\n",
fp@35:              cur->vendor_id, cur->product_code, i);
fp@2:       return -1;
fp@2:     }
fp@2:   }
fp@0: 
fp@17:   length = 0;
fp@17:   for (i = 0; i < slave_count; i++)
fp@17:   {
fp@17:     length += slaves[i].desc->data_length;
fp@17:   }
fp@17: 
fp@39:   if (unlikely((master->process_data = (unsigned char *)
fp@39:                 kmalloc(sizeof(unsigned char)
fp@39:                         * length, GFP_KERNEL)) == NULL)) {
fp@39:     printk(KERN_ERR "EtherCAT: Could not allocate %i"
fp@39:            " bytes for process data.\n", length);
fp@17:     return -1;
fp@17:   }
fp@17: 
fp@17:   master->process_data_length = length;
fp@17:   memset(master->process_data, 0x00, length);
fp@17: 
fp@17:   pos = 0;
fp@17:   for (i = 0; i < slave_count; i++)
fp@17:   {
fp@17:     slaves[i].process_data = master->process_data + pos;
fp@17:     slaves[i].logical_address0 = pos;
fp@17: 
fp@39:     printk(KERN_DEBUG "EtherCAT: Slave %i -"
fp@39:            " Address 0x%X, \"%s %s\", s/n %u\n",
fp@39:            i, pos, slaves[i].desc->vendor_name,
fp@39:            slaves[i].desc->product_name,
fp@17:            slaves[i].serial_number);
fp@17: 
fp@17:     pos += slaves[i].desc->data_length;
fp@17:   }
fp@17: 
fp@17:   master->slaves = slaves;
fp@17:   master->slave_count = slave_count;
fp@17: 
fp@17:   return 0;
fp@17: }
fp@17: 
fp@39: /*****************************************************************************/
fp@17: 
fp@17: /**
fp@17:    Entfernt den Zeiger auf das Slave-Array.
fp@17: 
fp@17:    @param master EtherCAT-Master
fp@17: */
fp@17: 
fp@17: void EtherCAT_clear_slaves(EtherCAT_master_t *master)
fp@17: {
fp@17:   master->slaves = NULL;
fp@17:   master->slave_count = 0;
fp@17: }
fp@17: 
fp@39: /*****************************************************************************/
fp@17: 
fp@17: /**
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@39:     return -3;
fp@39: 
fp@39:   if (unlikely(cmd.working_counter != 1)) {
fp@39:     printk(KERN_ERR "EtherCAT: SII-read - Slave"
fp@39:            " %04X did not respond!\n", node_address);
fp@17:     return -4;
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@39:       return -3;
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@17:       return -4;
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@39:     printk(KERN_WARNING "EtherCAT: SSI-read."
fp@39:            " Slave %04X timed out!\n", 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@39:     printk(KERN_ERR "EtherCAT: Could not set state %02X -"
fp@39:            " Unable to send!\n", state_and_ack);
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@39:   if (unlikely(cmd.working_counter != 1)) {
fp@39:     printk(KERN_ERR "EtherCAT: Could not set state %02X -"
fp@39:            " Device \"%s %s\" (%d) did not respond!\n",
fp@39:            state_and_ack, slave->desc->vendor_name,
fp@39:            slave->desc->product_name, slave->ring_position*(-1));
fp@0:     return -3;
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@39:       printk(KERN_ERR "EtherCAT: Could not check"
fp@39:              " state %02X - Unable to send!\n", state_and_ack);
fp@0:       return -2;
fp@0:     }
fp@8: 
fp@39:     if (unlikely(cmd.working_counter != 1)) {
fp@39:       printk(KERN_ERR "EtherCAT: Could not check"
fp@39:              " state %02X - Device did not respond!\n",
fp@39:              state_and_ack);
fp@0:       return -3;
fp@0:     }
fp@0: 
fp@39:     if (unlikely(cmd.data[0] & 0x10)) { // State change error
fp@39:       printk(KERN_ERR "EtherCAT: Could not set state %02X -"
fp@39:              " Device refused state change (code %02X)!\n",
fp@39:              state_and_ack, cmd.data[0]);
fp@0:       return -4;
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@39:     printk(KERN_ERR "EtherCAT: Could not check state %02X -"
fp@39:            " Timeout while checking!\n", state_and_ack);
fp@0:     return -5;
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@39:   if (unlikely(EtherCAT_state_change(master, slave,
fp@39:                                      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@39:   EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                          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@39:     printk(KERN_ERR "EtherCAT: Resetting FMMUs -"
fp@39:            " Slave %04X did not respond!\n",
fp@0:            slave->station_address);
fp@0:     return -2;
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@39:     EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                            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@39:       printk(KERN_ERR "EtherCAT: Resetting SMs -"
fp@39:              " Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -2;
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@39:       EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                              0x0800, 8, 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@39:         printk(KERN_ERR "EtherCAT: Setting SM0 -"
fp@39:                " Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -3;
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@39:       EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                              0x0808, 8, 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@0:         return -2;
fp@0:       }
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   // Change state to PREOP
fp@0: 
fp@39:   if (unlikely(EtherCAT_state_change(master, slave,
fp@39:                                      ECAT_STATE_PREOP) != 0))
fp@0:     return -5;
fp@0: 
fp@0:   // Set FMMU's
fp@0: 
fp@0:   if (desc->fmmu0)
fp@0:   {
fp@0:     memcpy(fmmu, desc->fmmu0, 16);
fp@0: 
fp@0:     fmmu[0] = slave->logical_address0 & 0x000000FF;
fp@8:     fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;
fp@0:     fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
fp@0:     fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;
fp@0: 
fp@39:     EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                            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@39:       printk(KERN_ERR "EtherCAT: Setting FMMU0 -"
fp@39:              " Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -2;
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@39:       EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                              0x0800, 8, 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@39:         printk(KERN_ERR "EtherCAT: Setting SM0 -"
fp@39:                " Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -3;
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@39:       EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                              0x0808, 8, 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@0:         return -3;
fp@0:       }
fp@8:     }
fp@8:   }
fp@8: 
fp@0:   if (desc->sm2)
fp@0:   {
fp@39:     EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                            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@39:       printk(KERN_ERR "EtherCAT: Setting SM2 -"
fp@39:              " Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -3;
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   if (desc->sm3)
fp@0:   {
fp@39:     EtherCAT_command_write(&cmd, slave->station_address,
fp@39:                            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@39:       printk(KERN_ERR "EtherCAT: Setting SM3 -"
fp@39:              " Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -3;
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   // Change state to SAVEOP
fp@39:   if (unlikely(EtherCAT_state_change(master, slave,
fp@39:                                      ECAT_STATE_SAVEOP) != 0))
fp@0:     return -12;
fp@0: 
fp@0:   // Change state to OP
fp@39:   if (unlikely(EtherCAT_state_change(master, slave,
fp@39:                                      ECAT_STATE_OP) != 0))
fp@0:     return -13;
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@0:    Aktiviert alle Slaves.
fp@0: 
fp@0:    @see EtherCAT_activate_slave
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@8: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_activate_all_slaves(EtherCAT_master_t *master)
fp@0: {
fp@0:   unsigned int i;
fp@0: 
fp@0:   for (i = 0; i < master->slave_count; i++)
fp@0:   {
fp@39:     if (unlikely(EtherCAT_activate_slave(master,
fp@39:                                          &master->slaves[i]) < 0))
fp@39:       return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@39: /*****************************************************************************/
fp@0: 
fp@0: /**
fp@0:    Deaktiviert alle Slaves.
fp@0: 
fp@0:    @see EtherCAT_deactivate_slave
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@8: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_deactivate_all_slaves(EtherCAT_master_t *master)
fp@0: {
fp@0:   unsigned int i;
fp@0:   int ret = 0;
fp@0: 
fp@0:   for (i = 0; i < master->slave_count; i++)
fp@0:   {
fp@39:     if (unlikely(EtherCAT_deactivate_slave(master,
fp@39:                                            &master->slaves[i]) < 0))
fp@0:       ret = -1;
fp@0:   }
fp@0: 
fp@0:   return ret;
fp@0: }
fp@0: 
fp@39: /*****************************************************************************/
fp@0: 
fp@0: /**
fp@0:    Sendet alle Prozessdaten an die Slaves.
fp@0: 
fp@0:    Erstellt ein "logical read write"-Kommando mit den
fp@0:    Prozessdaten des Masters und sendet es an den Bus.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@8: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_write_process_data(EtherCAT_master_t *master)
fp@0: {
fp@13:   EtherCAT_command_logical_read_write(&master->process_data_command,
fp@13:                                       0, master->process_data_length,
fp@13:                                       master->process_data);
fp@13: 
fp@39:   if (unlikely(EtherCAT_simple_send(master,
fp@39:                                     &master->process_data_command) < 0))
fp@39:   {
fp@39:     printk(KERN_ERR "EtherCAT: Could not send"
fp@39:            " process data command!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@39: /*****************************************************************************/
fp@0: 
fp@0: /**
fp@0:    Empfängt alle Prozessdaten von den Slaves.
fp@0: 
fp@0:    Empfängt ein zuvor gesendetes "logical read write"-Kommando
fp@0:    und kopiert die empfangenen daten in den Prozessdatenspeicher
fp@0:    des Masters.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@8: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_read_process_data(EtherCAT_master_t *master)
fp@0: {
fp@19:   unsigned int tries_left;
fp@19: 
fp@13:   EtherCAT_device_call_isr(master->dev);
fp@13: 
hm@29:   tries_left = 20;
fp@39:   while (unlikely(master->dev->state == ECAT_DS_SENT
fp@39:                   && tries_left))
fp@19:   {
fp@19:     udelay(1);
fp@19:     EtherCAT_device_call_isr(master->dev);
fp@19:     tries_left--;
fp@19:   }
fp@19: 
fp@39:   if (unlikely(!tries_left))
fp@19:   {
fp@26:     printk(KERN_ERR "EtherCAT: Timeout while receiving process data!\n");
fp@19:     return -1;
fp@19:   }
fp@19: 
fp@39:   if (unlikely(EtherCAT_simple_receive(master,
fp@39:                                        &master->process_data_command) < 0))
fp@13:   {
fp@26:     printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@39:   if (unlikely(master->process_data_command.state != ECAT_CS_RECEIVED))
fp@13:   {
fp@26:     printk(KERN_WARNING "EtherCAT: Process data command not received!\n");
fp@13:     return -1;
fp@13:   }
fp@13: 
fp@13:   // Daten von Kommando in Prozessdaten des Master kopieren
fp@13:   memcpy(master->process_data, master->process_data_command.data,
fp@13:          master->process_data_length);
fp@13: 
fp@13:   return 0;
fp@13: }
fp@13: 
fp@39: /*****************************************************************************/
fp@13: 
fp@13: /**
fp@13:    Verwirft das zuletzt gesendete Prozessdatenkommando.
fp@13: 
fp@13:    @param master EtherCAT-Master
fp@13: */
fp@13: 
fp@13: void EtherCAT_clear_process_data(EtherCAT_master_t *master)
fp@13: {
hm@6:   EtherCAT_device_call_isr(master->dev);
fp@13:   master->dev->state = ECAT_DS_READY;
fp@2: }
fp@2: 
fp@39: /*****************************************************************************/
fp@2: 
fp@2: /**
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@24: EXPORT_SYMBOL(EtherCAT_master_init);
fp@24: EXPORT_SYMBOL(EtherCAT_master_clear);
fp@33: EXPORT_SYMBOL(EtherCAT_master_open);
fp@33: EXPORT_SYMBOL(EtherCAT_master_close);
fp@24: EXPORT_SYMBOL(EtherCAT_read_process_data);
fp@24: EXPORT_SYMBOL(EtherCAT_write_process_data);
fp@24: EXPORT_SYMBOL(EtherCAT_check_slaves);
fp@24: EXPORT_SYMBOL(EtherCAT_activate_all_slaves);
fp@24: EXPORT_SYMBOL(EtherCAT_clear_process_data);
fp@24: EXPORT_SYMBOL(EtherCAT_deactivate_all_slaves);
fp@27: 
fp@39: /*****************************************************************************/