?nderungen an no_rtai r110:110 in drivers gemergt.
authorFlorian Pose <fp@igh-essen.com>
Fri, 28 Oct 2005 15:12:27 +0000
changeset 2 b0a7a4745bf9
parent 1 98acc19c7594
child 3 3ea74844c2df
?nderungen an no_rtai r110:110 in drivers gemergt.
drivers/ec_command.c
drivers/ec_command.h
drivers/ec_dbg.h
drivers/ec_device.c
drivers/ec_master.c
drivers/ec_master.h
drivers/ec_slave.c
--- a/drivers/ec_command.c	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_command.c	Fri Oct 28 15:12:27 2005 +0000
@@ -29,13 +29,10 @@
   cmd->type = ECAT_CMD_NONE;
   cmd->address.logical = 0x00000000;
   cmd->data_length = 0;
-
   cmd->next = NULL;
-
   cmd->state = ECAT_CS_READY;
   cmd->index = 0;
   cmd->working_counter = 0;
-  cmd->reserved = 0; //Hm
 }
 
 /***************************************************************/
@@ -43,16 +40,14 @@
 /**
    Kommando-Destruktor.
 
-   Setzt nur den Status auf ECAT_CS_READY und das
-   reserved-Flag auf 0.
+   Setzt alle Attribute auf den Anfangswert zurueck.
    
    @param cmd Zeiger auf das zu initialisierende Kommando.
 */
 
 void EtherCAT_command_clear(EtherCAT_command_t *cmd)
 {
-  cmd->state = ECAT_CS_READY;
-  cmd->reserved = 0;
+  EtherCAT_command_init(cmd);
 }
 
 /***************************************************************/
--- a/drivers/ec_command.h	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_command.h	Fri Oct 28 15:12:27 2005 +0000
@@ -78,8 +78,6 @@
   unsigned int working_counter; /**< Working-Counter bei Empfang (wird vom Master gesetzt) */
 
   unsigned char data[ECAT_FRAME_BUFFER_SIZE]; /**< Kommandodaten */
-
-  unsigned char reserved; /**< Markiert, das das Kommando gerade benutzt wird */
 }
 EtherCAT_command_t;
 
--- a/drivers/ec_dbg.h	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_dbg.h	Fri Oct 28 15:12:27 2005 +0000
@@ -15,6 +15,7 @@
 
 #define EC_DBG(fmt, args...) SDBG_print("%s: " fmt, __FUNCTION__ , ## args)
 //#define EC_DBG(fmt, args...) printk(KERN_INFO fmt, ## args)
+
 #else
 #define EC_DBG(fmt, args...)
 #endif
--- a/drivers/ec_device.c	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_device.c	Fri Oct 28 15:12:27 2005 +0000
@@ -13,6 +13,7 @@
 #include <linux/if_ether.h>
 #include <linux/netdevice.h>
 #include <rtai.h>
+#include <linux/delay.h>
 
 #include "ec_device.h"
 #include "ec_dbg.h"
@@ -237,7 +238,7 @@
   // Start sending of frame
   ecd->state = ECAT_DS_SENT;
   ecd->dev->hard_start_xmit(ecd->tx_skb, ecd->dev);
-  
+
   return 0;
 }
 
@@ -263,21 +264,16 @@
                             unsigned int size)
 {
   int cnt;
-//  unsigned long flags;
-  
+
   if (ecd->state != ECAT_DS_RECEIVED)
   {
     EC_DBG(KERN_ERR "EtherCAT: receive - Nothing received!\n");
     return -1;
   }
-    
-//  flags = rt_spin_lock_irqsave(ecd->lock);
 
   cnt = min(ecd->rx_data_length, size);
   memcpy(data,ecd->rx_data, cnt);
 
-//  rt_spin_unlock_irqrestore(ecd->lock, flags);
-
   return cnt;
 }
 
--- a/drivers/ec_master.c	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_master.c	Fri Oct 28 15:12:27 2005 +0000
@@ -55,10 +55,12 @@
   master->process_data = NULL;
   master->process_data_length = 0;
   master->cmd_ring_index = 0;
+  master->debug_level = 0;
 
   for (i = 0; i < ECAT_COMMAND_RING_SIZE; i++)
   {
     EtherCAT_command_init(&master->cmd_ring[i]);
+    master->cmd_reserved[i] = 0;
   }
 
   return 0;
@@ -77,8 +79,6 @@
 
 void EtherCAT_master_clear(EtherCAT_master_t *master)
 {
-  EC_DBG("Master clear");
-
   // Remove all pending commands
   while (master->first_command)
   {
@@ -95,8 +95,6 @@
   }
 
   master->process_data_length = 0;
-
-  EC_DBG("done...\n");
 }
 
 /***************************************************************/
@@ -464,12 +462,13 @@
   
   // Send all commands
 
-  //EC_DBG("Master async send");
-
   for (i = 0; i < ECAT_NUM_RETRIES; i++)
   {
     ASYNC = 1;
-    if (EtherCAT_send(master) < 0) return -1;
+    if (EtherCAT_send(master) < 0)
+    {
+      return -1;
+    }
     ASYNC = 0;
 
     // Wait until something is received or an error has occurred
@@ -529,6 +528,11 @@
   EtherCAT_command_t *cmd;
   int cmdcnt = 0;
 
+  if (master->debug_level > 0)
+  {
+    EC_DBG(KERN_DEBUG "EtherCAT_send, first_command = %X\n", (int) master->first_command);
+  }
+
   length = 0;
   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
   {
@@ -537,6 +541,11 @@
     cmdcnt++;
   }
 
+  if (master->debug_level > 0)
+  {
+    EC_DBG(KERN_DEBUG "%i commands to send.\n", cmdcnt);
+  }
+
   if (length == 0)
   {
     EC_DBG(KERN_WARNING "EtherCAT: Nothing to send...\n");
@@ -546,11 +555,10 @@
   framelength = length + 2;
   if (framelength < 46) framelength = 46;
 
-  if (ASYNC && framelength > 46)
-    EC_DBG(KERN_WARNING "Framelength: %d", framelength);
-
-  if (ASYNC && cmdcnt > 1)
-    EC_DBG(KERN_WARNING "CMDcount: %d", cmdcnt);
+  if (master->debug_level > 0)
+  {
+    EC_DBG(KERN_DEBUG "framelength: %i\n", framelength);
+  }
 
   master->tx_data[0] = length & 0xFF;
   master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;
@@ -563,6 +571,11 @@
     cmd->index = master->command_index;
     master->command_index = (master->command_index + 1) % 0x0100;
 
+    if (master->debug_level > 0)
+    {
+      EC_DBG(KERN_DEBUG "Sending command index %i\n", cmd->index);
+    }
+
     cmd->state = ECAT_CS_SENT;
 
     master->tx_data[pos + 0] = cmd->type;
@@ -617,6 +630,11 @@
   EC_DBG("\n");
   EC_DBG(KERN_DEBUG "-----------------------------------------------\n");
 #endif
+
+  if (master->debug_level > 0)
+  {
+    EC_DBG(KERN_DEBUG "device send...\n");
+  }
  
   // Send frame
   if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0)
@@ -633,6 +651,11 @@
     return -1;
   }
 
+  if (master->debug_level > 0)
+  {
+    EC_DBG(KERN_DEBUG "EtherCAT_send done.\n");
+  }
+
   return 0;
 }
 
@@ -830,12 +853,12 @@
 #define ECAT_FUNC_WRITE_FOOTER \
   cmd->data_length = length; \
   memcpy(cmd->data, data, length); \
-  add_command(master, cmd); \
+  if (add_command(master, cmd) < 0) return NULL; \
   return cmd
 
 #define ECAT_FUNC_READ_FOOTER \
   cmd->data_length = length; \
-  add_command(master, cmd); \
+  if (add_command(master, cmd) < 0) return NULL; \
   return cmd
 
 /***************************************************************/
@@ -1075,12 +1098,27 @@
   for (j = 0; j < ECAT_COMMAND_RING_SIZE; j++) // Einmal rum suchen
   { 
     // Solange suchen, bis freies Kommando gefunden
-    if (master->cmd_ring[master->cmd_ring_index].reserved == 0)
-    {
-      master->cmd_ring[master->cmd_ring_index].reserved = 1; // Belegen
+    if (master->cmd_reserved[master->cmd_ring_index] == 0)
+    {
+      master->cmd_reserved[master->cmd_ring_index] = 1; // Belegen
+
+      if (master->debug_level)
+      {
+        EC_DBG(KERN_DEBUG "Allocating command %i (addr %X).\n",
+               master->cmd_ring_index,
+               (int) &master->cmd_ring[master->cmd_ring_index]);
+      }
+
       return &master->cmd_ring[master->cmd_ring_index];
     }
 
+    if (master->debug_level)
+    {
+      EC_DBG(KERN_DEBUG "Command %i (addr %X) is reserved...\n",
+             master->cmd_ring_index,
+             (int) &master->cmd_ring[master->cmd_ring_index]);
+    }
+
     master->cmd_ring_index++;
     master->cmd_ring_index %= ECAT_COMMAND_RING_SIZE;
   }
@@ -1099,17 +1137,29 @@
    @param cmd Zeiger auf das einzufügende Kommando
 */
 
-void add_command(EtherCAT_master_t *master,
-                 EtherCAT_command_t *cmd)
-{
-  EtherCAT_command_t **last_cmd;
+int add_command(EtherCAT_master_t *master,
+                EtherCAT_command_t *new_cmd)
+{
+  EtherCAT_command_t *cmd, **last_cmd;
+
+  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
+  {
+    if (cmd == new_cmd)
+    {
+      EC_DBG(KERN_WARNING "EtherCAT: Trying to add a command"
+             " that is already in the list!\n");
+      return -1;
+    }
+  }
 
   // Find the address of the last pointer in the list
   last_cmd = &(master->first_command);
   while (*last_cmd) last_cmd = &(*last_cmd)->next;
 
   // Let this pointer point to the new command
-  *last_cmd = cmd;
+  *last_cmd = new_cmd;
+
+  return 0;
 }
 
 /***************************************************************/
@@ -1131,6 +1181,7 @@
                              EtherCAT_command_t *rem_cmd)
 {
   EtherCAT_command_t **last_cmd;
+  int i;
 
   last_cmd = &master->first_command;
   while (*last_cmd)
@@ -1138,9 +1189,19 @@
     if (*last_cmd == rem_cmd)
     {
       *last_cmd = rem_cmd->next;
-
       EtherCAT_command_clear(rem_cmd);
 
+      // Reservierung des Kommandos aufheben
+      for (i = 0; i < ECAT_COMMAND_RING_SIZE; i++)
+      {
+        if (&master->cmd_ring[i] == rem_cmd)
+        {
+          master->cmd_reserved[i] = 0;
+          return;
+        }
+      }
+
+      EC_DBG(KERN_WARNING "EtherCAT: Could not remove command reservation!\n");
       return;
     }
 
@@ -1595,14 +1656,10 @@
 
   for (i = 0; i < master->slave_count; i++)
   {
-    EC_DBG("Activate Slave: %d\n",i);
-
     if (EtherCAT_activate_slave(master, &master->slaves[i]) < 0)
     {
       return -1;
     }
-
-    EC_DBG("done...\n");
   }
 
   return 0;
@@ -1627,14 +1684,10 @@
 
   for (i = 0; i < master->slave_count; i++)
   {
-    EC_DBG("Deactivate Slave: %d\n",i);
-    
     if (EtherCAT_deactivate_slave(master, &master->slaves[i]) < 0)
     {
       ret = -1;
     }
-    
-    EC_DBG("done...\n");
   }
 
   return ret;
@@ -1663,8 +1716,8 @@
   }
 
   if ((master->process_data_command
-       = EtherCAT_logical_read_write(master, 0,
-                                     master->process_data_length,
+       = EtherCAT_logical_read_write(master,
+                                     0, master->process_data_length,
                                      master->process_data)) == NULL)
   {
     EC_DBG(KERN_ERR "EtherCAT: Could not allocate process data command!\n");
@@ -1732,6 +1785,27 @@
 /***************************************************************/
 
 /**
+   Verwirft ein zuvor gesendetes Prozessdatenkommando.
+
+   Diese Funktion sollte nach Ende des zyklischen Betriebes
+   aufgerufen werden, um das noch wartende Prozessdaten-Kommando
+   zu entfernen.
+
+   @param master EtherCAT-Master
+*/
+
+void EtherCAT_clear_process_data(EtherCAT_master_t *master)
+{
+  if (!master->process_data_command) return;
+
+  EtherCAT_remove_command(master, master->process_data_command);
+  EtherCAT_command_clear(master->process_data_command);
+  master->process_data_command = NULL;
+}
+
+/***************************************************************/
+
+/**
    Setzt einen Byte-Wert im Speicher.
 
    @param data Startadresse
--- a/drivers/ec_master.h	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_master.h	Fri Oct 28 15:12:27 2005 +0000
@@ -51,8 +51,11 @@
   unsigned char *process_data; /**< Zeiger auf Speicher mit Prozessdaten */
   unsigned int process_data_length; /**< Länge der Prozessdaten */
 
-  EtherCAT_command_t cmd_ring[ECAT_COMMAND_RING_SIZE]; /** Statischer Kommandoring */
+  EtherCAT_command_t cmd_ring[ECAT_COMMAND_RING_SIZE]; /**< Statischer Kommandoring */
+  int cmd_reserved[ECAT_COMMAND_RING_SIZE]; /**< Reservierungsflags für die Kommandos */
   unsigned int cmd_ring_index; /**< Index des nächsten Kommandos im Ring */
+
+  int debug_level; /**< Debug-Level im Master-Code */
 }
 EtherCAT_master_t;
 
@@ -76,6 +79,7 @@
 int EtherCAT_receive(EtherCAT_master_t *);
 int EtherCAT_write_process_data(EtherCAT_master_t *);
 int EtherCAT_read_process_data(EtherCAT_master_t *);
+void EtherCAT_clear_process_data(EtherCAT_master_t *);
 
 /***************************************************************/
 
@@ -125,7 +129,7 @@
 
 // Private functions
 EtherCAT_command_t *alloc_cmd(EtherCAT_master_t *);
-void add_command(EtherCAT_master_t *, EtherCAT_command_t *);
+int add_command(EtherCAT_master_t *, EtherCAT_command_t *);
 void set_byte(unsigned char *, unsigned int, unsigned char);
 void set_word(unsigned char *, unsigned int, unsigned int);
 
--- a/drivers/ec_slave.c	Fri Oct 21 11:44:10 2005 +0000
+++ b/drivers/ec_slave.c	Fri Oct 28 15:12:27 2005 +0000
@@ -83,26 +83,28 @@
 {
   if (!slave->desc)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Reading failed - "
-           "Slave %04X (at %0X) has no description.\n",
+    EC_DBG(KERN_WARNING "EtherCAT: Reading failed on slave %04X (addr %0X)"
+           " - Slave has no description.\n",
            slave->station_address, (unsigned int) slave);
     return 0;
   }
 
   if (!slave->desc->read)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Reading failed - "
-           "Slave type (%s %s) has no read method.\n",
+    EC_DBG(KERN_WARNING "EtherCAT: Reading failed on slave %04X (addr %0X)"
+           " - Slave type (%s %s) has no read method.\n",
+           slave->station_address, (unsigned int) slave,
            slave->desc->vendor_name, slave->desc->product_name);
     return 0;
   }
 
   if (channel >= slave->desc->channels)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Reading failed - "
-           "Slave %4X (%s %s) has no channel %i.\n",
-           slave->station_address, slave->desc->vendor_name,
-           slave->desc->product_name, channel);
+    EC_DBG(KERN_WARNING "EtherCAT: Reading failed on slave %4X (addr %0X)"
+           " - Type (%s %s) has no channel %i.\n",
+           slave->station_address, (unsigned int) slave,
+           slave->desc->vendor_name, slave->desc->product_name,
+           channel);
     return 0;
   }
 
@@ -131,26 +133,28 @@
 {
   if (!slave->desc)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Writing failed - "
-           "Slave %04X (at %0X) has no description.\n",
+    EC_DBG(KERN_WARNING "EtherCAT: Writing failed on slave %04X (addr %0X)"
+           " - Slave has no description.\n",
            slave->station_address, (unsigned int) slave);
     return;
   }
 
   if (!slave->desc->write)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Writing failed - "
-           "Slave type (%s %s) has no write method.\n",
+    EC_DBG(KERN_WARNING "EtherCAT: Writing failed on slave %04X (addr %0X)"
+           " - Type (%s %s) has no write method.\n",
+           slave->station_address, (unsigned int) slave,
            slave->desc->vendor_name, slave->desc->product_name);
     return;
   }
 
   if (channel >= slave->desc->channels)
   {
-    EC_DBG(KERN_WARNING "EtherCAT: Writing failed - "
-           "Slave %4X (%s %s) has no channel %i.\n",
-           slave->station_address, slave->desc->vendor_name,
-           slave->desc->product_name, channel);
+    EC_DBG(KERN_WARNING "EtherCAT: Writing failed on slave %4X (addr %0X)"
+           " - Type (%s %s) has no channel %i.\n",
+           slave->station_address, (unsigned int) slave,
+           slave->desc->vendor_name, slave->desc->product_name,
+           channel);
     return;
   }