fp@0: /****************************************************************
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@0:  *  $Date$
fp@0:  *  $Author$
fp@0:  *
fp@0:  ***************************************************************/
fp@0: 
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: #include "ec_dbg.h"
fp@0: 
fp@0: // FIXME: Klappt nur solange, wie es nur einen Master gibt! fp
fp@0: static int ASYNC = 0;
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Konstruktor des EtherCAT-Masters.
fp@0: 
fp@0:    @param master Zeiger auf den zu initialisierenden
fp@0:    EtherCAT-Master
fp@0:    @param dev Zeiger auf das EtherCAT-gerät, mit dem der
fp@0:    Master arbeiten soll
fp@0: 
fp@0:    @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)
fp@0: */
fp@0: 
fp@0: int EtherCAT_master_init(EtherCAT_master_t *master,
fp@0:                          EtherCAT_device_t *dev)
fp@0: {
fp@0:   unsigned int i;
fp@0: 
fp@0:   if (!dev)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Master init without device!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   master->slaves = NULL;
fp@0:   master->slave_count = 0;
fp@0:   master->first_command = NULL;
fp@0:   master->process_data_command = NULL;
fp@0:   master->dev = dev;
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@0:   master->cmd_ring_index = 0;
fp@0: 
fp@0:   for (i = 0; i < ECAT_COMMAND_RING_SIZE; i++)
fp@0:   {
fp@0:     EtherCAT_command_init(&master->cmd_ring[i]);
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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:   EC_DBG("Master clear");
fp@0: 
fp@0:   // Remove all pending commands
fp@0:   while (master->first_command)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, master->first_command);
fp@0:   }
fp@0: 
fp@0:   // Remove all slaves
fp@0:   EtherCAT_clear_slaves(master);
fp@0: 
fp@0:   if (master->process_data)
fp@0:   {
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:   EC_DBG("done...\n");
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Überprüft die angeschlossenen Slaves.
fp@0: 
fp@0:    Vergleicht die an den Bus angeschlossenen Slaves mit
fp@0:    den im statischen-Slave-Array vorgegebenen Konfigurationen.
fp@0:    Stimmen Anzahl oder Typen nicht überein, gibt diese
fp@0:    Methode einen Fehler aus.
fp@0: 
fp@0:    @param master Der EtherCAT-Master
fp@0:    @param slaves Zeiger auf ein statisches Slave-Array
fp@0:    @param slave_count Anzahl der Slaves im Array
fp@0: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_check_slaves(EtherCAT_master_t *master,
fp@0:                           EtherCAT_slave_t *slaves,
fp@0:                           unsigned int slave_count)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   EtherCAT_slave_t *cur;
fp@0:   unsigned int i, j, found, length, pos;
fp@0:   unsigned char data[2];
fp@0: 
fp@0:   // EtherCAT_clear_slaves() must be called before!
fp@0:   if (master->slaves || master->slave_count)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT duplicate slave check!");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // No slaves. 
fp@0:   if (slave_count == 0)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: No slaves in list!");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // Determine number of slaves on bus
fp@0: 
fp@0:   if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0:   
fp@0:   if (EtherCAT_async_send_receive(master) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != slave_count)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",
fp@0:            cmd->working_counter, slave_count);
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     return -1;
fp@0:   }
fp@0:   else
fp@0:   {
fp@0:     EC_DBG("EtherCAT: Slave count on bus: %i. Found: %i.\n",
fp@0:            cmd->working_counter, slave_count);
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);  
fp@0: 
fp@0:    // For every slave in the list
fp@0:   for (i = 0; i < slave_count; i++)
fp@0:   {
fp@0:     cur = &slaves[i];
fp@0: 
fp@0:     if (!cur->desc)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Slave %i has no description.\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     // Set ring position
fp@0:     cur->ring_position = -i;
fp@0:     cur->station_address = i + 1;
fp@0: 
fp@0:     // Write station address
fp@0:     
fp@0:     data[0] = cur->station_address & 0x00FF;
fp@0:     data[1] = (cur->station_address & 0xFF00) >> 8;
fp@0: 
fp@0:     if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL)
fp@0:     {
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_async_send_receive(master) != 0)
fp@0:     {
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     EtherCAT_remove_command(master, cmd);    
fp@0: 
fp@0:     // Read base data
fp@0: 
fp@0:     if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not create command!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_async_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not send command!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     // Get base data
fp@0:     cur->type = cmd->data[0];
fp@0:     cur->revision = cmd->data[1];
fp@0:     cur->build = cmd->data[2] | (cmd->data[3] << 8);
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     // Read identification from "Slave Information Interface" (SII)
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address,
fp@0:                                         0x0008, &cur->vendor_id) != 0)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not read SII!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address,
fp@0:                                         0x000A, &cur->product_code) != 0)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not read SII!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address,
fp@0:                                         0x000E, &cur->revision_number) != 0)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not read SII!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     // Search for identification in "database"
fp@0: 
fp@0:     found = 0;
fp@0: 
fp@0:     for (j = 0; j < slave_idents_count; j++)
fp@0:     {
fp@0:       if (slave_idents[j].vendor_id == cur->vendor_id
fp@0:           && slave_idents[j].product_code == cur->product_code)
fp@0:       {
fp@0:         found = 1;
fp@0: 
fp@0:         if (cur->desc != slave_idents[j].desc)
fp@0:         {
fp@0:           EC_DBG(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\""
fp@0:                  " at position %i. Expected: \"%s %s\"\n",
fp@0:                  slave_idents[j].desc->vendor_name,
fp@0:                  slave_idents[j].desc->product_name, i,
fp@0:                  cur->desc->vendor_name, cur->desc->product_name);
fp@0:           return -1;
fp@0:         }
fp@0: 
fp@0:         break;
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (!found)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Unknown slave device (vendor %X, code %X) at position %i.\n",
fp@0:              i, cur->vendor_id, cur->product_code);
fp@0:       return -1;      
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   length = 0;
fp@0:   for (i = 0; i < slave_count; i++)
fp@0:   {
fp@0:     length += slaves[i].desc->data_length;
fp@0:   }
fp@0: 
fp@0:   if ((master->process_data = (unsigned char *)
fp@0:        kmalloc(sizeof(unsigned char) * length, GFP_KERNEL)) == NULL)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not allocate %i bytes for process data.\n", length);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   master->process_data_length = length;
fp@0:   memset(master->process_data, 0x00, length);
fp@0: 
fp@0:   pos = 0;
fp@0:   for (i = 0; i < slave_count; i++)
fp@0:   {
fp@0:     slaves[i].process_data = master->process_data + pos;
fp@0:     slaves[i].logical_address0 = pos;
fp@0: 
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: Slave %i - \"%s %s\" - Logical Address 0x%08X\n",
fp@0:            i, slaves[i].desc->vendor_name, slaves[i].desc->product_name, pos);
fp@0: 
fp@0:     pos += slaves[i].desc->data_length;
fp@0:   }  
fp@0: 
fp@0:   master->slaves = slaves;
fp@0:   master->slave_count = slave_count;
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Entfernt den Zeiger auf das Slave-Array.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0: */
fp@0: 
fp@0: void EtherCAT_clear_slaves(EtherCAT_master_t *master)
fp@0: {
fp@0:   master->slaves = NULL;
fp@0:   master->slave_count = 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Liest Daten aus dem Slave-Information-Interface
fp@0:    eines EtherCAT-Slaves.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param node_address Knotenadresse des Slaves
fp@0:    @param offset Adresse des zu lesenden SII-Registers
fp@0:    @param target Zeiger auf einen 4 Byte großen Speicher
fp@0:    zum Ablegen der Daten
fp@0: 
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_read_slave_information(EtherCAT_master_t *master,
fp@0:                                     unsigned short int node_address,
fp@0:                                     unsigned short int offset,
fp@0:                                     unsigned int *target)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned char data[10];
fp@0:   unsigned int tries_left;
fp@0: 
fp@0:   // Initiate read operation
fp@0: 
fp@0:   data[0] = 0x00;
fp@0:   data[1] = 0x01;
fp@0:   data[2] = offset & 0xFF;
fp@0:   data[3] = (offset & 0xFF00) >> 8;
fp@0:   data[4] = 0x00;
fp@0:   data[5] = 0x00;
fp@0: 
fp@0:   if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL)
fp@0:     return -2;
fp@0: 
fp@0:   if (EtherCAT_async_send_receive(master) != 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     EC_DBG(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n",
fp@0:            node_address);
fp@0:     return -4;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   // Get status of read operation
fp@0: 
fp@0:   // ?? FIXME warum hier tries ?? Hm
fp@0: 
fp@0:   // Der Slave legt die Informationen des Slave-Information-Interface
fp@0:   // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange
fp@0:   // den Status auslesen, bis das Bit weg ist. fp
fp@0: 
fp@0:   tries_left = 1000;
fp@0:   while (tries_left)
fp@0:   {
fp@0:     udelay(10);
fp@0: 
fp@0:     if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL)
fp@0:       return -2;
fp@0:     
fp@0:     if (EtherCAT_async_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -3;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       EC_DBG(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n",
fp@0:              node_address);
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if ((cmd->data[1] & 0x81) == 0)
fp@0:     {
fp@0:       memcpy(target, cmd->data + 6, 4);
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       break;
fp@0:     }
fp@0:     
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     tries_left--;
fp@0:   }
fp@0: 
fp@0:   if (!tries_left)
fp@0:   {
fp@0:     EC_DBG(KERN_WARNING "EtherCAT: SSI-read. Slave %04X timed out!\n",
fp@0:            node_address);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Führt ein asynchrones Senden und Empfangen aus.
fp@0: 
fp@0:    Sendet alle wartenden Kommandos und wartet auf die
fp@0:    entsprechenden Antworten.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_async_send_receive(EtherCAT_master_t *master)
fp@0: {
fp@0:   unsigned int wait_cycles;
fp@0:   int i;
fp@0:   
fp@0:   // Send all commands
fp@0: 
fp@0:   //EC_DBG("Master async send");
fp@0: 
fp@0:   for (i = 0; i < ECAT_NUM_RETRIES; i++)
fp@0:   {
fp@0:     ASYNC = 1;
fp@0:     if (EtherCAT_send(master) < 0) return -1;
fp@0:     ASYNC = 0;
fp@0: 
fp@0:     // Wait until something is received or an error has occurred
fp@0:     wait_cycles = 10;
fp@0:     while (master->dev->state == ECAT_DS_SENT && wait_cycles)
fp@0:     {
fp@0:       udelay(1000);
fp@0:       wait_cycles--;
fp@0:     }
fp@0:     
fp@0:     //EC_DBG("Master async send: tries %d",tries_left);
fp@0:     
fp@0:     if (!wait_cycles)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Asynchronous receive timeout.\n");
fp@0:       continue;
fp@0:     }
fp@0: 
fp@0:     if (master->dev->state != ECAT_DS_RECEIVED)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Asyncronous send error. State %i\n", master->dev->state);
fp@0:       continue;
fp@0:     }
fp@0:     
fp@0:     // Receive all commands
fp@0:     if (EtherCAT_receive(master) < 0)
fp@0:     {
fp@0:       // Noch mal versuchen
fp@0:       master->dev->state = ECAT_DS_READY;
fp@0:       EC_DBG("Retry Asynchronous send/recieve: %d", i);
fp@0:       continue;
fp@0:     }
fp@0: 
fp@0:     return 0; // Erfolgreich
fp@0:   }
fp@0: 
fp@0:   return -1;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Sendet alle wartenden Kommandos.
fp@0: 
fp@0:    Errechnet erst die benötigte Gesamt-Rahmenlänge, und erstellt
fp@0:    dann alle Kommando-Bytefolgen im statischen Sendespeicher.
fp@0:    Danach wird die Sendefunktion des EtherCAT-Gerätes aufgerufen.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_send(EtherCAT_master_t *master)
fp@0: {
fp@0:   unsigned int i, length, framelength, pos;
fp@0:   EtherCAT_command_t *cmd;
fp@0:   int cmdcnt = 0;
fp@0: 
fp@0:   length = 0;
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     if (cmd->state != ECAT_CS_READY) continue;
fp@0:     length += cmd->data_length + 12;
fp@0:     cmdcnt++;
fp@0:   }
fp@0: 
fp@0:   if (length == 0)
fp@0:   {
fp@0:     EC_DBG(KERN_WARNING "EtherCAT: Nothing to send...\n");
fp@0:     return 0;
fp@0:   }
fp@0: 
fp@0:   framelength = length + 2;
fp@0:   if (framelength < 46) framelength = 46;
fp@0: 
fp@0:   if (ASYNC && framelength > 46)
fp@0:     EC_DBG(KERN_WARNING "Framelength: %d", framelength);
fp@0: 
fp@0:   if (ASYNC && cmdcnt > 1)
fp@0:     EC_DBG(KERN_WARNING "CMDcount: %d", cmdcnt);
fp@0: 
fp@0:   master->tx_data[0] = length & 0xFF;
fp@0:   master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;
fp@0:   pos = 2;
fp@0: 
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     if (cmd->state != ECAT_CS_READY) continue;
fp@0: 
fp@0:     cmd->index = master->command_index;
fp@0:     master->command_index = (master->command_index + 1) % 0x0100;
fp@0: 
fp@0:     cmd->state = ECAT_CS_SENT;
fp@0: 
fp@0:     master->tx_data[pos + 0] = cmd->type;
fp@0:     master->tx_data[pos + 1] = cmd->index;
fp@0: 
fp@0:     master->tx_data[pos + 2] = cmd->address.raw[0];
fp@0:     master->tx_data[pos + 3] = cmd->address.raw[1];
fp@0:     master->tx_data[pos + 4] = cmd->address.raw[2];
fp@0:     master->tx_data[pos + 5] = cmd->address.raw[3];
fp@0: 
fp@0:     master->tx_data[pos + 6] = cmd->data_length & 0xFF;
fp@0:     master->tx_data[pos + 7] = (cmd->data_length & 0x700) >> 8;
fp@0: 
fp@0:     if (cmd->next) master->tx_data[pos + 7] |= 0x80;
fp@0: 
fp@0:     master->tx_data[pos + 8] = 0x00;
fp@0:     master->tx_data[pos + 9] = 0x00;
fp@0: 
fp@0:     if (cmd->type == ECAT_CMD_APWR
fp@0:         || cmd->type == ECAT_CMD_NPWR
fp@0:         || cmd->type == ECAT_CMD_BWR
fp@0:         || cmd->type == ECAT_CMD_LRW) // Write
fp@0:     {
fp@0:       for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = cmd->data[i];
fp@0:     }
fp@0:     else // Read
fp@0:     {
fp@0:       for (i = 0; i < cmd->data_length; i++) master->tx_data[pos + 10 + i] = 0x00;
fp@0:     }
fp@0: 
fp@0:     master->tx_data[pos + 10 + cmd->data_length] = 0x00;
fp@0:     master->tx_data[pos + 11 + cmd->data_length] = 0x00;
fp@0: 
fp@0:     pos += 12 + cmd->data_length;
fp@0:   }
fp@0: 
fp@0:   // Pad with zeros
fp@0:   while (pos < 46) master->tx_data[pos++] = 0x00;
fp@0:   
fp@0:   master->tx_data_length = framelength;
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   EC_DBG(KERN_DEBUG "\n");
fp@0:   EC_DBG(KERN_DEBUG ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
fp@0:   EC_DBG(KERN_DEBUG);
fp@0:   for (i = 0; i < framelength; i++)
fp@0:   {
fp@0:     EC_DBG("%02X ", master->tx_data[i]);
fp@0: 
fp@0:     if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:   }
fp@0:   EC_DBG("\n");
fp@0:   EC_DBG(KERN_DEBUG "-----------------------------------------------\n");
fp@0: #endif
fp@0:  
fp@0:   // Send frame
fp@0:   if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not send!\n");
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n");
fp@0:     EC_DBG(KERN_DEBUG);
fp@0:     for (i = 0; i < framelength; i++)
fp@0:     {
fp@0:       EC_DBG("%02X ", master->tx_data[i]);      
fp@0:       if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:     }
fp@0:     EC_DBG("\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Holt alle empfangenen Kommandos vom EtherCAT-Gerät.
fp@0: 
fp@0:    Kopiert einen empfangenen Rahmen vom EtherCAT-Gerät
fp@0:    in den Statischen Empfangsspeicher und ordnet dann
fp@0:    allen gesendeten Kommandos ihre Antworten zu.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_receive(EtherCAT_master_t *master)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned int length, pos, found, rx_data_length;
fp@0:   unsigned int command_follows, command_type, command_index;
fp@0:   unsigned int i;
fp@0: 
fp@0:   // Copy received data into master buffer (with locking)
fp@0:   rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data,
fp@0:                                            ECAT_FRAME_BUFFER_SIZE);
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   for (i = 0; i < rx_data_length; i++)
fp@0:   {
fp@0:     if (master->rx_data[i] == master->tx_data[i]) EC_DBG("   ");
fp@0:     else EC_DBG("%02X ", master->rx_data[i]);
fp@0:     if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:   }
fp@0:   EC_DBG("\n");
fp@0:   EC_DBG(KERN_DEBUG "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
fp@0:   EC_DBG(KERN_DEBUG "\n");
fp@0: #endif
fp@0: 
fp@0:   if (rx_data_length < 2)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (illegal length)!\n");
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n");
fp@0:     EC_DBG(KERN_DEBUG);
fp@0:     for (i = 0; i < master->tx_data_length; i++)
fp@0:     {
fp@0:       EC_DBG("%02X ", master->tx_data[i]);      
fp@0:       if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:     }
fp@0:     EC_DBG("\n");
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n");
fp@0:     EC_DBG(KERN_DEBUG);
fp@0:     for (i = 0; i < rx_data_length; i++)
fp@0:     {
fp@0:       EC_DBG("%02X ", master->rx_data[i]);
fp@0:       if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:     }
fp@0:     EC_DBG("\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // Länge des gesamten Frames prüfen
fp@0:   length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF);
fp@0: 
fp@0:   if (length > rx_data_length)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n");
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n");
fp@0:     EC_DBG(KERN_DEBUG);
fp@0:     for (i = 0; i < master->tx_data_length; i++)
fp@0:     {
fp@0:       EC_DBG("%02X ", master->tx_data[i]);      
fp@0:       if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:     }
fp@0:     EC_DBG("\n");
fp@0:     EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n");
fp@0:     EC_DBG(KERN_DEBUG);
fp@0:     for (i = 0; i < rx_data_length; i++)
fp@0:     {
fp@0:       EC_DBG("%02X ", master->rx_data[i]);
fp@0:       if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:     }
fp@0:     EC_DBG("\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   pos = 2; // LibPCAP: 16
fp@0:   command_follows = 1;
fp@0:   while (command_follows)
fp@0:   {
fp@0:     if (pos + 10 > rx_data_length)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command header!\n");
fp@0:       EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n");
fp@0:       EC_DBG(KERN_DEBUG);
fp@0:       for (i = 0; i < master->tx_data_length; i++)
fp@0:       {
fp@0:         EC_DBG("%02X ", master->tx_data[i]);      
fp@0:         if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:       }
fp@0:       EC_DBG("\n");
fp@0:       EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n");
fp@0:       EC_DBG(KERN_DEBUG);
fp@0:       for (i = 0; i < rx_data_length; i++)
fp@0:       {
fp@0:         EC_DBG("%02X ", master->rx_data[i]);
fp@0:         if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:       }
fp@0:       EC_DBG("\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     command_type = master->rx_data[pos];
fp@0:     command_index = master->rx_data[pos + 1];
fp@0:     length = (master->rx_data[pos + 6] & 0xFF)
fp@0:       | ((master->rx_data[pos + 7] & 0x07) << 8);
fp@0:     command_follows = master->rx_data[pos + 7] & 0x80;
fp@0: 
fp@0:     if (pos + length + 12 > rx_data_length)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n");
fp@0:       EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n");
fp@0:       EC_DBG(KERN_DEBUG);
fp@0:       for (i = 0; i < master->tx_data_length; i++)
fp@0:       {
fp@0:         EC_DBG("%02X ", master->tx_data[i]);      
fp@0:         if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:       }
fp@0:       EC_DBG("\n");
fp@0:       EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n");
fp@0:       EC_DBG(KERN_DEBUG);
fp@0:       for (i = 0; i < rx_data_length; i++)
fp@0:       {
fp@0:         EC_DBG("%02X ", master->rx_data[i]);
fp@0:         if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG);
fp@0:       }
fp@0:       EC_DBG("\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     found = 0;
fp@0: 
fp@0:     for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:     {
fp@0:       if (cmd->state == ECAT_CS_SENT
fp@0:           && cmd->type == command_type
fp@0:           && cmd->index == command_index
fp@0:           && cmd->data_length == length)
fp@0:       {
fp@0:         found = 1;
fp@0:         cmd->state = ECAT_CS_RECEIVED;
fp@0: 
fp@0:         // Empfangene Daten in Kommandodatenspeicher kopieren
fp@0:         memcpy(cmd->data, master->rx_data + pos + 10, length);
fp@0: 
fp@0:         // Working-Counter setzen
fp@0:         cmd->working_counter = (master->rx_data[pos + length + 10] & 0xFF)
fp@0:           | ((master->rx_data[pos + length + 11] & 0xFF) << 8);
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (!found)
fp@0:     {
fp@0:       EC_DBG(KERN_WARNING "EtherCAT: WARNING - Command not assigned!\n");
fp@0:     }
fp@0: 
fp@0:     pos += length + 12;
fp@0:   }
fp@0: 
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     if (cmd->state == ECAT_CS_SENT)
fp@0:     {
fp@0:       EC_DBG(KERN_WARNING "EtherCAT: WARNING - One command received no response!\n");
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   master->dev->state = ECAT_DS_READY;
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: #define ECAT_FUNC_HEADER \
fp@0:   EtherCAT_command_t *cmd; \
fp@0:   if ((cmd = alloc_cmd(master)) == NULL) \
fp@0:   { \
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Out of memory while creating command!\n"); \
fp@0:     return NULL; \
fp@0:   } \
fp@0:   EtherCAT_command_init(cmd)
fp@0: 
fp@0: #define ECAT_FUNC_WRITE_FOOTER \
fp@0:   cmd->data_length = length; \
fp@0:   memcpy(cmd->data, data, length); \
fp@0:   add_command(master, cmd); \
fp@0:   return cmd
fp@0: 
fp@0: #define ECAT_FUNC_READ_FOOTER \
fp@0:   cmd->data_length = length; \
fp@0:   add_command(master, cmd); \
fp@0:   return cmd
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-NPRD-Kommando.
fp@0: 
fp@0:    Alloziert ein "node-adressed physical read"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param node_address Adresse des Knotens (Slaves)
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu lesenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master,
fp@0:                                   unsigned short node_address,
fp@0:                                   unsigned short offset,
fp@0:                                   unsigned int length)
fp@0: {
fp@0:   if (node_address == 0x0000)
fp@0:     EC_DBG(KERN_WARNING "EtherCAT: Using node address 0x0000!\n");
fp@0: 
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_NPRD;
fp@0:   cmd->address.phy.dev.node = node_address;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_READ_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-NPWR-Kommando.
fp@0: 
fp@0:    Alloziert ein "node-adressed physical write"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param node_address Adresse des Knotens (Slaves)
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu schreibenden Daten
fp@0:    @param data Zeiger auf Speicher mit zu schreibenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master,
fp@0:                                    unsigned short node_address,
fp@0:                                    unsigned short offset,
fp@0:                                    unsigned int length,
fp@0:                                    const unsigned char *data)
fp@0: {
fp@0:   if (node_address == 0x0000)
fp@0:     EC_DBG(KERN_WARNING "WARNING: Using node address 0x0000!\n");
fp@0: 
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_NPWR;
fp@0:   cmd->address.phy.dev.node = node_address;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_WRITE_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-APRD-Kommando.
fp@0: 
fp@0:    Alloziert ein "autoincerement physical read"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param ring_position (Negative) Position des Slaves im Bus
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu lesenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master,
fp@0:                                            short ring_position,
fp@0:                                            unsigned short offset,
fp@0:                                            unsigned int length)
fp@0: {
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_APRD;
fp@0:   cmd->address.phy.dev.pos = ring_position;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_READ_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-APWR-Kommando.
fp@0: 
fp@0:    Alloziert ein "autoincrement physical write"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param ring_position (Negative) Position des Slaves im Bus
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu schreibenden Daten
fp@0:    @param data Zeiger auf Speicher mit zu schreibenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master,
fp@0:                                             short ring_position,
fp@0:                                             unsigned short offset,
fp@0:                                             unsigned int length,
fp@0:                                             const unsigned char *data)
fp@0: {
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_APWR;
fp@0:   cmd->address.phy.dev.pos = ring_position;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_WRITE_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-BRD-Kommando.
fp@0: 
fp@0:    Alloziert ein "broadcast read"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu lesenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master,
fp@0:                                             unsigned short offset,
fp@0:                                             unsigned int length)
fp@0: {
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_BRD;
fp@0:   cmd->address.phy.dev.node = 0x0000;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_READ_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-BWR-Kommando.
fp@0: 
fp@0:    Alloziert ein "broadcast write"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param offset Physikalische Speicheradresse im Slave
fp@0:    @param length Länge der zu schreibenden Daten
fp@0:    @param data Zeiger auf Speicher mit zu schreibenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master,
fp@0:                                              unsigned short offset,
fp@0:                                              unsigned int length,
fp@0:                                              const unsigned char *data)
fp@0: {
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_BWR;
fp@0:   cmd->address.phy.dev.node = 0x0000;
fp@0:   cmd->address.phy.mem = offset;
fp@0: 
fp@0:   ECAT_FUNC_WRITE_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Erstellt ein EtherCAT-LRW-Kommando.
fp@0: 
fp@0:    Alloziert ein "logical read write"-Kommando
fp@0:    und fügt es in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param offset Logische Speicheradresse
fp@0:    @param length Länge der zu lesenden/schreibenden Daten
fp@0:    @param data Zeiger auf Speicher mit zu lesenden/schreibenden Daten
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master,
fp@0:                                                 unsigned int offset,
fp@0:                                                 unsigned int length,
fp@0:                                                 unsigned char *data)
fp@0: {
fp@0:   ECAT_FUNC_HEADER;
fp@0: 
fp@0:   cmd->type = ECAT_CMD_LRW;
fp@0:   cmd->address.logical = offset;
fp@0: 
fp@0:   ECAT_FUNC_WRITE_FOOTER;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Alloziert ein neues Kommando aus dem Kommandoring.
fp@0: 
fp@0:    Durchsucht den Kommandoring nach einem freien Kommando,
fp@0:    reserviert es und gibt dessen Adresse zurück.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    
fp@0:    @return Adresse des Kommandos bei Erfolg, sonst NULL
fp@0: */
fp@0: 
fp@0: EtherCAT_command_t *alloc_cmd(EtherCAT_master_t *master)
fp@0: {
fp@0:   int j;
fp@0: 
fp@0:   for (j = 0; j < ECAT_COMMAND_RING_SIZE; j++) // Einmal rum suchen
fp@0:   { 
fp@0:     // Solange suchen, bis freies Kommando gefunden
fp@0:     if (master->cmd_ring[master->cmd_ring_index].reserved == 0)
fp@0:     {
fp@0:       master->cmd_ring[master->cmd_ring_index].reserved = 1; // Belegen
fp@0:       return &master->cmd_ring[master->cmd_ring_index];
fp@0:     }
fp@0: 
fp@0:     master->cmd_ring_index++;
fp@0:     master->cmd_ring_index %= ECAT_COMMAND_RING_SIZE;
fp@0:   }
fp@0: 
fp@0:   EC_DBG(KERN_WARNING "EtherCAT: Command ring full!\n");
fp@0: 
fp@0:   return NULL; // Nix gefunden
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Fügt ein Kommando in die Liste des Masters ein.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param cmd Zeiger auf das einzufügende Kommando
fp@0: */
fp@0: 
fp@0: void add_command(EtherCAT_master_t *master,
fp@0:                  EtherCAT_command_t *cmd)
fp@0: {
fp@0:   EtherCAT_command_t **last_cmd;
fp@0: 
fp@0:   // Find the address of the last pointer in the list
fp@0:   last_cmd = &(master->first_command);
fp@0:   while (*last_cmd) last_cmd = &(*last_cmd)->next;
fp@0: 
fp@0:   // Let this pointer point to the new command
fp@0:   *last_cmd = cmd;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Entfernt ein Kommando aus der Liste und gibt es frei.
fp@0: 
fp@0:    Prüft erst, ob das Kommando in der Liste des Masters
fp@0:    ist. Wenn ja, wird es entfernt und die Reservierung wird
fp@0:    aufgehoben.
fp@0: 
fp@0:    @param master EtherCAT-Master
fp@0:    @param rem_cmd Zeiger auf das zu entfernende Kommando
fp@0:    
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: void EtherCAT_remove_command(EtherCAT_master_t *master,
fp@0:                              EtherCAT_command_t *rem_cmd)
fp@0: {
fp@0:   EtherCAT_command_t **last_cmd;
fp@0: 
fp@0:   last_cmd = &master->first_command;
fp@0:   while (*last_cmd)
fp@0:   {
fp@0:     if (*last_cmd == rem_cmd)
fp@0:     {
fp@0:       *last_cmd = rem_cmd->next;
fp@0: 
fp@0:       EtherCAT_command_clear(rem_cmd);
fp@0: 
fp@0:       return;
fp@0:     }
fp@0: 
fp@0:     last_cmd = &(*last_cmd)->next;
fp@0:   }
fp@0: 
fp@0:   EC_DBG(KERN_WARNING "EtherCAT: Trying to remove non-existent command!\n");
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
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@0:   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@0:   if ((cmd = EtherCAT_write(master, slave->station_address,
fp@0:                             0x0120, 2, data)) == NULL)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Out of memory!\n", state_and_ack);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_async_send_receive(master) != 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack);
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device did not respond!\n", state_and_ack);
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);  
fp@0: 
fp@0:   slave->requested_state = state_and_ack & 0x0F;
fp@0: 
fp@0:   tries_left = 1000;
fp@0: 
fp@0:   while (tries_left)
fp@0:   {
fp@0:     udelay(10);
fp@0: 
fp@0:     if ((cmd = EtherCAT_read(master, slave->station_address,
fp@0:                              0x0130, 2)) == NULL)
fp@0:     {
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Out of memory!\n", state_and_ack);
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_async_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack);
fp@0:       return -2;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack);
fp@0:       return -3;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] & 0x10) // State change error
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd->data[0]);
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] == (state_and_ack & 0x0F)) // State change successful
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       break;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     tries_left--;
fp@0:   }
fp@0: 
fp@0:   if (!tries_left)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - 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@0: /***************************************************************/
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@0:    
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@0:   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@0:   if (EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // Resetting FMMU's
fp@0: 
fp@0:   memset(data, 0x00, 256);
fp@0: 
fp@0:   if ((cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data)) == NULL)
fp@0:     return -1;
fp@0: 
fp@0:   if (EtherCAT_async_send_receive(master) < 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n",
fp@0:            slave->station_address);
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
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@0:     if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data)) == NULL)
fp@0:       return -1;
fp@0: 
fp@0:     if (EtherCAT_async_send_receive(master) < 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -2;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
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@0:       if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0)) == NULL)
fp@0:         return -1;
fp@0:       
fp@0:       if (EtherCAT_async_send_receive(master) < 0)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         return -1;
fp@0:       }
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -3;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@0:       if ((cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1)) == NULL)
fp@0:         return -1;
fp@0:       
fp@0:       if (EtherCAT_async_send_receive(master) < 0)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         return -1;
fp@0:       }
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -2;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   // Change state to PREOP
fp@0: 
fp@0:   if (EtherCAT_state_change(master, slave, ECAT_STATE_PREOP) != 0)
fp@0:   {
fp@0:     return -5;
fp@0:   }
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@0:     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@0:     if ((cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu)) == NULL)
fp@0:       return -1;
fp@0: 
fp@0:     if (EtherCAT_async_send_receive(master) < 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -2;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
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@0:       if ((cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0)) == NULL)
fp@0:         return -1;
fp@0:       
fp@0:       if (EtherCAT_async_send_receive(master) < 0)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         return -1;
fp@0:       }
fp@0: 
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -3;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@0:       if ((cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1)) == NULL)
fp@0:         return -1;
fp@0:       
fp@0:       if (EtherCAT_async_send_receive(master) < 0)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         return -1;
fp@0:       }
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         EtherCAT_remove_command(master, cmd);
fp@0:         
fp@0:         EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n",
fp@0:                slave->station_address);
fp@0:         return -3;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0:   }
fp@0:   
fp@0:   if (desc->sm2)
fp@0:   {
fp@0:     if ((cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2)) == NULL)
fp@0:       return -1;
fp@0:       
fp@0:     if (EtherCAT_async_send_receive(master) < 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       return -1;
fp@0:     }
fp@0:       
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -3;
fp@0:     }
fp@0:       
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:   }
fp@0: 
fp@0:   if (desc->sm3)
fp@0:   {
fp@0:     if ((cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3)) == NULL)
fp@0:       return -1;
fp@0:       
fp@0:     if (EtherCAT_async_send_receive(master) < 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       
fp@0:       EC_DBG(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n",
fp@0:              slave->station_address);
fp@0:       return -3;
fp@0:     }
fp@0:       
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:   }
fp@0: 
fp@0:   // Change state to SAVEOP
fp@0:   if (EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0)
fp@0:   {
fp@0:     return -12;
fp@0:   }
fp@0: 
fp@0:   // Change state to OP
fp@0:   if (EtherCAT_state_change(master, slave, ECAT_STATE_OP) != 0)
fp@0:   {
fp@0:     return -13;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
fp@0:    @return 0 bei Erfolg, sonst < 0
fp@0: */
fp@0: 
fp@0: int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
fp@0:                             EtherCAT_slave_t *slave)
fp@0: {
fp@0:   if (EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
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@0:     EC_DBG("Activate Slave: %d\n",i);
fp@0: 
fp@0:     if (EtherCAT_activate_slave(master, &master->slaves[i]) < 0)
fp@0:     {
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     EC_DBG("done...\n");
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
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@0:     EC_DBG("Deactivate Slave: %d\n",i);
fp@0:     
fp@0:     if (EtherCAT_deactivate_slave(master, &master->slaves[i]) < 0)
fp@0:     {
fp@0:       ret = -1;
fp@0:     }
fp@0:     
fp@0:     EC_DBG("done...\n");
fp@0:   }
fp@0: 
fp@0:   return ret;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
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@0:   if (master->process_data_command)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, master->process_data_command);
fp@0:     EtherCAT_command_clear(master->process_data_command);
fp@0:     master->process_data_command = NULL;
fp@0:   }
fp@0: 
fp@0:   if ((master->process_data_command
fp@0:        = EtherCAT_logical_read_write(master, 0,
fp@0:                                      master->process_data_length,
fp@0:                                      master->process_data)) == NULL)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not allocate process data command!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_send(master) < 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, master->process_data_command);
fp@0:     EtherCAT_command_clear(master->process_data_command);
fp@0:     master->process_data_command = NULL;
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not send process data command!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
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@0:    
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@0:   int ret = -1;
fp@0: 
fp@0:   if (!master->process_data_command)
fp@0:   {
fp@0:     EC_DBG(KERN_WARNING "EtherCAT: No process data command available!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_receive(master) < 0)
fp@0:   {
fp@0:     EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n");
fp@0:   }
fp@0:   else if (master->process_data_command->state != ECAT_CS_RECEIVED)
fp@0:   {
fp@0:     EC_DBG(KERN_WARNING "EtherCAT: Process data command not received!\n");
fp@0:   }
fp@0:   else
fp@0:   {
fp@0:     // Daten von Kommando in Prozessdaten des Master kopieren
fp@0:     memcpy(master->process_data, master->process_data_command->data, master->process_data_length);
fp@0:     ret = 0;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, master->process_data_command);
fp@0:   EtherCAT_command_clear(master->process_data_command);
fp@0:   master->process_data_command = NULL;
fp@0: 
fp@0:   return ret;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Setzt einen Byte-Wert im Speicher.
fp@0: 
fp@0:    @param data Startadresse
fp@0:    @param offset Byte-Offset
fp@0:    @param value Wert
fp@0: */
fp@0: 
fp@0: void set_byte(unsigned char *data,
fp@0:               unsigned int offset,
fp@0:               unsigned char value)
fp@0: {
fp@0:   data[offset] = value;
fp@0: }
fp@0: 
fp@0: /***************************************************************/
fp@0: 
fp@0: /**
fp@0:    Setzt einen Word-Wert im Speicher.
fp@0: 
fp@0:    @param data Startadresse
fp@0:    @param offset Byte-Offset
fp@0:    @param value Wert
fp@0: */
fp@0: 
fp@0: void set_word(unsigned char *data,
fp@0:               unsigned int offset,
fp@0:               unsigned int value)
fp@0: {
fp@0:   data[offset] = value & 0xFF;
fp@0:   data[offset + 1] = (value & 0xFF00) >> 8;
fp@0: }
fp@0: 
fp@0: /***************************************************************/