Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
drivers/ec_master.c
changeset 39 6965c23a6826
parent 35 ed834aa98f89
child 42 a22a202d0f42
equal deleted inserted replaced
38:3213cbbd58b7 39:6965c23a6826 
     1 /****************************************************************
 
     1 /******************************************************************************
 
     2  *
 
     2  *
 
     3  *  e c _ m a s t e r . c
 
     3  *  e c _ m a s t e r . c
 
     4  *
 
     4  *
 
     5  *  Methoden für einen EtherCAT-Master.
 
     5  *  Methoden für einen EtherCAT-Master.
 
     6  *
 
     6  *
 
     7  *  $Date$
 
     7  *  $Id$
 
     8  *  $Author$
 
       
     9  *
 
     8  *
 
    10  ***************************************************************/
 
     9  *****************************************************************************/
 
    11 
    10 
    12 #include <linux/module.h>
 
    11 #include <linux/module.h>
 
    13 #include <linux/kernel.h>
 
    12 #include <linux/kernel.h>
 
    14 #include <linux/string.h>
 
    13 #include <linux/string.h>
 
    15 #include <linux/slab.h>
 
    14 #include <linux/slab.h>
 
    16 #include <linux/delay.h>
 
    15 #include <linux/delay.h>
 
    17 
    16 
    18 #include "ec_globals.h"
 
    17 #include "ec_globals.h"
 
    19 #include "ec_master.h"
 
    18 #include "ec_master.h"
 
    20 
    19 
    21 /***************************************************************/
 
    20 /*****************************************************************************/
 
    22 
    21 
    23 /**
 
    22 /**
 
    24    Konstruktor des EtherCAT-Masters.
 
    23    Konstruktor des EtherCAT-Masters.
 
    25 
    24 
    26    @param master Zeiger auf den zu initialisierenden
 
    25    @param master Zeiger auf den zu initialisierenden
 
    27                  EtherCAT-Master
 
    26    EtherCAT-Master
 
    28 
    27 
    29    @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)
 
    28    @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)
 
    30 */
 
    29 */
 
    31 
    30 
    32 void EtherCAT_master_init(EtherCAT_master_t *master)
 
    31 void EtherCAT_master_init(EtherCAT_master_t *master)
 
    39   master->process_data = NULL;
 
    38   master->process_data = NULL;
 
    40   master->process_data_length = 0;
 
    39   master->process_data_length = 0;
 
    41   master->debug_level = 0;
 
    40   master->debug_level = 0;
 
    42 }
 
    41 }
 
    43 
    42 
    44 /***************************************************************/
 
    43 /*****************************************************************************/
 
    45 
    44 
    46 /**
 
    45 /**
 
    47    Destruktor eines EtherCAT-Masters.
 
    46    Destruktor eines EtherCAT-Masters.
 
    48 
    47 
    49    Entfernt alle Kommandos aus der Liste, löscht den Zeiger
 
    48    Entfernt alle Kommandos aus der Liste, löscht den Zeiger
 
    55 void EtherCAT_master_clear(EtherCAT_master_t *master)
 
    54 void EtherCAT_master_clear(EtherCAT_master_t *master)
 
    56 {
 
    55 {
 
    57   // Remove all slaves
 
    56   // Remove all slaves
 
    58   EtherCAT_clear_slaves(master);
 
    57   EtherCAT_clear_slaves(master);
 
    59 
    58 
    60   if (master->process_data)
 
    59   if (master->process_data) {
 
    61   {
 
       
    62     kfree(master->process_data);
 
    60     kfree(master->process_data);
 
    63     master->process_data = NULL;
 
    61     master->process_data = NULL;
 
    64   }
 
    62   }
 
    65 
    63 
    66   master->process_data_length = 0;
 
    64   master->process_data_length = 0;
 
    67 }
 
    65 }
 
    68 
    66 
    69 /***************************************************************/
 
    67 /*****************************************************************************/
 
    70 
    68 
    71 /**
 
    69 /**
 
    72    Öffnet ein EtherCAT-Geraet für den Master.
 
    70    Öffnet ein EtherCAT-Geraet für den Master.
 
    73 
    71 
    74    Registriert das Geraet beim Master, der es daraufhin oeffnet.
 
    72    Registriert das Geraet beim Master, der es daraufhin oeffnet.
 
    75 
    73 
    76    @param master Der EtherCAT-Master
 
    74    @param master Der EtherCAT-Master
 
    77    @param device Das EtherCAT-Geraet
 
    75    @param device Das EtherCAT-Geraet
 
    78    @return 0, wenn alles o.k.,
 
    76    @return 0, wenn alles o.k.,
 
    79            < 0, wenn bereits ein Geraet registriert
 
    77    < 0, wenn bereits ein Geraet registriert
 
    80            oder das Geraet nicht geoeffnet werden konnte.
 
    78    oder das Geraet nicht geoeffnet werden konnte.
 
    81 */
 
    79 */
 
    82 
    80 
    83 int EtherCAT_master_open(EtherCAT_master_t *master,
 
    81 int EtherCAT_master_open(EtherCAT_master_t *master,
 
    84                          EtherCAT_device_t *device)
 
    82                          EtherCAT_device_t *device)
 
    85 {
 
    83 {
 
    86   if (!master || !device)
 
    84   if (!master || !device) {
 
    87   {
 
    85     printk(KERN_ERR "EtherCAT: Illegal parameters"
 
    88     printk(KERN_ERR "EtherCAT: Illegal parameters for master_open()!\n");
 
    86            " for master_open()!\n");
 
    89     return -1;
 
    87     return -1;
 
    90   }
 
    88   }
 
    91 
    89 
    92   if (master->dev)
 
    90   if (master->dev) {
 
    93   {
 
    91     printk(KERN_ERR "EtherCAT: Master already"
 
    94     printk(KERN_ERR "EtherCAT: Master already has a device.\n");
 
    92            " has a device.\n");
 
    95     return -1;
 
    93     return -1;
 
    96   }
 
    94   }
 
    97 
    95 
    98   if (EtherCAT_device_open(device) < 0)
 
    96   if (EtherCAT_device_open(device) < 0) {
 
    99   {
 
       
   100     printk(KERN_ERR "EtherCAT: Could not open device %X!\n",
 
    97     printk(KERN_ERR "EtherCAT: Could not open device %X!\n",
 
   101            (unsigned int) master->dev);
 
    98            (unsigned int) master->dev);
 
   102     return -1;
 
    99     return -1;
 
   103   }
 
   100   }
 
   104 
   101 
   105   master->dev = device;
 
   102   master->dev = device;
 
   106 
   103 
   107   return 0;
 
   104   return 0;
 
   108 }
 
   105 }
 
   109 
   106 
   110 /***************************************************************/
 
   107 /*****************************************************************************/
 
   111 
   108 
   112 /**
 
   109 /**
 
   113    Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.
 
   110    Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.
 
   114 
   111 
   115    @param master Der EtherCAT-Master
 
   112    @param master Der EtherCAT-Master
 
   117 */
 
   114 */
 
   118 
   115 
   119 void EtherCAT_master_close(EtherCAT_master_t *master,
 
   116 void EtherCAT_master_close(EtherCAT_master_t *master,
 
   120                            EtherCAT_device_t *device)
 
   117                            EtherCAT_device_t *device)
 
   121 {
 
   118 {
 
   122   if (master->dev != device)
 
   119   if (master->dev != device) {
 
   123   {
 
   120     printk(KERN_WARNING "EtherCAT: Warning -"
 
   124     printk(KERN_WARNING "EtherCAT: Trying to close an unknown device!\n");
 
   121            " Trying to close an unknown device!\n");
 
   125     return;
 
   122     return;
 
   126   }
 
   123   }
 
   127 
   124 
   128   if (EtherCAT_device_close(master->dev) < 0)
 
   125   if (EtherCAT_device_close(master->dev) < 0) {
 
   129   {
 
   126     printk(KERN_WARNING "EtherCAT: Warning -"
 
   130     printk(KERN_WARNING "EtherCAT: Could not close device!\n");
 
   127            " Could not close device!\n");
 
   131   }
 
   128   }
 
   132 
   129 
   133   master->dev = NULL;
 
   130   master->dev = NULL;
 
   134 }
 
   131 }
 
   135 
   132 
   136 /***************************************************************/
 
   133 /*****************************************************************************/
 
   137 
   134 
   138 /**
 
   135 /**
 
   139    Sendet ein einzelnes Kommando in einem Frame und
 
   136    Sendet ein einzelnes Kommando in einem Frame und
 
   140    wartet auf dessen Empfang.
 
   137    wartet auf dessen Empfang.
 
   141 
   138 
   148 int EtherCAT_simple_send_receive(EtherCAT_master_t *master,
 
   145 int EtherCAT_simple_send_receive(EtherCAT_master_t *master,
 
   149                                  EtherCAT_command_t *cmd)
 
   146                                  EtherCAT_command_t *cmd)
 
   150 {
 
   147 {
 
   151   unsigned int tries_left;
 
   148   unsigned int tries_left;
 
   152 
   149 
   153   if (EtherCAT_simple_send(master, cmd) < 0) return -1;
 
   150   if (unlikely(EtherCAT_simple_send(master, cmd) < 0))
 
       
   151     return -1;
 
   154 
   152 
   155   udelay(3);
 
   153   udelay(3);
 
   156 
   154 
   157   EtherCAT_device_call_isr(master->dev);
 
   155   EtherCAT_device_call_isr(master->dev);
 
   158 
   156 
   159   tries_left = 20;
 
   157   tries_left = 20;
 
   160   while (master->dev->state == ECAT_DS_SENT && tries_left)
 
   158   while (unlikely(master->dev->state == ECAT_DS_SENT
 
   161   {
 
   159                   && tries_left)) {
 
   162     udelay(1);
 
   160     udelay(1);
 
   163     EtherCAT_device_call_isr(master->dev);
 
   161     EtherCAT_device_call_isr(master->dev);
 
   164     tries_left--;
 
   162     tries_left--;
 
   165   }
 
   163   }
 
   166 
   164 
   167   if (EtherCAT_simple_receive(master, cmd) < 0) return -1;
 
   165   if (unlikely(EtherCAT_simple_receive(master, cmd) < 0))
 
       
   166     return -1;
 
   168 
   167 
   169   return 0;
 
   168   return 0;
 
   170 }
 
   169 }
 
   171 
   170 
   172 /***************************************************************/
 
   171 /*****************************************************************************/
 
   173 
   172 
   174 /**
 
   173 /**
 
   175    Sendet ein einzelnes Kommando in einem Frame.
 
   174    Sendet ein einzelnes Kommando in einem Frame.
 
   176 
   175 
   177    @param master EtherCAT-Master
 
   176    @param master EtherCAT-Master
 
   183 int EtherCAT_simple_send(EtherCAT_master_t *master,
 
   182 int EtherCAT_simple_send(EtherCAT_master_t *master,
 
   184                          EtherCAT_command_t *cmd)
 
   183                          EtherCAT_command_t *cmd)
 
   185 {
 
   184 {
 
   186   unsigned int length, framelength, i;
 
   185   unsigned int length, framelength, i;
 
   187 
   186 
   188   if (master->debug_level > 0)
 
   187   if (unlikely(master->debug_level > 0)) {
 
   189   {
 
       
   190     printk(KERN_DEBUG "EtherCAT_send_receive_command\n");
 
   188     printk(KERN_DEBUG "EtherCAT_send_receive_command\n");
 
   191   }
 
   189   }
 
   192 
   190 
   193   if (cmd->state != ECAT_CS_READY)
 
   191   if (unlikely(cmd->state != ECAT_CS_READY)) {
 
   194   {
 
   192     printk(KERN_WARNING "EtherCAT_send_receive_command:"
 
   195     printk(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n");
 
   193            "Command not in ready state!\n");
 
   196   }
 
   194   }
 
   197 
   195 
   198   length = cmd->data_length + 12;
 
   196   length = cmd->data_length + 12;
 
   199   framelength = length + 2;
 
   197   framelength = length + 2;
 
   200 
   198 
   201   if (framelength > ECAT_FRAME_BUFFER_SIZE)
 
   199   if (unlikely(framelength > ECAT_FRAME_BUFFER_SIZE)) {
 
   202   {
 
   200     printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n",
 
   203     printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength);
 
   201            framelength);
 
   204     return -1;
 
   202     return -1;
 
   205   }
 
   203   }
 
   206 
   204 
   207   if (framelength < 46) framelength = 46;
 
   205   if (framelength < 46) framelength = 46;
 
   208 
   206 
   209   if (master->debug_level > 0)
 
   207   if (unlikely(master->debug_level > 0)) {
 
   210   {
 
       
   211     printk(KERN_DEBUG "Frame length: %i\n", framelength);
 
   208     printk(KERN_DEBUG "Frame length: %i\n", framelength);
 
   212   }
 
   209   }
 
   213 
   210 
   214   master->tx_data[0] = length & 0xFF;
 
   211   master->tx_data[0] = length & 0xFF;
 
   215   master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;
 
   212   master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;
 
   216 
   213 
   217   cmd->index = master->command_index;
 
   214   cmd->index = master->command_index;
 
   218   master->command_index = (master->command_index + 1) % 0x0100;
 
   215   master->command_index = (master->command_index + 1) % 0x0100;
 
   219 
   216 
   220   if (master->debug_level > 0)
 
   217   if (unlikely(master->debug_level > 0)) {
 
   221   {
 
       
   222     printk(KERN_DEBUG "Sending command index %i\n", cmd->index);
 
   218     printk(KERN_DEBUG "Sending command index %i\n", cmd->index);
 
   223   }
 
   219   }
 
   224 
   220 
   225   cmd->state = ECAT_CS_SENT;
 
   221   cmd->state = ECAT_CS_SENT;
 
   226 
   222 
   233   master->tx_data[2 + 6] = cmd->data_length & 0xFF;
 
   229   master->tx_data[2 + 6] = cmd->data_length & 0xFF;
 
   234   master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8;
 
   230   master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8;
 
   235   master->tx_data[2 + 8] = 0x00;
 
   231   master->tx_data[2 + 8] = 0x00;
 
   236   master->tx_data[2 + 9] = 0x00;
 
   232   master->tx_data[2 + 9] = 0x00;
 
   237 
   233 
   238   if (cmd->type == ECAT_CMD_APWR
 
   234   if (likely(cmd->type == ECAT_CMD_APWR
 
   239       || cmd->type == ECAT_CMD_NPWR
 
   235              || cmd->type == ECAT_CMD_NPWR
 
   240       || cmd->type == ECAT_CMD_BWR
 
   236              || cmd->type == ECAT_CMD_BWR
 
   241       || cmd->type == ECAT_CMD_LRW) // Write
 
   237              || cmd->type == ECAT_CMD_LRW)) // Write commands
 
   242   {
 
   238   {
 
   243     for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = cmd->data[i];
 
   239     for (i = 0; i < cmd->data_length; i++)
 
   244   }
 
   240       master->tx_data[2 + 10 + i] = cmd->data[i];
 
   245   else // Read
 
   241   }
 
       
   242   else // Read commands
 
   246   {
 
   243   {
 
   247     for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00;
 
   244     for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00;
 
   248   }
 
   245   }
 
   249 
   246 
   250   master->tx_data[2 + 10 + cmd->data_length] = 0x00;
 
   247   master->tx_data[2 + 10 + cmd->data_length] = 0x00;
 
   253   // Pad with zeros
 
   250   // Pad with zeros
 
   254   for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00;
 
   251   for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00;
 
   255 
   252 
   256   master->tx_data_length = framelength;
 
   253   master->tx_data_length = framelength;
 
   257 
   254 
   258   if (master->debug_level > 0)
 
   255   if (unlikely(master->debug_level > 0)) {
 
   259   {
 
       
   260     printk(KERN_DEBUG "device send...\n");
 
   256     printk(KERN_DEBUG "device send...\n");
 
   261   }
 
   257   }
 
   262 
   258 
   263   // Send frame
 
   259   // Send frame
 
   264   if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0)
 
   260   if (unlikely(EtherCAT_device_send(master->dev,
 
   265   {
 
   261                                     master->tx_data,
 
       
   262                                     framelength) != 0)) {
 
   266     printk(KERN_ERR "EtherCAT: Could not send!\n");
 
   263     printk(KERN_ERR "EtherCAT: Could not send!\n");
 
   267     return -1;
 
   264     return -1;
 
   268   }
 
   265   }
 
   269 
   266 
   270   if (master->debug_level > 0)
 
   267   if (unlikely(master->debug_level > 0)) {
 
   271   {
 
       
   272     printk(KERN_DEBUG "EtherCAT_send done.\n");
 
   268     printk(KERN_DEBUG "EtherCAT_send done.\n");
 
   273   }
 
   269   }
 
   274 
   270 
   275   return 0;
 
   271   return 0;
 
   276 }
 
   272 }
 
   277 
   273 
   278 /***************************************************************/
 
   274 /*****************************************************************************/
 
   279 
   275 
   280 /**
 
   276 /**
 
   281    Wartet auf den Empfang eines einzeln gesendeten
 
   277    Wartet auf den Empfang eines einzeln gesendeten
 
   282    Kommandos.
 
   278    Kommandos.
 
   283 
   279 
   289 
   285 
   290 int EtherCAT_simple_receive(EtherCAT_master_t *master,
 
   286 int EtherCAT_simple_receive(EtherCAT_master_t *master,
 
   291                             EtherCAT_command_t *cmd)
 
   287                             EtherCAT_command_t *cmd)
 
   292 {
 
   288 {
 
   293   unsigned int length;
 
   289   unsigned int length;
 
   294   int receive_ret;
 
   290   int ret;
 
   295   unsigned char command_type, command_index;
 
   291   unsigned char command_type, command_index;
 
   296 
   292 
   297   if ((receive_ret = EtherCAT_device_receive(master->dev,
 
   293   if (unlikely((ret = EtherCAT_device_receive(master->dev,
 
   298                                              master->rx_data)) < 0)
 
   294                                               master->rx_data)) < 0))
 
   299   {
 
   295     return -1;
 
   300     return -1;
 
   296 
   301   }
 
   297   master->rx_data_length = (unsigned int) ret;
 
   302 
   298 
   303   master->rx_data_length = (unsigned int) receive_ret;
 
   299   if (unlikely(master->rx_data_length < 2)) {
 
   304 
   300     printk(KERN_ERR "EtherCAT: Received frame with"
 
   305   if (master->rx_data_length < 2)
 
   301            " incomplete EtherCAT header!\n");
 
   306   {
 
       
   307     printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n");
 
       
   308     output_debug_data(master);
 
   302     output_debug_data(master);
 
   309     return -1;
 
   303     return -1;
 
   310   }
 
   304   }
 
   311 
   305 
   312   // Länge des gesamten Frames prüfen
 
   306   // Länge des gesamten Frames prüfen
 
   313   length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF);
 
   307   length = ((master->rx_data[1] & 0x07) << 8)
 
   314 
   308     | (master->rx_data[0] & 0xFF);
 
   315   if (length > master->rx_data_length)
 
   309 
   316   {
 
   310   if (unlikely(length > master->rx_data_length)) {
 
   317     printk(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n");
 
   311     printk(KERN_ERR "EtherCAT: Received corrupted"
 
       
   312            " frame (length does not match)!\n");
 
   318     output_debug_data(master);
 
   313     output_debug_data(master);
 
   319     return -1;
 
   314     return -1;
 
   320   }
 
   315   }
 
   321 
   316 
   322   command_type = master->rx_data[2];
 
   317   command_type = master->rx_data[2];
 
   323   command_index = master->rx_data[2 + 1];
 
   318   command_index = master->rx_data[2 + 1];
 
   324   length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8);
 
   319   length = (master->rx_data[2 + 6] & 0xFF)
 
   325 
   320     | ((master->rx_data[2 + 7] & 0x07) << 8);
 
   326   if (master->rx_data_length - 2 < length + 12)
 
   321 
   327   {
 
   322   if (unlikely(master->rx_data_length - 2 < length + 12)) {
 
   328     printk(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n");
 
   323     printk(KERN_ERR "EtherCAT: Received frame with"
 
       
   324            " incomplete command data!\n");
 
   329     output_debug_data(master);
 
   325     output_debug_data(master);
 
   330     return -1;
 
   326     return -1;
 
   331   }
 
   327   }
 
   332 
   328 
   333   if (cmd->state == ECAT_CS_SENT
 
   329   if (likely(cmd->state == ECAT_CS_SENT
 
   334       && cmd->type == command_type
 
   330              && cmd->type == command_type
 
   335       && cmd->index == command_index
 
   331              && cmd->index == command_index
 
   336       && cmd->data_length == length)
 
   332              && cmd->data_length == length))
 
   337   {
 
   333   {
 
   338     cmd->state = ECAT_CS_RECEIVED;
 
   334     cmd->state = ECAT_CS_RECEIVED;
 
   339 
   335 
   340     // Empfangene Daten in Kommandodatenspeicher kopieren
 
   336     // Empfangene Daten in Kommandodatenspeicher kopieren
 
   341     memcpy(cmd->data, master->rx_data + 2 + 10, length);
 
   337     memcpy(cmd->data, master->rx_data + 2 + 10, length);
 
   353   master->dev->state = ECAT_DS_READY;
 
   349   master->dev->state = ECAT_DS_READY;
 
   354 
   350 
   355   return 0;
 
   351   return 0;
 
   356 }
 
   352 }
 
   357 
   353 
   358 /***************************************************************/
 
   354 /*****************************************************************************/
 
   359 
   355 
   360 /**
 
   356 /**
 
   361    Überprüft die angeschlossenen Slaves.
 
   357    Überprüft die angeschlossenen Slaves.
 
   362 
   358 
   363    Vergleicht die an den Bus angeschlossenen Slaves mit
 
   359    Vergleicht die an den Bus angeschlossenen Slaves mit
 
   379   EtherCAT_command_t cmd;
 
   375   EtherCAT_command_t cmd;
 
   380   EtherCAT_slave_t *cur;
 
   376   EtherCAT_slave_t *cur;
 
   381   unsigned int i, j, found, length, pos;
 
   377   unsigned int i, j, found, length, pos;
 
   382   unsigned char data[2];
 
   378   unsigned char data[2];
 
   383 
   379 
   384   if (slave_count == 0)
 
   380   if (unlikely(!slave_count)) {
 
   385   {
 
       
   386     printk(KERN_ERR "EtherCAT: No slaves in list!\n");
 
   381     printk(KERN_ERR "EtherCAT: No slaves in list!\n");
 
   387     return -1;
 
   382     return -1;
 
   388   }
 
   383   }
 
   389 
   384 
   390   // Determine number of slaves on bus
 
   385   // Determine number of slaves on bus
 
   391 
   386 
   392   EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);
 
   387   EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);
 
   393 
   388 
   394   if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   389   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   395 
   390     return -1;
 
   396   if (cmd.working_counter != slave_count)
 
   391 
   397   {
 
   392   if (unlikely(cmd.working_counter != slave_count)) {
 
   398     printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",
 
   393     printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",
 
   399            cmd.working_counter, slave_count);
 
   394            cmd.working_counter, slave_count);
 
   400     return -1;
 
   395     return -1;
 
   401   }
 
   396   }
 
   402   else
 
   397   else
 
   403   {
 
       
   404     printk("EtherCAT: Found all %i slaves.\n", slave_count);
 
   398     printk("EtherCAT: Found all %i slaves.\n", slave_count);
 
   405   }
 
       
   406 
   399 
   407   // For every slave in the list
 
   400   // For every slave in the list
 
   408   for (i = 0; i < slave_count; i++)
 
   401   for (i = 0; i < slave_count; i++)
 
   409   {
 
   402   {
 
   410     cur = &slaves[i];
 
   403     cur = &slaves[i];
 
   411 
   404 
   412     if (!cur->desc)
 
   405     if (unlikely(!cur->desc)) {
 
   413     {
 
   406       printk(KERN_ERR "EtherCAT: Slave %i has"
 
   414       printk(KERN_ERR "EtherCAT: Slave %i has no description.\n", i);
 
   407              " no description.\n", i);
 
   415       return -1;
 
   408       return -1;
 
   416     }
 
   409     }
 
   417 
   410 
   418     // Set ring position
 
   411     // Set ring position
 
   419     cur->ring_position = -i;
 
   412     cur->ring_position = -i;
 
   422     // Write station address
 
   415     // Write station address
 
   423 
   416 
   424     data[0] = cur->station_address & 0x00FF;
 
   417     data[0] = cur->station_address & 0x00FF;
 
   425     data[1] = (cur->station_address & 0xFF00) >> 8;
 
   418     data[1] = (cur->station_address & 0xFF00) >> 8;
 
   426 
   419 
   427     EtherCAT_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data);
 
   420     EtherCAT_command_position_write(&cmd, cur->ring_position,
 
   428     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   421                                     0x0010, 2, data);
 
   429 
   422 
   430     if (cmd.working_counter != 1)
 
   423     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   431     {
 
   424       return -1;
 
   432       printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i);
 
   425 
       
   426     if (unlikely(cmd.working_counter != 1)) {
 
       
   427       printk(KERN_ERR "EtherCAT: Slave %i did not repond"
 
       
   428              " while writing station address!\n", i);
 
   433       return -1;
 
   429       return -1;
 
   434     }
 
   430     }
 
   435 
   431 
   436     // Read base data
 
   432     // Read base data
 
   437 
   433 
   438     EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);
 
   434     EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);
 
   439     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   435 
   440 
   436     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   441     if (cmd.working_counter != 1)
 
   437       return -1;
 
   442     {
 
   438 
   443       printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i);
 
   439     if (unlikely(cmd.working_counter != 1)) {
 
       
   440       printk(KERN_ERR "EtherCAT: Slave %i did not respond"
 
       
   441              " while reading base data!\n", i);
 
   444       return -1;
 
   442       return -1;
 
   445     }
 
   443     }
 
   446 
   444 
   447     // Get base data
 
   445     // Get base data
 
   448     cur->type = cmd.data[0];
 
   446     cur->type = cmd.data[0];
 
   449     cur->revision = cmd.data[1];
 
   447     cur->revision = cmd.data[1];
 
   450     cur->build = cmd.data[2] | (cmd.data[3] << 8);
 
   448     cur->build = cmd.data[2] | (cmd.data[3] << 8);
 
   451 
   449 
   452     // Read identification from "Slave Information Interface" (SII)
 
   450     // Read identification from "Slave Information Interface" (SII)
 
   453 
   451 
   454     if (EtherCAT_read_slave_information(master, cur->station_address,
 
   452     if (unlikely(EtherCAT_read_slave_information(master,
 
   455                                         0x0008, &cur->vendor_id) != 0)
 
   453                                                  cur->station_address,
 
       
   454                                                  0x0008,
 
       
   455                                                  &cur->vendor_id) != 0))
 
   456     {
 
   456     {
 
   457       printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n");
 
   457       printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n");
 
   458       return -1;
 
   458       return -1;
 
   459     }
 
   459     }
 
   460 
   460 
   461     if (EtherCAT_read_slave_information(master, cur->station_address,
 
   461     if (unlikely(EtherCAT_read_slave_information(master,
 
   462                                         0x000A, &cur->product_code) != 0)
 
   462                                                  cur->station_address,
 
       
   463                                                  0x000A,
 
       
   464                                                  &cur->product_code) != 0))
 
   463     {
 
   465     {
 
   464       printk(KERN_ERR "EtherCAT: Could not read SII product code!\n");
 
   466       printk(KERN_ERR "EtherCAT: Could not read SII product code!\n");
 
   465       return -1;
 
   467       return -1;
 
   466     }
 
   468     }
 
   467 
   469 
   468     if (EtherCAT_read_slave_information(master, cur->station_address,
 
   470     if (unlikely(EtherCAT_read_slave_information(master,
 
   469                                         0x000C, &cur->revision_number) != 0)
 
   471                                                  cur->station_address,
 
       
   472                                                  0x000C,
 
       
   473                                                  &cur->revision_number) != 0))
 
   470     {
 
   474     {
 
   471       printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n");
 
   475       printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n");
 
   472       return -1;
 
   476       return -1;
 
   473     }
 
   477     }
 
   474 
   478 
   475     if (EtherCAT_read_slave_information(master, cur->station_address,
 
   479     if (unlikely(EtherCAT_read_slave_information(master,
 
   476                                         0x000E, &cur->serial_number) != 0)
 
   480                                                  cur->station_address,
 
       
   481                                                  0x000E,
 
       
   482                                                  &cur->serial_number) != 0))
 
   477     {
 
   483     {
 
   478       printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n");
 
   484       printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n");
 
   479       return -1;
 
   485       return -1;
 
   480     }
 
   486     }
 
   481 
   487 
   482     // Search for identification in "database"
 
   488     // Search for identification in "database"
 
   483 
   489 
   484     found = 0;
 
   490     found = 0;
 
   485 
   491 
   486     for (j = 0; j < slave_idents_count; j++)
 
   492     for (j = 0; j < slave_ident_count; j++)
 
   487     {
 
   493     {
 
   488       if (slave_idents[j].vendor_id == cur->vendor_id
 
   494       if (unlikely(slave_idents[j].vendor_id == cur->vendor_id
 
   489           && slave_idents[j].product_code == cur->product_code)
 
   495                    && slave_idents[j].product_code == cur->product_code))
 
   490       {
 
   496       {
 
   491         found = 1;
 
   497         found = 1;
 
   492 
   498 
   493         if (cur->desc != slave_idents[j].desc)
 
   499         if (unlikely(cur->desc != slave_idents[j].desc)) {
 
   494         {
 
   500           printk(KERN_ERR "EtherCAT: Unexpected slave device"
 
   495           printk(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\""
 
   501                  " \"%s %s\" at position %i. Expected: \"%s %s\"\n",
 
   496                  " at position %i. Expected: \"%s %s\"\n",
 
       
   497                  slave_idents[j].desc->vendor_name,
 
   502                  slave_idents[j].desc->vendor_name,
 
   498                  slave_idents[j].desc->product_name, i,
 
   503                  slave_idents[j].desc->product_name, i,
 
   499                  cur->desc->vendor_name, cur->desc->product_name);
 
   504                  cur->desc->vendor_name, cur->desc->product_name);
 
   500           return -1;
 
   505           return -1;
 
   501         }
 
   506         }
 
   502 
   507 
   503         break;
 
   508         break;
 
   504       }
 
   509       }
 
   505     }
 
   510     }
 
   506 
   511 
   507     if (!found)
 
   512     if (unlikely(!found)) {
 
   508     {
 
       
   509       printk(KERN_ERR "EtherCAT: Unknown slave device"
 
   513       printk(KERN_ERR "EtherCAT: Unknown slave device"
 
   510              " (vendor %X, code %X) at position %i.\n",
 
   514              " (vendor %X, code %X) at position %i.\n",
 
   511              cur->vendor_id, cur->product_code, i);
 
   515              cur->vendor_id, cur->product_code, i);
 
   512       return -1;
 
   516       return -1;
 
   513     }
 
   517     }
 
   517   for (i = 0; i < slave_count; i++)
 
   521   for (i = 0; i < slave_count; i++)
 
   518   {
 
   522   {
 
   519     length += slaves[i].desc->data_length;
 
   523     length += slaves[i].desc->data_length;
 
   520   }
 
   524   }
 
   521 
   525 
   522   if ((master->process_data = (unsigned char *)
 
   526   if (unlikely((master->process_data = (unsigned char *)
 
   523        kmalloc(sizeof(unsigned char) * length, GFP_KERNEL)) == NULL)
 
   527                 kmalloc(sizeof(unsigned char)
 
   524   {
 
   528                         * length, GFP_KERNEL)) == NULL)) {
 
   525     printk(KERN_ERR "EtherCAT: Could not allocate %i bytes for process data.\n", length);
 
   529     printk(KERN_ERR "EtherCAT: Could not allocate %i"
 
       
   530            " bytes for process data.\n", length);
 
   526     return -1;
 
   531     return -1;
 
   527   }
 
   532   }
 
   528 
   533 
   529   master->process_data_length = length;
 
   534   master->process_data_length = length;
 
   530   memset(master->process_data, 0x00, length);
 
   535   memset(master->process_data, 0x00, length);
 
   533   for (i = 0; i < slave_count; i++)
 
   538   for (i = 0; i < slave_count; i++)
 
   534   {
 
   539   {
 
   535     slaves[i].process_data = master->process_data + pos;
 
   540     slaves[i].process_data = master->process_data + pos;
 
   536     slaves[i].logical_address0 = pos;
 
   541     slaves[i].logical_address0 = pos;
 
   537 
   542 
   538     printk(KERN_DEBUG "EtherCAT: Slave %i - Address 0x%X, \"%s %s\", s/n %u\n",
 
   543     printk(KERN_DEBUG "EtherCAT: Slave %i -"
 
   539            i, pos, slaves[i].desc->vendor_name, slaves[i].desc->product_name,
 
   544            " Address 0x%X, \"%s %s\", s/n %u\n",
 
       
   545            i, pos, slaves[i].desc->vendor_name,
 
       
   546            slaves[i].desc->product_name,
 
   540            slaves[i].serial_number);
 
   547            slaves[i].serial_number);
 
   541 
   548 
   542     pos += slaves[i].desc->data_length;
 
   549     pos += slaves[i].desc->data_length;
 
   543   }
 
   550   }
 
   544 
   551 
   546   master->slave_count = slave_count;
 
   553   master->slave_count = slave_count;
 
   547 
   554 
   548   return 0;
 
   555   return 0;
 
   549 }
 
   556 }
 
   550 
   557 
   551 /***************************************************************/
 
   558 /*****************************************************************************/
 
   552 
   559 
   553 /**
 
   560 /**
 
   554    Entfernt den Zeiger auf das Slave-Array.
 
   561    Entfernt den Zeiger auf das Slave-Array.
 
   555 
   562 
   556    @param master EtherCAT-Master
 
   563    @param master EtherCAT-Master
 
   560 {
 
   567 {
 
   561   master->slaves = NULL;
 
   568   master->slaves = NULL;
 
   562   master->slave_count = 0;
 
   569   master->slave_count = 0;
 
   563 }
 
   570 }
 
   564 
   571 
   565 /***************************************************************/
 
   572 /*****************************************************************************/
 
   566 
   573 
   567 /**
 
   574 /**
 
   568    Liest Daten aus dem Slave-Information-Interface
 
   575    Liest Daten aus dem Slave-Information-Interface
 
   569    eines EtherCAT-Slaves.
 
   576    eines EtherCAT-Slaves.
 
   570 
   577 
   594   data[3] = (offset & 0xFF00) >> 8;
 
   601   data[3] = (offset & 0xFF00) >> 8;
 
   595   data[4] = 0x00;
 
   602   data[4] = 0x00;
 
   596   data[5] = 0x00;
 
   603   data[5] = 0x00;
 
   597 
   604 
   598   EtherCAT_command_write(&cmd, node_address, 0x502, 6, data);
 
   605   EtherCAT_command_write(&cmd, node_address, 0x502, 6, data);
 
   599   if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -3;
 
   606 
   600 
   607   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   601   if (cmd.working_counter != 1)
 
   608     return -3;
 
   602   {
 
   609 
   603     printk(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n",
 
   610   if (unlikely(cmd.working_counter != 1)) {
 
   604            node_address);
 
   611     printk(KERN_ERR "EtherCAT: SII-read - Slave"
 
       
   612            " %04X did not respond!\n", node_address);
 
   605     return -4;
 
   613     return -4;
 
   606   }
 
   614   }
 
   607 
   615 
   608   // Der Slave legt die Informationen des Slave-Information-Interface
 
   616   // Der Slave legt die Informationen des Slave-Information-Interface
 
   609   // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange
 
   617   // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange
 
   610   // den Status auslesen, bis das Bit weg ist.
 
   618   // den Status auslesen, bis das Bit weg ist.
 
   611 
   619 
   612   tries_left = 100;
 
   620   tries_left = 100;
 
   613   while (tries_left)
 
   621   while (likely(tries_left))
 
   614   {
 
   622   {
 
   615     udelay(10);
 
   623     udelay(10);
 
   616 
   624 
   617     EtherCAT_command_read(&cmd, node_address, 0x502, 10);
 
   625     EtherCAT_command_read(&cmd, node_address, 0x502, 10);
 
   618     if (EtherCAT_simple_send_receive(master, &cmd) != 0) return -3;
 
   626 
   619 
   627     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0))
 
   620     if (cmd.working_counter != 1)
 
   628       return -3;
 
   621     {
 
   629 
   622       printk(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n",
 
   630     if (unlikely(cmd.working_counter != 1)) {
 
   623              node_address);
 
   631       printk(KERN_ERR "EtherCAT: SII-read status -"
 
       
   632              " Slave %04X did not respond!\n", node_address);
 
   624       return -4;
 
   633       return -4;
 
   625     }
 
   634     }
 
   626 
   635 
   627     if ((cmd.data[1] & 0x81) == 0)
 
   636     if (likely((cmd.data[1] & 0x81) == 0)) {
 
   628     {
 
       
   629       memcpy(target, cmd.data + 6, 4);
 
   637       memcpy(target, cmd.data + 6, 4);
 
   630       break;
 
   638       break;
 
   631     }
 
   639     }
 
   632 
   640 
   633     tries_left--;
 
   641     tries_left--;
 
   634   }
 
   642   }
 
   635 
   643 
   636   if (!tries_left)
 
   644   if (unlikely(!tries_left)) {
 
   637   {
 
   645     printk(KERN_WARNING "EtherCAT: SSI-read."
 
   638     printk(KERN_WARNING "EtherCAT: SSI-read. Slave %04X timed out!\n",
 
   646            " Slave %04X timed out!\n", node_address);
 
   639            node_address);
 
       
   640     return -1;
 
   647     return -1;
 
   641   }
 
   648   }
 
   642 
   649 
   643   return 0;
 
   650   return 0;
 
   644 }
 
   651 }
 
   645 
   652 
   646 /***************************************************************/
 
   653 /*****************************************************************************/
 
   647 
   654 
   648 /**
 
   655 /**
 
   649    Ändert den Zustand eines Slaves (asynchron).
 
   656    Ändert den Zustand eines Slaves (asynchron).
 
   650 
   657 
   651    Führt eine (asynchrone) Zustandsänderung bei einem Slave durch.
 
   658    Führt eine (asynchrone) Zustandsänderung bei einem Slave durch.
 
   667   unsigned int tries_left;
 
   674   unsigned int tries_left;
 
   668 
   675 
   669   data[0] = state_and_ack;
 
   676   data[0] = state_and_ack;
 
   670   data[1] = 0x00;
 
   677   data[1] = 0x00;
 
   671 
   678 
   672   EtherCAT_command_write(&cmd, slave->station_address, 0x0120, 2, data);
 
   679   EtherCAT_command_write(&cmd, slave->station_address,
 
   673 
   680                          0x0120, 2, data);
 
   674   if (EtherCAT_simple_send_receive(master, &cmd) != 0)
 
   681 
   675   {
 
   682   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) {
 
   676     printk(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack);
 
   683     printk(KERN_ERR "EtherCAT: Could not set state %02X -"
 
       
   684            " Unable to send!\n", state_and_ack);
 
   677     return -2;
 
   685     return -2;
 
   678   }
 
   686   }
 
   679 
   687 
   680   if (cmd.working_counter != 1)
 
   688   if (unlikely(cmd.working_counter != 1)) {
 
   681   {
 
   689     printk(KERN_ERR "EtherCAT: Could not set state %02X -"
 
   682     printk(KERN_ERR "EtherCAT: Could not set state %02X - Device \"%s %s\" (%d) did not respond!\n", 
   690            " Device \"%s %s\" (%d) did not respond!\n",
 
   683 	   state_and_ack,
 
   691            state_and_ack, slave->desc->vendor_name,
 
   684 	   slave->desc->vendor_name, 
   692            slave->desc->product_name, slave->ring_position*(-1));
 
   685 	   slave->desc->product_name,
 
       
   686 	   slave->ring_position*(-1));
 
       
   687     return -3;
 
   693     return -3;
 
   688   }
 
   694   }
 
   689 
   695 
   690   slave->requested_state = state_and_ack & 0x0F;
 
   696   slave->requested_state = state_and_ack & 0x0F;
 
   691 
   697 
   692   tries_left = 100;
 
   698   tries_left = 100;
 
   693   while (tries_left)
 
   699   while (likely(tries_left))
 
   694   {
 
   700   {
 
   695     udelay(10);
 
   701     udelay(10);
 
   696 
   702 
   697     EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2);
 
   703     EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2);
 
   698 
   704 
   699     if (EtherCAT_simple_send_receive(master, &cmd) != 0)
 
   705     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) {
 
   700     {
 
   706       printk(KERN_ERR "EtherCAT: Could not check"
 
   701       printk(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack);
 
   707              " state %02X - Unable to send!\n", state_and_ack);
 
   702       return -2;
 
   708       return -2;
 
   703     }
 
   709     }
 
   704 
   710 
   705     if (cmd.working_counter != 1)
 
   711     if (unlikely(cmd.working_counter != 1)) {
 
   706     {
 
   712       printk(KERN_ERR "EtherCAT: Could not check"
 
   707       printk(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack);
 
   713              " state %02X - Device did not respond!\n",
 
       
   714              state_and_ack);
 
   708       return -3;
 
   715       return -3;
 
   709     }
 
   716     }
 
   710 
   717 
   711     if (cmd.data[0] & 0x10) // State change error
 
   718     if (unlikely(cmd.data[0] & 0x10)) { // State change error
 
   712     {
 
   719       printk(KERN_ERR "EtherCAT: Could not set state %02X -"
 
   713       printk(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd.data[0]);
 
   720              " Device refused state change (code %02X)!\n",
 
       
   721              state_and_ack, cmd.data[0]);
 
   714       return -4;
 
   722       return -4;
 
   715     }
 
   723     }
 
   716 
   724 
   717     if (cmd.data[0] == (state_and_ack & 0x0F)) // State change successful
 
   725     if (likely(cmd.data[0] == (state_and_ack & 0x0F))) {
 
   718     {
 
   726       // State change successful
 
   719       break;
 
   727       break;
 
   720     }
 
   728     }
 
   721 
   729 
   722     tries_left--;
 
   730     tries_left--;
 
   723   }
 
   731   }
 
   724 
   732 
   725   if (!tries_left)
 
   733   if (unlikely(!tries_left)) {
 
   726   {
 
   734     printk(KERN_ERR "EtherCAT: Could not check state %02X -"
 
   727     printk(KERN_ERR "EtherCAT: Could not check state %02X - Timeout while checking!\n", state_and_ack);
 
   735            " Timeout while checking!\n", state_and_ack);
 
   728     return -5;
 
   736     return -5;
 
   729   }
 
   737   }
 
   730 
   738 
   731   slave->current_state = state_and_ack & 0x0F;
 
   739   slave->current_state = state_and_ack & 0x0F;
 
   732 
   740 
   733   return 0;
 
   741   return 0;
 
   734 }
 
   742 }
 
   735 
   743 
   736 /***************************************************************/
 
   744 /*****************************************************************************/
 
   737 
   745 
   738 /**
 
   746 /**
 
   739    Konfiguriert einen Slave und setzt den Operational-Zustand.
 
   747    Konfiguriert einen Slave und setzt den Operational-Zustand.
 
   740 
   748 
   741    Führt eine komplette Konfiguration eines Slaves durch,
 
   749    Führt eine komplette Konfiguration eines Slaves durch,
 
   756   unsigned char fmmu[16];
 
   764   unsigned char fmmu[16];
 
   757   unsigned char data[256];
 
   765   unsigned char data[256];
 
   758 
   766 
   759   desc = slave->desc;
 
   767   desc = slave->desc;
 
   760 
   768 
   761   if (EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0)
 
   769   if (unlikely(EtherCAT_state_change(master, slave,
 
   762   {
 
   770                                      ECAT_STATE_INIT) != 0))
 
   763     return -1;
 
   771     return -1;
 
   764   }
 
       
   765 
   772 
   766   // Resetting FMMU's
 
   773   // Resetting FMMU's
 
   767 
   774 
   768   memset(data, 0x00, 256);
 
   775   memset(data, 0x00, 256);
 
   769 
   776 
   770   EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 256, data);
 
   777   EtherCAT_command_write(&cmd, slave->station_address,
 
   771 
   778                          0x0600, 256, data);
 
   772   if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   779 
   773 
   780   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   774   if (cmd.working_counter != 1)
 
   781     return -1;
 
   775   {
 
   782 
   776     printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n",
 
   783   if (unlikely(cmd.working_counter != 1)) {
 
       
   784     printk(KERN_ERR "EtherCAT: Resetting FMMUs -"
 
       
   785            " Slave %04X did not respond!\n",
 
   777            slave->station_address);
 
   786            slave->station_address);
 
   778     return -2;
 
   787     return -2;
 
   779   }
 
   788   }
 
   780 
   789 
   781   // Resetting Sync Manager channels
 
   790   // Resetting Sync Manager channels
 
   782 
   791 
   783   if (desc->type != ECAT_ST_SIMPLE_NOSYNC)
 
   792   if (desc->type != ECAT_ST_SIMPLE_NOSYNC)
 
   784   {
 
   793   {
 
   785     memset(data, 0x00, 256);
 
   794     memset(data, 0x00, 256);
 
   786 
   795 
   787     EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data);
 
   796     EtherCAT_command_write(&cmd, slave->station_address,
 
   788     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   797                            0x0800, 256, data);
 
   789 
   798 
   790     if (cmd.working_counter != 1)
 
   799     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   791     {
 
   800       return -1;
 
   792       printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n",
 
   801 
       
   802     if (unlikely(cmd.working_counter != 1)) {
 
       
   803       printk(KERN_ERR "EtherCAT: Resetting SMs -"
 
       
   804              " Slave %04X did not respond!\n",
 
   793              slave->station_address);
 
   805              slave->station_address);
 
   794       return -2;
 
   806       return -2;
 
   795     }
 
   807     }
 
   796   }
 
   808   }
 
   797 
   809 
   799 
   811 
   800   if (desc->type == ECAT_ST_MAILBOX)
 
   812   if (desc->type == ECAT_ST_MAILBOX)
 
   801   {
 
   813   {
 
   802     if (desc->sm0)
 
   814     if (desc->sm0)
 
   803     {
 
   815     {
 
   804       EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0);
 
   816       EtherCAT_command_write(&cmd, slave->station_address,
 
   805       if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   817                              0x0800, 8, desc->sm0);
 
   806 
   818 
   807       if (cmd.working_counter != 1)
 
   819       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   808       {
 
   820         return -1;
 
   809         printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n",
 
   821 
       
   822       if (unlikely(cmd.working_counter != 1)) {
 
       
   823         printk(KERN_ERR "EtherCAT: Setting SM0 -"
 
       
   824                " Slave %04X did not respond!\n",
 
   810                slave->station_address);
 
   825                slave->station_address);
 
   811         return -3;
 
   826         return -3;
 
   812       }
 
   827       }
 
   813     }
 
   828     }
 
   814 
   829 
   815     if (desc->sm1)
 
   830     if (desc->sm1)
 
   816     {
 
   831     {
 
   817       EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1);
 
   832       EtherCAT_command_write(&cmd, slave->station_address,
 
   818       if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   833                              0x0808, 8, desc->sm1);
 
   819 
   834 
   820       if (cmd.working_counter != 1)
 
   835       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   821       {
 
   836         return -1;
 
   822         printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n",
 
   837 
       
   838       if (unlikely(cmd.working_counter != 1)) {
 
       
   839         printk(KERN_ERR "EtherCAT: Setting SM1 -"
 
       
   840                " Slave %04X did not respond!\n",
 
   823                slave->station_address);
 
   841                slave->station_address);
 
   824         return -2;
 
   842         return -2;
 
   825       }
 
   843       }
 
   826     }
 
   844     }
 
   827   }
 
   845   }
 
   828 
   846 
   829   // Change state to PREOP
 
   847   // Change state to PREOP
 
   830 
   848 
   831   if (EtherCAT_state_change(master, slave, ECAT_STATE_PREOP) != 0)
 
   849   if (unlikely(EtherCAT_state_change(master, slave,
 
   832   {
 
   850                                      ECAT_STATE_PREOP) != 0))
 
   833     return -5;
 
   851     return -5;
 
   834   }
 
       
   835 
   852 
   836   // Set FMMU's
 
   853   // Set FMMU's
 
   837 
   854 
   838   if (desc->fmmu0)
 
   855   if (desc->fmmu0)
 
   839   {
 
   856   {
 
   842     fmmu[0] = slave->logical_address0 & 0x000000FF;
 
   859     fmmu[0] = slave->logical_address0 & 0x000000FF;
 
   843     fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;
 
   860     fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;
 
   844     fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
 
   861     fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
 
   845     fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;
 
   862     fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;
 
   846 
   863 
   847     EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu);
 
   864     EtherCAT_command_write(&cmd, slave->station_address,
 
   848     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   865                            0x0600, 16, fmmu);
 
   849 
   866 
   850     if (cmd.working_counter != 1)
 
   867     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   851     {
 
   868       return -1;
 
   852       printk(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n",
 
   869 
       
   870     if (unlikely(cmd.working_counter != 1)) {
 
       
   871       printk(KERN_ERR "EtherCAT: Setting FMMU0 -"
 
       
   872              " Slave %04X did not respond!\n",
 
   853              slave->station_address);
 
   873              slave->station_address);
 
   854       return -2;
 
   874       return -2;
 
   855     }
 
   875     }
 
   856   }
 
   876   }
 
   857 
   877 
   859 
   879 
   860   if (desc->type != ECAT_ST_MAILBOX)
 
   880   if (desc->type != ECAT_ST_MAILBOX)
 
   861   {
 
   881   {
 
   862     if (desc->sm0)
 
   882     if (desc->sm0)
 
   863     {
 
   883     {
 
   864       EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0);
 
   884       EtherCAT_command_write(&cmd, slave->station_address,
 
   865       if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   885                              0x0800, 8, desc->sm0);
 
   866 
   886 
   867       if (cmd.working_counter != 1)
 
   887       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   868       {
 
   888         return -1;
 
   869         printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n",
 
   889 
       
   890       if (unlikely(cmd.working_counter != 1)) {
 
       
   891         printk(KERN_ERR "EtherCAT: Setting SM0 -"
 
       
   892                " Slave %04X did not respond!\n",
 
   870                slave->station_address);
 
   893                slave->station_address);
 
   871         return -3;
 
   894         return -3;
 
   872       }
 
   895       }
 
   873     }
 
   896     }
 
   874 
   897 
   875     if (desc->sm1)
 
   898     if (desc->sm1)
 
   876     {
 
   899     {
 
   877       EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1);
 
   900       EtherCAT_command_write(&cmd, slave->station_address,
 
   878       if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   901                              0x0808, 8, desc->sm1);
 
   879 
   902 
   880       if (cmd.working_counter != 1)
 
   903       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   881       {
 
   904         return -1;
 
   882         printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n",
 
   905 
       
   906       if (unlikely(cmd.working_counter != 1)) {
 
       
   907         printk(KERN_ERR "EtherCAT: Setting SM1 -"
 
       
   908                " Slave %04X did not respond!\n",
 
   883                slave->station_address);
 
   909                slave->station_address);
 
   884         return -3;
 
   910         return -3;
 
   885       }
 
   911       }
 
   886     }
 
   912     }
 
   887   }
 
   913   }
 
   888 
   914 
   889   if (desc->sm2)
 
   915   if (desc->sm2)
 
   890   {
 
   916   {
 
   891     EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2);
 
   917     EtherCAT_command_write(&cmd, slave->station_address,
 
   892     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   918                            0x0810, 8, desc->sm2);
 
   893 
   919 
   894     if (cmd.working_counter != 1)
 
   920     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   895     {
 
   921       return -1;
 
   896       printk(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n",
 
   922 
       
   923     if (unlikely(cmd.working_counter != 1)) {
 
       
   924       printk(KERN_ERR "EtherCAT: Setting SM2 -"
 
       
   925              " Slave %04X did not respond!\n",
 
   897              slave->station_address);
 
   926              slave->station_address);
 
   898       return -3;
 
   927       return -3;
 
   899     }
 
   928     }
 
   900   }
 
   929   }
 
   901 
   930 
   902   if (desc->sm3)
 
   931   if (desc->sm3)
 
   903   {
 
   932   {
 
   904     EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3);
 
   933     EtherCAT_command_write(&cmd, slave->station_address,
 
   905     if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;
 
   934                            0x0818, 8, desc->sm3);
 
   906 
   935 
   907     if (cmd.working_counter != 1)
 
   936     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
 
   908     {
 
   937       return -1;
 
   909       printk(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n",
 
   938 
       
   939     if (unlikely(cmd.working_counter != 1)) {
 
       
   940       printk(KERN_ERR "EtherCAT: Setting SM3 -"
 
       
   941              " Slave %04X did not respond!\n",
 
   910              slave->station_address);
 
   942              slave->station_address);
 
   911       return -3;
 
   943       return -3;
 
   912     }
 
   944     }
 
   913   }
 
   945   }
 
   914 
   946 
   915   // Change state to SAVEOP
 
   947   // Change state to SAVEOP
 
   916   if (EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0)
 
   948   if (unlikely(EtherCAT_state_change(master, slave,
 
   917   {
 
   949                                      ECAT_STATE_SAVEOP) != 0))
 
   918     return -12;
 
   950     return -12;
 
   919   }
 
       
   920 
   951 
   921   // Change state to OP
 
   952   // Change state to OP
 
   922   if (EtherCAT_state_change(master, slave, ECAT_STATE_OP) != 0)
 
   953   if (unlikely(EtherCAT_state_change(master, slave,
 
   923   {
 
   954                                      ECAT_STATE_OP) != 0))
 
   924     return -13;
 
   955     return -13;
 
   925   }
 
       
   926 
   956 
   927   return 0;
 
   957   return 0;
 
   928 }
 
   958 }
 
   929 
   959 
   930 /***************************************************************/
 
   960 /*****************************************************************************/
 
   931 
   961 
   932 /**
 
   962 /**
 
   933    Setzt einen Slave zurück in den Init-Zustand.
 
   963    Setzt einen Slave zurück in den Init-Zustand.
 
   934 
   964 
   935    @param master EtherCAT-Master
 
   965    @param master EtherCAT-Master
 
   937 
   967 
   938    @return 0 bei Erfolg, sonst < 0
 
   968    @return 0 bei Erfolg, sonst < 0
 
   939 */
 
   969 */
 
   940 
   970 
   941 int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
 
   971 int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
 
   942                             EtherCAT_slave_t *slave)
 
   972                               EtherCAT_slave_t *slave)
 
   943 {
 
   973 {
 
   944   if (EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0)
 
   974   if (unlikely(EtherCAT_state_change(master, slave,
 
   945   {
 
   975                                      ECAT_STATE_INIT) != 0))
 
   946     return -1;
 
   976     return -1;
 
   947   }
 
       
   948 
   977 
   949   return 0;
 
   978   return 0;
 
   950 }
 
   979 }
 
   951 
   980 
   952 /***************************************************************/
 
   981 /*****************************************************************************/
 
   953 
   982 
   954 /**
 
   983 /**
 
   955    Aktiviert alle Slaves.
 
   984    Aktiviert alle Slaves.
 
   956 
   985 
   957    @see EtherCAT_activate_slave
 
   986    @see EtherCAT_activate_slave
 
   965 {
 
   994 {
 
   966   unsigned int i;
 
   995   unsigned int i;
 
   967 
   996 
   968   for (i = 0; i < master->slave_count; i++)
 
   997   for (i = 0; i < master->slave_count; i++)
 
   969   {
 
   998   {
 
   970     if (EtherCAT_activate_slave(master, &master->slaves[i]) < 0)
 
   999     if (unlikely(EtherCAT_activate_slave(master,
 
   971     {
 
  1000                                          &master->slaves[i]) < 0))
 
   972       return -1;
 
  1001       return -1;
 
   973     }
 
       
   974   }
 
  1002   }
 
   975 
  1003 
   976   return 0;
 
  1004   return 0;
 
   977 }
 
  1005 }
 
   978 
  1006 
   979 /***************************************************************/
 
  1007 /*****************************************************************************/
 
   980 
  1008 
   981 /**
 
  1009 /**
 
   982    Deaktiviert alle Slaves.
 
  1010    Deaktiviert alle Slaves.
 
   983 
  1011 
   984    @see EtherCAT_deactivate_slave
 
  1012    @see EtherCAT_deactivate_slave
 
   993   unsigned int i;
 
  1021   unsigned int i;
 
   994   int ret = 0;
 
  1022   int ret = 0;
 
   995 
  1023 
   996   for (i = 0; i < master->slave_count; i++)
 
  1024   for (i = 0; i < master->slave_count; i++)
 
   997   {
 
  1025   {
 
   998     if (EtherCAT_deactivate_slave(master, &master->slaves[i]) < 0)
 
  1026     if (unlikely(EtherCAT_deactivate_slave(master,
 
   999     {
 
  1027                                            &master->slaves[i]) < 0))
 
  1000       ret = -1;
 
  1028       ret = -1;
 
  1001     }
 
       
  1002   }
 
  1029   }
 
  1003 
  1030 
  1004   return ret;
 
  1031   return ret;
 
  1005 }
 
  1032 }
 
  1006 
  1033 
  1007 /***************************************************************/
 
  1034 /*****************************************************************************/
 
  1008 
  1035 
  1009 /**
 
  1036 /**
 
  1010    Sendet alle Prozessdaten an die Slaves.
 
  1037    Sendet alle Prozessdaten an die Slaves.
 
  1011 
  1038 
  1012    Erstellt ein "logical read write"-Kommando mit den
 
  1039    Erstellt ein "logical read write"-Kommando mit den
 
  1021 {
 
  1048 {
 
  1022   EtherCAT_command_logical_read_write(&master->process_data_command,
 
  1049   EtherCAT_command_logical_read_write(&master->process_data_command,
 
  1023                                       0, master->process_data_length,
 
  1050                                       0, master->process_data_length,
 
  1024                                       master->process_data);
 
  1051                                       master->process_data);
 
  1025 
  1052 
  1026   if (EtherCAT_simple_send(master, &master->process_data_command) < 0)
 
  1053   if (unlikely(EtherCAT_simple_send(master,
 
  1027   {
 
  1054                                     &master->process_data_command) < 0))
 
  1028     printk(KERN_ERR "EtherCAT: Could not send process data command!\n");
 
  1055   {
 
       
  1056     printk(KERN_ERR "EtherCAT: Could not send"
 
       
  1057            " process data command!\n");
 
  1029     return -1;
 
  1058     return -1;
 
  1030   }
 
  1059   }
 
  1031 
  1060 
  1032   return 0;
 
  1061   return 0;
 
  1033 }
 
  1062 }
 
  1034 
  1063 
  1035 /***************************************************************/
 
  1064 /*****************************************************************************/
 
  1036 
  1065 
  1037 /**
 
  1066 /**
 
  1038    Empfängt alle Prozessdaten von den Slaves.
 
  1067    Empfängt alle Prozessdaten von den Slaves.
 
  1039 
  1068 
  1040    Empfängt ein zuvor gesendetes "logical read write"-Kommando
 
  1069    Empfängt ein zuvor gesendetes "logical read write"-Kommando
 
  1051   unsigned int tries_left;
 
  1080   unsigned int tries_left;
 
  1052 
  1081 
  1053   EtherCAT_device_call_isr(master->dev);
 
  1082   EtherCAT_device_call_isr(master->dev);
 
  1054 
  1083 
  1055   tries_left = 20;
 
  1084   tries_left = 20;
 
  1056   while (master->dev->state == ECAT_DS_SENT && tries_left)
 
  1085   while (unlikely(master->dev->state == ECAT_DS_SENT
 
       
  1086                   && tries_left))
 
  1057   {
 
  1087   {
 
  1058     udelay(1);
 
  1088     udelay(1);
 
  1059     EtherCAT_device_call_isr(master->dev);
 
  1089     EtherCAT_device_call_isr(master->dev);
 
  1060     tries_left--;
 
  1090     tries_left--;
 
  1061   }
 
  1091   }
 
  1062 
  1092 
  1063   if (!tries_left)
 
  1093   if (unlikely(!tries_left))
 
  1064   {
 
  1094   {
 
  1065     printk(KERN_ERR "EtherCAT: Timeout while receiving process data!\n");
 
  1095     printk(KERN_ERR "EtherCAT: Timeout while receiving process data!\n");
 
  1066     return -1;
 
  1096     return -1;
 
  1067   }
 
  1097   }
 
  1068 
  1098 
  1069   if (EtherCAT_simple_receive(master, &master->process_data_command) < 0)
 
  1099   if (unlikely(EtherCAT_simple_receive(master,
 
       
  1100                                        &master->process_data_command) < 0))
 
  1070   {
 
  1101   {
 
  1071     printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n");
 
  1102     printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n");
 
  1072     return -1;
 
  1103     return -1;
 
  1073   }
 
  1104   }
 
  1074 
  1105 
  1075   if (master->process_data_command.state != ECAT_CS_RECEIVED)
 
  1106   if (unlikely(master->process_data_command.state != ECAT_CS_RECEIVED))
 
  1076   {
 
  1107   {
 
  1077     printk(KERN_WARNING "EtherCAT: Process data command not received!\n");
 
  1108     printk(KERN_WARNING "EtherCAT: Process data command not received!\n");
 
  1078     return -1;
 
  1109     return -1;
 
  1079   }
 
  1110   }
 
  1080 
  1111 
  1083          master->process_data_length);
 
  1114          master->process_data_length);
 
  1084 
  1115 
  1085   return 0;
 
  1116   return 0;
 
  1086 }
 
  1117 }
 
  1087 
  1118 
  1088 /***************************************************************/
 
  1119 /*****************************************************************************/
 
  1089 
  1120 
  1090 /**
 
  1121 /**
 
  1091    Verwirft das zuletzt gesendete Prozessdatenkommando.
 
  1122    Verwirft das zuletzt gesendete Prozessdatenkommando.
 
  1092 
  1123 
  1093    @param master EtherCAT-Master
 
  1124    @param master EtherCAT-Master
 
  1097 {
 
  1128 {
 
  1098   EtherCAT_device_call_isr(master->dev);
 
  1129   EtherCAT_device_call_isr(master->dev);
 
  1099   master->dev->state = ECAT_DS_READY;
 
  1130   master->dev->state = ECAT_DS_READY;
 
  1100 }
 
  1131 }
 
  1101 
  1132 
  1102 /***************************************************************/
 
  1133 /*****************************************************************************/
 
  1103 
  1134 
  1104 /**
 
  1135 /**
 
  1105    Gibt Frame-Inhalte zwecks Debugging aus.
 
  1136    Gibt Frame-Inhalte zwecks Debugging aus.
 
  1106 
  1137 
  1107    @param master EtherCAT-Master
 
  1138    @param master EtherCAT-Master
 
  1132     if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG);
 
  1163     if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG);
 
  1133   }
 
  1164   }
 
  1134   printk("\n");
 
  1165   printk("\n");
 
  1135 }
 
  1166 }
 
  1136 
  1167 
  1137 /***************************************************************/
 
  1168 /*****************************************************************************/
 
  1138 
  1169 
  1139 EXPORT_SYMBOL(EtherCAT_master_init);
 
  1170 EXPORT_SYMBOL(EtherCAT_master_init);
 
  1140 EXPORT_SYMBOL(EtherCAT_master_clear);
 
  1171 EXPORT_SYMBOL(EtherCAT_master_clear);
 
  1141 EXPORT_SYMBOL(EtherCAT_master_open);
 
  1172 EXPORT_SYMBOL(EtherCAT_master_open);
 
  1142 EXPORT_SYMBOL(EtherCAT_master_close);
 
  1173 EXPORT_SYMBOL(EtherCAT_master_close);
 
  1145 EXPORT_SYMBOL(EtherCAT_check_slaves);
 
  1176 EXPORT_SYMBOL(EtherCAT_check_slaves);
 
  1146 EXPORT_SYMBOL(EtherCAT_activate_all_slaves);
 
  1177 EXPORT_SYMBOL(EtherCAT_activate_all_slaves);
 
  1147 EXPORT_SYMBOL(EtherCAT_clear_process_data);
 
  1178 EXPORT_SYMBOL(EtherCAT_clear_process_data);
 
  1148 EXPORT_SYMBOL(EtherCAT_deactivate_all_slaves);
 
  1179 EXPORT_SYMBOL(EtherCAT_deactivate_all_slaves);
 
  1149 
  1180 
  1150 /***************************************************************/
 
  1181 /*****************************************************************************/
etherlabmaster