1 /******************************************************************************

     2  *

     3  *  e c _ m a s t e r . c

     4  *

     5  *  Methoden für einen EtherCAT-Master.

     6  *

     7  *  $Id$

     8  *

     9  *****************************************************************************/

    10 

    11 #include <linux/module.h>

    12 #include <linux/kernel.h>

    13 #include <linux/string.h>

    14 #include <linux/slab.h>

    15 #include <linux/delay.h>

    16 

    17 #include "ec_globals.h"

    18 #include "ec_master.h"

    19 

    20 /*****************************************************************************/

    21 

    22 /**

    23    Konstruktor des EtherCAT-Masters.

    24 

    25    @param master Zeiger auf den zu initialisierenden EtherCAT-Master

    26 */

    27 

    28 void EtherCAT_master_init(EtherCAT_master_t *master)

    29 {

    30   master->bus_slaves = NULL;

    31   master->bus_slaves_count = 0;

    32   master->dev = NULL;

    33   master->command_index = 0x00;

    34   master->tx_data_length = 0;

    35   master->rx_data_length = 0;

    36   master->domain_count = 0;

    37   master->debug_level = 0;

    38   master->bus_time = 0;

    39   master->frames_lost = 0;

    40   master->t_lost_output = 0;

    41 }

    42 

    43 /*****************************************************************************/

    44 

    45 /**

    46    Destruktor eines EtherCAT-Masters.

    47 

    48    Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    49    auf das Slave-Array und gibt die Prozessdaten frei.

    50 

    51    @param master Zeiger auf den zu löschenden Master

    52 */

    53 

    54 void EtherCAT_master_clear(EtherCAT_master_t *master)

    55 {

    56   if (master->bus_slaves) {

    57     kfree(master->bus_slaves);

    58     master->bus_slaves = NULL;

    59   }

    60 

    61   master->domain_count = 0;

    62 }

    63 

    64 /*****************************************************************************/

    65 

    66 /**

    67    Öffnet ein EtherCAT-Geraet für den Master.

    68 

    69    Registriert das Geraet beim Master, der es daraufhin oeffnet.

    70 

    71    @param master Der EtherCAT-Master

    72    @param device Das EtherCAT-Geraet

    73    @return 0, wenn alles o.k.,

    74    < 0, wenn bereits ein Geraet registriert

    75    oder das Geraet nicht geoeffnet werden konnte.

    76 */

    77 

    78 int EtherCAT_master_open(EtherCAT_master_t *master,

    79                          EtherCAT_device_t *device)

    80 {

    81   if (!master || !device) {

    82     printk(KERN_ERR "EtherCAT: Illegal parameters for master_open()!\n");

    83     return -1;

    84   }

    85 

    86   if (master->dev) {

    87     printk(KERN_ERR "EtherCAT: Master already has a device.\n");

    88     return -1;

    89   }

    90 

    91   if (EtherCAT_device_open(device) < 0) {

    92     printk(KERN_ERR "EtherCAT: Could not open device %X!\n",

    93            (unsigned int) master->dev);

    94     return -1;

    95   }

    96 

    97   master->dev = device;

    98 

    99   return 0;

   100 }

   101 

   102 /*****************************************************************************/

   103 

   104 /**

   105    Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   106 

   107    @param master Der EtherCAT-Master

   108    @param device Das EtherCAT-Geraet

   109 */

   110 

   111 void EtherCAT_master_close(EtherCAT_master_t *master,

   112                            EtherCAT_device_t *device)

   113 {

   114   if (master->dev != device) {

   115     printk(KERN_WARNING "EtherCAT: Warning -"

   116            " Trying to close an unknown device!\n");

   117     return;

   118   }

   119 

   120   if (EtherCAT_device_close(master->dev) < 0) {

   121     printk(KERN_WARNING "EtherCAT: Warning -"

   122            " Could not close device!\n");

   123   }

   124 

   125   master->dev = NULL;

   126 }

   127 

   128 /*****************************************************************************/

   129 

   130 /**

   131    Sendet ein einzelnes Kommando in einem Frame und

   132    wartet auf dessen Empfang.

   133 

   134    @param master EtherCAT-Master

   135    @param cmd    Kommando zum Senden/Empfangen

   136 

   137    @return 0 bei Erfolg, sonst < 0

   138 */

   139 

   140 int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

   141                                  EtherCAT_command_t *cmd)

   142 {

   143   unsigned int tries_left;

   144 

   145   if (unlikely(EtherCAT_simple_send(master, cmd) < 0))

   146     return -1;

   147 

   148   udelay(3);

   149 

   150   EtherCAT_device_call_isr(master->dev);

   151 

   152   tries_left = 20;

   153   while (unlikely(master->dev->state == ECAT_DS_SENT

   154                   && tries_left)) {

   155     udelay(1);

   156     EtherCAT_device_call_isr(master->dev);

   157     tries_left--;

   158   }

   159 

   160   if (unlikely(EtherCAT_simple_receive(master, cmd) < 0))

   161     return -1;

   162 

   163   return 0;

   164 }

   165 

   166 /*****************************************************************************/

   167 

   168 /**

   169    Sendet ein einzelnes Kommando in einem Frame.

   170 

   171    @param master EtherCAT-Master

   172    @param cmd    Kommando zum Senden

   173 

   174    @return 0 bei Erfolg, sonst < 0

   175 */

   176 

   177 int EtherCAT_simple_send(EtherCAT_master_t *master,

   178                          EtherCAT_command_t *cmd)

   179 {

   180   unsigned int length, framelength, i;

   181 

   182   if (unlikely(master->debug_level > 0)) {

   183     printk(KERN_DEBUG "EtherCAT_send_receive_command\n");

   184   }

   185 

   186   if (unlikely(cmd->state != ECAT_CS_READY)) {

   187     printk(KERN_WARNING "EtherCAT_send_receive_command:"

   188            "Command not in ready state!\n");

   189   }

   190 

   191   length = cmd->data_length + 12;

   192   framelength = length + 2;

   193 

   194   if (unlikely(framelength > ECAT_FRAME_BUFFER_SIZE)) {

   195     printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength);

   196     return -1;

   197   }

   198 

   199   if (framelength < 46) framelength = 46;

   200 

   201   if (unlikely(master->debug_level > 0)) {

   202     printk(KERN_DEBUG "Frame length: %i\n", framelength);

   203   }

   204 

   205   master->tx_data[0] = length & 0xFF;

   206   master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;

   207 

   208   cmd->index = master->command_index;

   209   master->command_index = (master->command_index + 1) % 0x0100;

   210 

   211   if (unlikely(master->debug_level > 0)) {

   212     printk(KERN_DEBUG "Sending command index %i\n", cmd->index);

   213   }

   214 

   215   cmd->state = ECAT_CS_SENT;

   216 

   217   master->tx_data[2 + 0] = cmd->type;

   218   master->tx_data[2 + 1] = cmd->index;

   219   master->tx_data[2 + 2] = cmd->address.raw[0];

   220   master->tx_data[2 + 3] = cmd->address.raw[1];

   221   master->tx_data[2 + 4] = cmd->address.raw[2];

   222   master->tx_data[2 + 5] = cmd->address.raw[3];

   223   master->tx_data[2 + 6] = cmd->data_length & 0xFF;

   224   master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8;

   225   master->tx_data[2 + 8] = 0x00;

   226   master->tx_data[2 + 9] = 0x00;

   227 

   228   if (likely(cmd->type == ECAT_CMD_APWR

   229              || cmd->type == ECAT_CMD_NPWR

   230              || cmd->type == ECAT_CMD_BWR

   231              || cmd->type == ECAT_CMD_LRW)) // Write commands

   232   {

   233     for (i = 0; i < cmd->data_length; i++)

   234       master->tx_data[2 + 10 + i] = cmd->data[i];

   235   }

   236   else // Read commands

   237   {

   238     for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00;

   239   }

   240 

   241   master->tx_data[2 + 10 + cmd->data_length] = 0x00;

   242   master->tx_data[2 + 11 + cmd->data_length] = 0x00;

   243 

   244   // Pad with zeros

   245   for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00;

   246 

   247   master->tx_data_length = framelength;

   248 

   249   if (unlikely(master->debug_level > 0)) {

   250     printk(KERN_DEBUG "device send...\n");

   251   }

   252 

   253   // Send frame

   254   if (unlikely(EtherCAT_device_send(master->dev,

   255                                     master->tx_data,

   256                                     framelength) != 0)) {

   257     printk(KERN_ERR "EtherCAT: Could not send!\n");

   258     return -1;

   259   }

   260 

   261   if (unlikely(master->debug_level > 0)) {

   262     printk(KERN_DEBUG "EtherCAT_send done.\n");

   263   }

   264 

   265   return 0;

   266 }

   267 

   268 /*****************************************************************************/

   269 

   270 /**

   271    Wartet auf den Empfang eines einzeln gesendeten

   272    Kommandos.

   273 

   274    @param master EtherCAT-Master

   275    @param cmd    Gesendetes Kommando

   276 

   277    @return 0 bei Erfolg, sonst < 0

   278 */

   279 

   280 int EtherCAT_simple_receive(EtherCAT_master_t *master,

   281                             EtherCAT_command_t *cmd)

   282 {

   283   unsigned int length;

   284   int ret;

   285   unsigned char command_type, command_index;

   286 

   287   if (unlikely((ret = EtherCAT_device_receive(master->dev,

   288                                               master->rx_data)) < 0))

   289     return -1;

   290 

   291   master->rx_data_length = (unsigned int) ret;

   292 

   293   if (unlikely(master->rx_data_length < 2)) {

   294     printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT"

   295            " header!\n");

   296     output_debug_data(master);

   297     return -1;

   298   }

   299 

   300   // Länge des gesamten Frames prüfen

   301   length = ((master->rx_data[1] & 0x07) << 8)

   302     | (master->rx_data[0] & 0xFF);

   303 

   304   if (unlikely(length > master->rx_data_length)) {

   305     printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"

   306            " not match)!\n");

   307     output_debug_data(master);

   308     return -1;

   309   }

   310 

   311   command_type = master->rx_data[2];

   312   command_index = master->rx_data[2 + 1];

   313   length = (master->rx_data[2 + 6] & 0xFF)

   314     | ((master->rx_data[2 + 7] & 0x07) << 8);

   315 

   316   if (unlikely(master->rx_data_length - 2 < length + 12)) {

   317     printk(KERN_ERR "EtherCAT: Received frame with"

   318            " incomplete command data!\n");

   319     output_debug_data(master);

   320     return -1;

   321   }

   322 

   323   if (likely(cmd->state == ECAT_CS_SENT

   324              && cmd->type == command_type

   325              && cmd->index == command_index

   326              && cmd->data_length == length))

   327   {

   328     cmd->state = ECAT_CS_RECEIVED;

   329 

   330     // Empfangene Daten in Kommandodatenspeicher kopieren

   331     memcpy(cmd->data, master->rx_data + 2 + 10, length);

   332 

   333     // Working-Counter setzen

   334     cmd->working_counter

   335       = ((master->rx_data[length + 2 + 10] & 0xFF)

   336          | ((master->rx_data[length + 2 + 11] & 0xFF) << 8));

   337 

   338     if (unlikely(master->debug_level > 1)) {

   339       output_debug_data(master);

   340     }

   341   }

   342   else

   343   {

   344     printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");

   345     output_debug_data(master);

   346   }

   347 

   348   master->dev->state = ECAT_DS_READY;

   349 

   350   return 0;

   351 }

   352 

   353 /*****************************************************************************/

   354 

   355 /**

   356    Durchsucht den Bus nach Slaves.

   357 

   358    @param master Der EtherCAT-Master

   359 

   360    @return 0 bei Erfolg, sonst < 0

   361 */

   362 

   363 int EtherCAT_scan_for_slaves(EtherCAT_master_t *master)

   364 {

   365   EtherCAT_command_t cmd;

   366   EtherCAT_slave_t *cur;

   367   unsigned int i, j;

   368   unsigned char data[2];

   369 

   370   // Determine number of slaves on bus

   371 

   372   EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   373 

   374   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   375     return -1;

   376 

   377   master->bus_slaves_count = cmd.working_counter;

   378   printk("EtherCAT: Found %i slaves on bus.\n", master->bus_slaves_count);

   379 

   380   if (!master->bus_slaves_count) return 0;

   381 

   382   if (!(master->bus_slaves =

   383         (EtherCAT_slave_t *) kmalloc(master->bus_slaves_count *

   384                                      sizeof(EtherCAT_slave_t), GFP_KERNEL))) {

   385     printk(KERN_ERR "EtherCAT: Could not allocate memory for bus slaves!\n");

   386     return -1;

   387   }

   388 

   389   // For every slave in the list

   390   for (i = 0; i < master->bus_slaves_count; i++)

   391   {

   392     cur = master->bus_slaves + i;

   393 

   394     EtherCAT_slave_init(cur);

   395 

   396     // Read base data

   397 

   398     EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);

   399 

   400     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   401       return -1;

   402 

   403     if (unlikely(cmd.working_counter != 1)) {

   404       printk(KERN_ERR "EtherCAT: Slave %i did not respond"

   405              " while reading base data!\n", i);

   406       return -1;

   407     }

   408 

   409     // Get base data

   410 

   411     cur->type = cmd.data[0];

   412     cur->revision = cmd.data[1];

   413     cur->build = cmd.data[2] | (cmd.data[3] << 8);

   414 

   415     // Read identification from "Slave Information Interface" (SII)

   416 

   417     if (unlikely(EtherCAT_read_slave_information(master,

   418                                                  cur->station_address,

   419                                                  0x0008,

   420                                                  &cur->vendor_id) != 0))

   421     {

   422       printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n");

   423       return -1;

   424     }

   425 

   426     if (unlikely(EtherCAT_read_slave_information(master,

   427                                                  cur->station_address,

   428                                                  0x000A,

   429                                                  &cur->product_code) != 0))

   430     {

   431       printk(KERN_ERR "EtherCAT: Could not read SII product code!\n");

   432       return -1;

   433     }

   434 

   435     if (unlikely(EtherCAT_read_slave_information(master,

   436                                                  cur->station_address,

   437                                                  0x000C,

   438                                                  &cur->revision_number) != 0))

   439     {

   440       printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n");

   441       return -1;

   442     }

   443 

   444     if (unlikely(EtherCAT_read_slave_information(master,

   445                                                  cur->station_address,

   446                                                  0x000E,

   447                                                  &cur->serial_number) != 0))

   448     {

   449       printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n");

   450       return -1;

   451     }

   452 

   453     // Search for identification in "database"

   454 

   455     for (j = 0; j < slave_ident_count; j++)

   456     {

   457       if (unlikely(slave_idents[j].vendor_id == cur->vendor_id

   458                    && slave_idents[j].product_code == cur->product_code))

   459       {

   460         cur->desc = slave_idents[j].desc;

   461         break;

   462       }

   463     }

   464 

   465     if (unlikely(!cur->desc)) {

   466       printk(KERN_ERR "EtherCAT: Unknown slave device (vendor %X, code %X) at "

   467              " position %i.\n", cur->vendor_id, cur->product_code, i);

   468       return -1;

   469     }

   470 

   471     // Set ring position

   472     cur->ring_position = -i;

   473     cur->station_address = i + 1;

   474 

   475     // Write station address

   476 

   477     data[0] = cur->station_address & 0x00FF;

   478     data[1] = (cur->station_address & 0xFF00) >> 8;

   479 

   480     EtherCAT_command_position_write(&cmd, cur->ring_position,

   481                                     0x0010, 2, data);

   482 

   483     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   484       return -1;

   485 

   486     if (unlikely(cmd.working_counter != 1)) {

   487       printk(KERN_ERR "EtherCAT: Slave %i did not repond"

   488              " while writing station address!\n", i);

   489       return -1;

   490     }

   491   }

   492 

   493   return 0;

   494 }

   495 

   496 /*****************************************************************************/

   497 

   498 /**

   499    Registriert einen Slave beim Master.

   500 

   501    @param master Der EtherCAT-Master

   502 

   503    @return 0 bei Erfolg, sonst < 0

   504 */

   505 

   506 void *EtherCAT_register_slave(EtherCAT_master_t *master,

   507                               unsigned int bus_index,

   508                               const char *vendor_name,

   509                               const char *product_name,

   510                               unsigned int domain)

   511 {

   512   EtherCAT_slave_t *slave;

   513   EtherCAT_domain_t *dom;

   514   unsigned int j;

   515 

   516   if (bus_index >= master->bus_slaves_count) {

   517     printk(KERN_ERR "EtherCAT: Illegal bus index! (%i / %i)\n", bus_index,

   518            master->bus_slaves_count);

   519     return NULL;

   520   }

   521 

   522   slave = master->bus_slaves + bus_index;

   523 

   524   if (slave->process_data) {

   525     printk(KERN_ERR "EtherCAT: Slave %i is already registered!\n", bus_index);

   526     return NULL;

   527   }

   528 

   529   if (strcmp(vendor_name, slave->desc->vendor_name) ||

   530       strcmp(product_name, slave->desc->product_name)) {

   531     printk(KERN_ERR "Invalid Slave Type! Requested: \"%s %s\", present: \"%s"

   532            "%s\".\n", vendor_name, product_name, slave->desc->vendor_name,

   533            slave->desc->product_name);

   534     return NULL;

   535   }

   536 

   537   // Check, if process data domain already exists...

   538   dom = NULL;

   539   for (j = 0; j < master->domain_count; j++) {

   540     if (domain == master->domains[j].number) {

   541       dom = master->domains + j;

   542     }

   543   }

   544 

   545   // Create process data domain

   546   if (!dom) {

   547     if (master->domain_count > ECAT_MAX_DOMAINS - 1) {

   548       printk(KERN_ERR "EtherCAT: Too many domains!\n");

   549       return NULL;

   550     }

   551 

   552     dom = master->domains + master->domain_count;

   553     EtherCAT_domain_init(dom);

   554     dom->number = domain;

   555     dom->logical_offset = master->domain_count * ECAT_FRAME_BUFFER_SIZE;

   556     master->domain_count++;

   557   }

   558 

   559   if (dom->data_size + slave->desc->process_data_size

   560       > ECAT_FRAME_BUFFER_SIZE - 14) {

   561     printk(KERN_ERR "EtherCAT: Oversized domain %i: %i / %i Bytes!\n",

   562            dom->number, dom->data_size + slave->desc->process_data_size,

   563            ECAT_FRAME_BUFFER_SIZE - 14);

   564     return NULL;

   565   }

   566 

   567   slave->process_data = dom->data + dom->data_size;

   568   dom->data_size += slave->desc->process_data_size;

   569 

   570   return slave->process_data;

   571 }

   572 

   573 /*****************************************************************************/

   574 

   575 /**

   576    Liest Daten aus dem Slave-Information-Interface

   577    eines EtherCAT-Slaves.

   578 

   579    @param master EtherCAT-Master

   580    @param node_address Knotenadresse des Slaves

   581    @param offset Adresse des zu lesenden SII-Registers

   582    @param target Zeiger auf einen 4 Byte großen Speicher

   583    zum Ablegen der Daten

   584 

   585    @return 0 bei Erfolg, sonst < 0

   586 */

   587 

   588 int EtherCAT_read_slave_information(EtherCAT_master_t *master,

   589                                     unsigned short int node_address,

   590                                     unsigned short int offset,

   591                                     unsigned int *target)

   592 {

   593   EtherCAT_command_t cmd;

   594   unsigned char data[10];

   595   unsigned int tries_left;

   596 

   597   // Initiate read operation

   598 

   599   data[0] = 0x00;

   600   data[1] = 0x01;

   601   data[2] = offset & 0xFF;

   602   data[3] = (offset & 0xFF00) >> 8;

   603   data[4] = 0x00;

   604   data[5] = 0x00;

   605 

   606   EtherCAT_command_write(&cmd, node_address, 0x502, 6, data);

   607 

   608   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   609     return -1;

   610 

   611   if (unlikely(cmd.working_counter != 1)) {

   612     printk(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n",

   613            node_address);

   614     return -1;

   615   }

   616 

   617   // Der Slave legt die Informationen des Slave-Information-Interface

   618   // in das Datenregister und löscht daraufhin ein Busy-Bit. Solange

   619   // den Status auslesen, bis das Bit weg ist.

   620 

   621   tries_left = 100;

   622   while (likely(tries_left))

   623   {

   624     udelay(10);

   625 

   626     EtherCAT_command_read(&cmd, node_address, 0x502, 10);

   627 

   628     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0))

   629       return -1;

   630 

   631     if (unlikely(cmd.working_counter != 1)) {

   632       printk(KERN_ERR "EtherCAT: SII-read status -"

   633              " Slave %04X did not respond!\n", node_address);

   634       return -1;

   635     }

   636 

   637     if (likely((cmd.data[1] & 0x81) == 0)) {

   638       memcpy(target, cmd.data + 6, 4);

   639       break;

   640     }

   641 

   642     tries_left--;

   643   }

   644 

   645   if (unlikely(!tries_left)) {

   646     printk(KERN_WARNING "EtherCAT: SSI-read. Slave %04X timed out!\n",

   647            node_address);

   648     return -1;

   649   }

   650 

   651   return 0;

   652 }

   653 

   654 /*****************************************************************************/

   655 

   656 /**

   657    Ändert den Zustand eines Slaves (asynchron).

   658 

   659    Führt eine (asynchrone) Zustandsänderung bei einem Slave durch.

   660 

   661    @param master EtherCAT-Master

   662    @param slave Slave, dessen Zustand geändert werden soll

   663    @param state_and_ack Neuer Zustand, evtl. mit gesetztem

   664    Acknowledge-Flag

   665 

   666    @return 0 bei Erfolg, sonst < 0

   667 */

   668 

   669 int EtherCAT_state_change(EtherCAT_master_t *master,

   670                           EtherCAT_slave_t *slave,

   671                           unsigned char state_and_ack)

   672 {

   673   EtherCAT_command_t cmd;

   674   unsigned char data[2];

   675   unsigned int tries_left;

   676 

   677   data[0] = state_and_ack;

   678   data[1] = 0x00;

   679 

   680   EtherCAT_command_write(&cmd, slave->station_address,

   681                          0x0120, 2, data);

   682 

   683   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) {

   684     printk(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n",

   685            state_and_ack);

   686     return -1;

   687   }

   688 

   689   if (unlikely(cmd.working_counter != 1)) {

   690     printk(KERN_ERR "EtherCAT: Could not set state %02X - Device \"%s %s\""

   691            " (%d) did not respond!\n", state_and_ack, slave->desc->vendor_name,

   692            slave->desc->product_name, slave->ring_position * (-1));

   693     return -1;

   694   }

   695 

   696   slave->requested_state = state_and_ack & 0x0F;

   697 

   698   tries_left = 100;

   699   while (likely(tries_left))

   700   {

   701     udelay(10);

   702 

   703     EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2);

   704 

   705     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) != 0)) {

   706       printk(KERN_ERR "EtherCAT: Could not check state %02X - Unable to"

   707              " send!\n", state_and_ack);

   708       return -1;

   709     }

   710 

   711     if (unlikely(cmd.working_counter != 1)) {

   712       printk(KERN_ERR "EtherCAT: Could not check state %02X - Device did not"

   713              " respond!\n", state_and_ack);

   714       return -1;

   715     }

   716 

   717     if (unlikely(cmd.data[0] & 0x10)) { // State change error

   718       printk(KERN_ERR "EtherCAT: Could not set state %02X - Device refused"

   719              " state change (code %02X)!\n", state_and_ack, cmd.data[0]);

   720       return -1;

   721     }

   722 

   723     if (likely(cmd.data[0] == (state_and_ack & 0x0F))) {

   724       // State change successful

   725       break;

   726     }

   727 

   728     tries_left--;

   729   }

   730 

   731   if (unlikely(!tries_left)) {

   732     printk(KERN_ERR "EtherCAT: Could not check state %02X - Timeout while"

   733            " checking!\n", state_and_ack);

   734     return -1;

   735   }

   736 

   737   slave->current_state = state_and_ack & 0x0F;

   738 

   739   return 0;

   740 }

   741 

   742 /*****************************************************************************/

   743 

   744 /**

   745    Konfiguriert einen Slave und setzt den Operational-Zustand.

   746 

   747    Führt eine komplette Konfiguration eines Slaves durch,

   748    setzt Sync-Manager und FMMU's, führt die entsprechenden

   749    Zustandsübergänge durch, bis der Slave betriebsbereit ist.

   750 

   751    @param master EtherCAT-Master

   752    @param slave Zu aktivierender Slave

   753 

   754    @return 0 bei Erfolg, sonst < 0

   755 */

   756 

   757 int EtherCAT_activate_slave(EtherCAT_master_t *master,

   758                             EtherCAT_slave_t *slave)

   759 {

   760   EtherCAT_command_t cmd;

   761   const EtherCAT_slave_desc_t *desc;

   762   unsigned char fmmu[16];

   763   unsigned char data[256];

   764 

   765   desc = slave->desc;

   766 

   767   if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_INIT) != 0))

   768     return -1;

   769 

   770   // Resetting FMMU's

   771 

   772   memset(data, 0x00, 256);

   773 

   774   EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 256, data);

   775 

   776   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   777     return -1;

   778 

   779   if (unlikely(cmd.working_counter != 1)) {

   780     printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not"

   781            " respond!\n", slave->station_address);

   782     return -1;

   783   }

   784 

   785   // Resetting Sync Manager channels

   786 

   787   if (desc->type != ECAT_ST_SIMPLE_NOSYNC)

   788   {

   789     memset(data, 0x00, 256);

   790 

   791     EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data);

   792 

   793     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   794       return -1;

   795 

   796     if (unlikely(cmd.working_counter != 1)) {

   797       printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not"

   798              " respond!\n", slave->station_address);

   799       return -1;

   800     }

   801   }

   802 

   803   // Init Mailbox communication

   804 

   805   if (desc->type == ECAT_ST_MAILBOX)

   806   {

   807     if (desc->sm0)

   808     {

   809       EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8,

   810                              desc->sm0);

   811 

   812       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   813         return -1;

   814 

   815       if (unlikely(cmd.working_counter != 1)) {

   816         printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not"

   817                " respond!\n", slave->station_address);

   818         return -1;

   819       }

   820     }

   821 

   822     if (desc->sm1)

   823     {

   824       EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8,

   825                              desc->sm1);

   826 

   827       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   828         return -1;

   829 

   830       if (unlikely(cmd.working_counter != 1)) {

   831         printk(KERN_ERR "EtherCAT: Setting SM1 -"

   832                " Slave %04X did not respond!\n",

   833                slave->station_address);

   834         return -1;

   835       }

   836     }

   837   }

   838 

   839   // Change state to PREOP

   840 

   841   if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_PREOP) != 0))

   842     return -1;

   843 

   844   // Set FMMU's

   845 

   846   if (desc->fmmu0)

   847   {

   848     if (unlikely(!slave->process_data)) {

   849       printk(KERN_ERR "EtherCAT: Warning - Slave %04X is not assigned to any"

   850              " process data object!\n", slave->station_address);

   851       return -1;

   852     }

   853 

   854     memcpy(fmmu, desc->fmmu0, 16);

   855 

   856     fmmu[0] = slave->logical_address & 0x000000FF;

   857     fmmu[1] = (slave->logical_address & 0x0000FF00) >> 8;

   858     fmmu[2] = (slave->logical_address & 0x00FF0000) >> 16;

   859     fmmu[3] = (slave->logical_address & 0xFF000000) >> 24;

   860 

   861     EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu);

   862 

   863     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   864       return -1;

   865 

   866     if (unlikely(cmd.working_counter != 1)) {

   867       printk(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not"

   868              " respond!\n", slave->station_address);

   869       return -1;

   870     }

   871   }

   872 

   873   // Set Sync Managers

   874 

   875   if (desc->type != ECAT_ST_MAILBOX)

   876   {

   877     if (desc->sm0)

   878     {

   879       EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8,

   880                              desc->sm0);

   881 

   882       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   883         return -1;

   884 

   885       if (unlikely(cmd.working_counter != 1)) {

   886         printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not"

   887                " respond!\n", slave->station_address);

   888         return -1;

   889       }

   890     }

   891 

   892     if (desc->sm1)

   893     {

   894       EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8,

   895                              desc->sm1);

   896 

   897       if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   898         return -1;

   899 

   900       if (unlikely(cmd.working_counter != 1)) {

   901         printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not"

   902                " respond!\n", slave->station_address);

   903         return -1;

   904       }

   905     }

   906   }

   907 

   908   if (desc->sm2)

   909   {

   910     EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2);

   911 

   912     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   913       return -1;

   914 

   915     if (unlikely(cmd.working_counter != 1)) {

   916       printk(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n",

   917              slave->station_address);

   918       return -1;

   919     }

   920   }

   921 

   922   if (desc->sm3)

   923   {

   924     EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3);

   925 

   926     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   927       return -1;

   928 

   929     if (unlikely(cmd.working_counter != 1)) {

   930       printk(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n",

   931              slave->station_address);

   932       return -1;

   933     }

   934   }

   935 

   936   // Change state to SAVEOP

   937   if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_SAVEOP) != 0))

   938     return -1;

   939 

   940   // Change state to OP

   941   if (unlikely(EtherCAT_state_change(master, slave, ECAT_STATE_OP) != 0))

   942     return -1;

   943 

   944   return 0;

   945 }

   946 

   947 /*****************************************************************************/

   948 

   949 /**

   950    Setzt einen Slave zurück in den Init-Zustand.

   951 

   952    @param master EtherCAT-Master

   953    @param slave Zu deaktivierender Slave

   954 

   955    @return 0 bei Erfolg, sonst < 0

   956 */

   957 

   958 int EtherCAT_deactivate_slave(EtherCAT_master_t *master,

   959                               EtherCAT_slave_t *slave)

   960 {

   961   if (unlikely(EtherCAT_state_change(master, slave,

   962                                      ECAT_STATE_INIT) != 0))

   963     return -1;

   964 

   965   return 0;

   966 }

   967 

   968 /*****************************************************************************/

   969 

   970 /**

   971    Sendet und empfängt Prozessdaten der angegebenen Domäne

   972 

   973    @param master     EtherCAT-Master

   974           domain     Domäne

   975           timeout_us Timeout in Mikrosekunden

   976 

   977    @return 0 bei Erfolg, sonst < 0

   978 */

   979 

   980 int EtherCAT_process_data_cycle(EtherCAT_master_t *master, unsigned int domain,

   981                                 unsigned int timeout_us)

   982 {

   983   unsigned int i;

   984   EtherCAT_domain_t *dom;

   985   unsigned long start_ticks, end_ticks, timeout_ticks;

   986 

   987   ecat_output_lost_frames(master); // Evtl. verlorene Frames ausgeben

   988 

   989   // Domäne bestimmen

   990   dom = NULL;

   991   for (i = 0; i < master->domain_count; i++) {

   992     if (master->domains[i].number == domain) {

   993       dom = master->domains + i;

   994       break;

   995     }

   996   }

   997 

   998   if (unlikely(!dom)) {

   999     printk(KERN_ERR "EtherCAT: No such domain: %i!\n", domain);

  1000     return -1;

  1001   }

  1002 

  1003   EtherCAT_command_logical_read_write(&dom->command,

  1004                                       dom->logical_offset, dom->data_size,

  1005                                       dom->data);

  1006 

  1007   rdtscl(start_ticks); // Sendezeit nehmen

  1008 

  1009   if (unlikely(EtherCAT_simple_send(master, &dom->command) < 0)) {

  1010     printk(KERN_ERR "EtherCAT: Could not send process data command!\n");

  1011     return -1;

  1012   }

  1013 

  1014   timeout_ticks = timeout_us * cpu_khz / 1000;

  1015 

  1016   // Warten

  1017   do {

  1018     EtherCAT_device_call_isr(master->dev);

  1019     rdtscl(end_ticks); // Empfangszeit nehmen

  1020   }

  1021   while (unlikely(master->dev->state == ECAT_DS_SENT

  1022                   && end_ticks - start_ticks < timeout_ticks));

  1023 

  1024   master->bus_time = (end_ticks - start_ticks) * 1000 / cpu_khz;

  1025 

  1026   if (unlikely(end_ticks - start_ticks >= timeout_ticks)) {

  1027     master->dev->state = ECAT_DS_READY;

  1028     master->frames_lost++;

  1029     ecat_output_lost_frames(master);

  1030     return -1;

  1031   }

  1032 

  1033   if (unlikely(EtherCAT_simple_receive(master, &dom->command) < 0)) {

  1034     printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n");

  1035     return -1;

  1036   }

  1037 

  1038   if (unlikely(dom->command.state != ECAT_CS_RECEIVED)) {

  1039     printk(KERN_WARNING "EtherCAT: Process data command not received!\n");

  1040     return -1;

  1041   }

  1042 

  1043   if (dom->command.working_counter != dom->response_count) {

  1044     dom->response_count = dom->command.working_counter;

  1045     printk(KERN_INFO "EtherCAT: Domain %i State change - %i slaves"

  1046            " responding.\n", dom->number, dom->response_count);

  1047   }

  1048 

  1049   // Daten vom Kommando in den Prozessdatenspeicher kopieren

  1050   memcpy(dom->data, dom->command.data, dom->data_size);

  1051 

  1052   return 0;

  1053 }

  1054 

  1055 /*****************************************************************************/

  1056 

  1057 /**

  1058    Gibt Frame-Inhalte zwecks Debugging aus.

  1059 

  1060    @param master EtherCAT-Master

  1061 */

  1062 

  1063 void output_debug_data(const EtherCAT_master_t *master)

  1064 {

  1065   unsigned int i;

  1066 

  1067   printk(KERN_DEBUG "EtherCAT: tx_data content (%i Bytes):\n",

  1068          master->tx_data_length);

  1069 

  1070   printk(KERN_DEBUG);

  1071   for (i = 0; i < master->tx_data_length; i++)

  1072   {

  1073     printk("%02X ", master->tx_data[i]);

  1074     if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG);

  1075   }

  1076   printk("\n");

  1077 

  1078   printk(KERN_DEBUG "EtherCAT: rx_data content (%i Bytes):\n",

  1079          master->rx_data_length);

  1080 

  1081   printk(KERN_DEBUG);

  1082   for (i = 0; i < master->rx_data_length; i++)

  1083   {

  1084     printk("%02X ", master->rx_data[i]);

  1085     if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG);

  1086   }

  1087   printk("\n");

  1088 }

  1089 

  1090 /*****************************************************************************/

  1091 

  1092 /**

  1093    Gibt von Zeit zu Zeit die Anzahl verlorener Frames aus.

  1094 

  1095    @param master EtherCAT-Master

  1096 */

  1097 

  1098 void ecat_output_lost_frames(EtherCAT_master_t *master)

  1099 {

  1100   unsigned long int t;

  1101 

  1102   if (master->frames_lost) {

  1103     rdtscl(t);

  1104     if ((t - master->t_lost_output) / cpu_khz > 1000) {

  1105       printk(KERN_ERR "EtherCAT: %u frame(s) LOST!\n", master->frames_lost);

  1106       master->frames_lost = 0;

  1107       master->t_lost_output = t;

  1108     }

  1109   }

  1110 }

  1111 

  1112 /*****************************************************************************/

  1113 

  1114 EXPORT_SYMBOL(EtherCAT_master_open);

  1115 EXPORT_SYMBOL(EtherCAT_master_close);

  1116 EXPORT_SYMBOL(EtherCAT_activate_slave);

  1117 EXPORT_SYMBOL(EtherCAT_deactivate_slave);

  1118 EXPORT_SYMBOL(EtherCAT_process_data_cycle);

  1119 

  1120 /*****************************************************************************/

  1121 

  1122 /* Emacs-Konfiguration

  1123 ;;; Local Variables: ***

  1124 ;;; c-basic-offset:2 ***

  1125 ;;; End: ***

  1126 */