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@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@42:   master->tx_time = 0;
fp@42:   master->rx_time = 0;
fp@42:   master->rx_tries = 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@42:   unsigned int i;
fp@42: 
fp@42:   // Remove domains
fp@42:   for (i = 0; i < master->domain_count; i++) {
fp@42:     EtherCAT_domain_clear(master->domains + i);
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@42:   // Zeit nehmen
fp@42:   rdtscl(master->tx_time);
fp@42: 
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@42:     printk(KERN_ERR "EtherCAT: Received frame with incomplete"
fp@42:            " 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@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@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@42:   unsigned int i, j, found, size, offset;
fp@17:   unsigned char data[2];
fp@42:   EtherCAT_domain_t *dom;
fp@42: 
fp@42:   // Clear domains
fp@42:   for (i = 0; i < master->domain_count; i++) {
fp@42:     printk(KERN_DEBUG "EtherCAT: Clearing domain %i!\n",
fp@42:            master->domains[i].number);
fp@42:     EtherCAT_domain_clear(master->domains + i);
fp@42:   }
fp@42:   master->domain_count = 0;
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@42: 
fp@42:   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@42:       printk(KERN_ERR "EtherCAT: Slave %i has 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@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@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@42: 
fp@42:     // Check, if process data domain already exists...
fp@42:     found = 0;
fp@42:     for (j = 0; j < master->domain_count; j++) {
fp@42:       if (cur->domain == master->domains[j].number) {
fp@42:         found = 1;
fp@42:       }
fp@42:     }
fp@42: 
fp@42:     // Create process data domain
fp@42:     if (!found) {
fp@42:       if (master->domain_count + 1 >= ECAT_MAX_DOMAINS) {
fp@42:         printk(KERN_ERR "EtherCAT: Too many domains!\n");
fp@42:         return -1;
fp@42:       }
fp@42: 
fp@42:       EtherCAT_domain_init(&master->domains[master->domain_count]);
fp@42:       master->domains[master->domain_count].number = cur->domain;
fp@42:       master->domain_count++;
fp@42:     }
fp@42:   }
fp@42: 
fp@42:   // Calculate domain sizes
fp@42:   offset = 0;
fp@42:   for (i = 0; i < master->domain_count; i++)
fp@42:   {
fp@42:     dom = master->domains + i;
fp@42: 
fp@42:     dom->logical_offset = offset;
fp@42: 
fp@42:     // For every slave in the list
fp@42:     size = 0;
fp@42:     for (j = 0; j < slave_count; j++) {
fp@42:       if (slaves[j].domain == dom->number) {
fp@42:         size += slaves[j].desc->process_data_size;
fp@42:       }
fp@42:     }
fp@42: 
fp@42:     if (size > ECAT_FRAME_BUFFER_SIZE - 14) {
fp@42:       printk(KERN_ERR "EtherCAT: Oversized domain %i: %i / %i Bytes!\n",
fp@42:              dom->number, size, ECAT_FRAME_BUFFER_SIZE - 14);
fp@42:       return -1;
fp@42:     }
fp@42: 
fp@42:     if (!(dom->data = (unsigned char *) kmalloc(sizeof(unsigned char)
fp@42:                                                * size, GFP_KERNEL))) {
fp@42:       printk(KERN_ERR "EtherCAT: Could not allocate"
fp@42:              " %i bytes of domain data.\n", size);
fp@42:       return -1;
fp@42:     }
fp@42: 
fp@42:     dom->data_size = size;
fp@42:     memset(dom->data, 0x00, size);
fp@42: 
fp@42:     printk(KERN_INFO "EtherCAT: Domain %i: %i Bytes of process data.\n",
fp@42:            dom->number, size);
fp@42: 
fp@42:     // Set logical addresses and data pointers of domain slaves
fp@42:     size = 0;
fp@42:     for (j = 0; j < slave_count; j++) {
fp@42:       if (slaves[j].domain == dom->number) {
fp@42:         slaves[j].process_data = dom->data + size;
fp@42:         slaves[j].logical_address = dom->logical_offset + size;
fp@42:         size += slaves[j].desc->process_data_size;
fp@42:       }
fp@42:     }
fp@42: 
fp@42:     offset += size;
fp@42:   }
fp@17: 
fp@17:   return 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@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@0:    @param master EtherCAT-Master
fp@42:           domain Domäne
fp@8: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@42: int EtherCAT_process_data_cycle(EtherCAT_master_t *master,
fp@42:                                 unsigned int domain)
fp@42: {
fp@42:   unsigned int i, tries;
fp@42:   EtherCAT_domain_t *dom = NULL;
fp@42: 
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@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@42:   udelay(3);
fp@42: 
fp@42: #if 1
fp@42:   // Warten
fp@42:   tries = 0;
fp@13:   EtherCAT_device_call_isr(master->dev);
fp@42:   while (unlikely(master->dev->state == ECAT_DS_SENT && tries < 100)) {
fp@19:     udelay(1);
fp@19:     EtherCAT_device_call_isr(master->dev);
fp@42:     tries++;
fp@42:   }
fp@42: 
fp@42:   rdtscl(master->rx_time);
fp@42:   master->rx_tries = tries;
fp@42: 
fp@42:   if (unlikely(tries == 100)) {
fp@26:     printk(KERN_ERR "EtherCAT: Timeout while receiving process data!\n");
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@42:   // Daten vom Kommando in den Prozessdatenspeicher kopieren
fp@42:   memcpy(dom->data, dom->command.data, dom->data_size);
fp@42: #endif
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@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_check_slaves);
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: */