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

     2  *

     3  *  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 "globals.h"

    18 #include "master.h"

    19 #include "device.h"

    20 #include "command.h"

    21 

    22 /*****************************************************************************/

    23 

    24 // Prototypen

    25 

    26 int ec_simple_send(ec_master_t *, ec_command_t *);

    27 int ec_simple_receive(ec_master_t *, ec_command_t *);

    28 void ec_output_debug_data(const ec_master_t *);

    29 int ec_read_slave_information(ec_master_t *, unsigned short, unsigned short,

    30                               unsigned int *);

    31 void ec_output_lost_frames(ec_master_t *);

    32 

    33 /*****************************************************************************/

    34 

    35 /**

    36    Konstruktor des EtherCAT-Masters.

    37 

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

    39 */

    40 

    41 void ec_master_init(ec_master_t *master)

    42 {

    43   master->bus_slaves = NULL;

    44   master->bus_slaves_count = 0;

    45   master->device_registered = 0;

    46   master->command_index = 0x00;

    47   master->tx_data_length = 0;

    48   master->rx_data_length = 0;

    49   master->domain_count = 0;

    50   master->debug_level = 0;

    51   master->bus_time = 0;

    52   master->frames_lost = 0;

    53   master->t_lost_output = 0;

    54 }

    55 

    56 /*****************************************************************************/

    57 

    58 /**

    59    Destruktor eines EtherCAT-Masters.

    60 

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

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

    63 

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

    65 */

    66 

    67 void ec_master_clear(ec_master_t *master)

    68 {

    69   if (master->bus_slaves) {

    70     kfree(master->bus_slaves);

    71     master->bus_slaves = NULL;

    72   }

    73 

    74   ec_device_clear(&master->device);

    75 

    76   master->domain_count = 0;

    77 }

    78 

    79 /*****************************************************************************/

    80 

    81 /**

    82    Öffnet das EtherCAT-Geraet des Masters.

    83 

    84    @param master Der EtherCAT-Master

    85 

    86    @return 0, wenn alles o.k., < 0, wenn das Geraet nicht geoeffnet werden

    87            konnte.

    88 */

    89 

    90 int ec_master_open(ec_master_t *master)

    91 {

    92   if (!master->device_registered) {

    93     printk(KERN_ERR "EtherCAT: No device registered!\n");

    94     return -1;

    95   }

    96 

    97   if (ec_device_open(&master->device) < 0) {

    98     printk(KERN_ERR "EtherCAT: Could not open device!\n");

    99     return -1;

   100   }

   101 

   102   return 0;

   103 }

   104 

   105 /*****************************************************************************/

   106 

   107 /**

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

   109 

   110    @param master Der EtherCAT-Master

   111    @param device Das EtherCAT-Geraet

   112 */

   113 

   114 void ec_master_close(ec_master_t *master)

   115 {

   116   if (!master->device_registered) {

   117     printk(KERN_WARNING "EtherCAT: Warning -"

   118            " Trying to close an unregistered device!\n");

   119     return;

   120   }

   121 

   122   if (ec_device_close(&master->device) < 0) {

   123     printk(KERN_WARNING "EtherCAT: Warning - Could not close device!\n");

   124   }

   125 }

   126 

   127 /*****************************************************************************/

   128 

   129 /**

   130    Sendet ein einzelnes Kommando in einem Frame und

   131    wartet auf dessen Empfang.

   132 

   133    @param master EtherCAT-Master

   134    @param cmd    Kommando zum Senden/Empfangen

   135 

   136    @return 0 bei Erfolg, sonst < 0

   137 */

   138 

   139 int ec_simple_send_receive(ec_master_t *master, ec_command_t *cmd)

   140 {

   141   unsigned int tries_left;

   142 

   143   if (unlikely(ec_simple_send(master, cmd) < 0))

   144     return -1;

   145 

   146   tries_left = 20;

   147 

   148   do

   149   {

   150     udelay(1);

   151     ec_device_call_isr(&master->device);

   152     tries_left--;

   153   }

   154   while (unlikely(master->device.state == EC_DEVICE_STATE_SENT && tries_left));

   155 

   156   if (unlikely(ec_simple_receive(master, cmd) < 0))

   157     return -1;

   158 

   159   return 0;

   160 }

   161 

   162 /*****************************************************************************/

   163 

   164 /**

   165    Sendet ein einzelnes Kommando in einem Frame.

   166 

   167    @param master EtherCAT-Master

   168    @param cmd    Kommando zum Senden

   169 

   170    @return 0 bei Erfolg, sonst < 0

   171 */

   172 

   173 int ec_simple_send(ec_master_t *master, ec_command_t *cmd)

   174 {

   175   unsigned int length, framelength, i;

   176 

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

   178     printk(KERN_DEBUG "EtherCAT: ec_simple_send\n");

   179   }

   180 

   181   if (unlikely(cmd->state != EC_COMMAND_STATE_READY)) {

   182     printk(KERN_WARNING "EtherCAT: cmd not in ready state!\n");

   183   }

   184 

   185   length = cmd->data_length + 12;

   186   framelength = length + 2;

   187 

   188   if (unlikely(framelength > EC_FRAME_SIZE)) {

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

   190     return -1;

   191   }

   192 

   193   if (framelength < 46) framelength = 46;

   194 

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

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

   197   }

   198 

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

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

   201 

   202   cmd->index = master->command_index;

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

   204 

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

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

   207   }

   208 

   209   cmd->state = EC_COMMAND_STATE_SENT;

   210 

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

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

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

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

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

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

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

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

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

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

   221 

   222   if (likely(cmd->type == EC_COMMAND_APWR

   223              || cmd->type == EC_COMMAND_NPWR

   224              || cmd->type == EC_COMMAND_BWR

   225              || cmd->type == EC_COMMAND_LRW)) // Write commands

   226   {

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

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

   229   }

   230   else // Read commands

   231   {

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

   233   }

   234 

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

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

   237 

   238   // Pad with zeros

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

   240 

   241   master->tx_data_length = framelength;

   242 

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

   244     printk(KERN_DEBUG "EtherCAT: Device send...\n");

   245   }

   246 

   247   // Send frame

   248   if (unlikely(ec_device_send(&master->device, master->tx_data,

   249                               framelength) != 0)) {

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

   251     return -1;

   252   }

   253 

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

   255     printk(KERN_DEBUG "EtherCAT: ec_simple_send done.\n");

   256   }

   257 

   258   return 0;

   259 }

   260 

   261 /*****************************************************************************/

   262 

   263 /**

   264    Wartet auf den Empfang eines einzeln gesendeten

   265    Kommandos.

   266 

   267    @param master EtherCAT-Master

   268    @param cmd    Gesendetes Kommando

   269 

   270    @return 0 bei Erfolg, sonst < 0

   271 */

   272 

   273 int ec_simple_receive(ec_master_t *master, ec_command_t *cmd)

   274 {

   275   unsigned int length;

   276   int ret;

   277   unsigned char command_type, command_index;

   278 

   279   if (unlikely((ret = ec_device_receive(&master->device, master->rx_data)) < 0))

   280     return -1;

   281 

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

   283 

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

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

   286            " header!\n");

   287     ec_output_debug_data(master);

   288     return -1;

   289   }

   290 

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

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

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

   294 

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

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

   297            " not match)!\n");

   298     ec_output_debug_data(master);

   299     return -1;

   300   }

   301 

   302   command_type = master->rx_data[2];

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

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

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

   306 

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

   308     printk(KERN_ERR "EtherCAT: Received frame with"

   309            " incomplete command data!\n");

   310     ec_output_debug_data(master);

   311     return -1;

   312   }

   313 

   314   if (likely(cmd->state == EC_COMMAND_STATE_SENT

   315              && cmd->type == command_type

   316              && cmd->index == command_index

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

   318   {

   319     cmd->state = EC_COMMAND_STATE_RECEIVED;

   320 

   321     // Empfangene Daten in Kommandodatenspeicher kopieren

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

   323 

   324     // Working-Counter setzen

   325     cmd->working_counter

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

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

   328 

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

   330       ec_output_debug_data(master);

   331     }

   332   }

   333   else

   334   {

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

   336     ec_output_debug_data(master);

   337   }

   338 

   339   master->device.state = EC_DEVICE_STATE_READY;

   340 

   341   return 0;

   342 }

   343 

   344 /*****************************************************************************/

   345 

   346 /**

   347    Durchsucht den Bus nach Slaves.

   348 

   349    @param master Der EtherCAT-Master

   350 

   351    @return 0 bei Erfolg, sonst < 0

   352 */

   353 

   354 int ec_scan_for_slaves(ec_master_t *master)

   355 {

   356   ec_command_t cmd;

   357   ec_slave_t *cur;

   358   unsigned int i, j;

   359   unsigned char data[2];

   360 

   361   // Determine number of slaves on bus

   362 

   363   ec_command_broadcast_read(&cmd, 0x0000, 4);

   364 

   365   if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   366     return -1;

   367 

   368   master->bus_slaves_count = cmd.working_counter;

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

   370 

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

   372 

   373   if (!(master->bus_slaves = (ec_slave_t *) kmalloc(master->bus_slaves_count

   374                                                     * sizeof(ec_slave_t),

   375                                                     GFP_KERNEL))) {

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

   377     return -1;

   378   }

   379 

   380   // For every slave in the list

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

   382   {

   383     cur = master->bus_slaves + i;

   384 

   385     ec_slave_init(cur);

   386 

   387     // Read base data

   388 

   389     ec_command_read(&cmd, cur->station_address, 0x0000, 4);

   390 

   391     if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   392       return -1;

   393 

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

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

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

   397       return -1;

   398     }

   399 

   400     // Get base data

   401 

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

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

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

   405 

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

   407 

   408     if (unlikely(ec_read_slave_information(master, cur->station_address,

   409                                            0x0008, &cur->vendor_id) != 0)) {

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

   411       return -1;

   412     }

   413 

   414     if (unlikely(ec_read_slave_information(master, cur->station_address,

   415                                            0x000A, &cur->product_code) != 0)) {

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

   417       return -1;

   418     }

   419 

   420     if (unlikely(ec_read_slave_information(master, cur->station_address,

   421                                            0x000C,

   422                                            &cur->revision_number) != 0)) {

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

   424       return -1;

   425     }

   426 

   427     if (unlikely(ec_read_slave_information(master, cur->station_address,

   428                                            0x000E,

   429                                            &cur->serial_number) != 0)) {

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

   431       return -1;

   432     }

   433 

   434     // Search for identification in "database"

   435 

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

   437     {

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

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

   440       {

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

   442         break;

   443       }

   444     }

   445 

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

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

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

   449       return -1;

   450     }

   451 

   452     // Set ring position

   453     cur->ring_position = -i;

   454     cur->station_address = i + 1;

   455 

   456     // Write station address

   457 

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

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

   460 

   461     ec_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data);

   462 

   463     if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   464       return -1;

   465 

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

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

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

   469       return -1;

   470     }

   471   }

   472 

   473   return 0;

   474 }

   475 

   476 /*****************************************************************************/

   477 

   478 /**

   479    Liest Daten aus dem Slave-Information-Interface

   480    eines EtherCAT-Slaves.

   481 

   482    @param master EtherCAT-Master

   483    @param node_address Knotenadresse des Slaves

   484    @param offset Adresse des zu lesenden SII-Registers

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

   486    zum Ablegen der Daten

   487 

   488    @return 0 bei Erfolg, sonst < 0

   489 */

   490 

   491 int ec_read_slave_information(ec_master_t *master,

   492                               unsigned short int node_address,

   493                               unsigned short int offset,

   494                               unsigned int *target)

   495 {

   496   ec_command_t cmd;

   497   unsigned char data[10];

   498   unsigned int tries_left;

   499 

   500   // Initiate read operation

   501 

   502   data[0] = 0x00;

   503   data[1] = 0x01;

   504   data[2] = offset & 0xFF;

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

   506   data[4] = 0x00;

   507   data[5] = 0x00;

   508 

   509   ec_command_write(&cmd, node_address, 0x502, 6, data);

   510 

   511   if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   512     return -1;

   513 

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

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

   516            node_address);

   517     return -1;

   518   }

   519 

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

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

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

   523 

   524   tries_left = 100;

   525   while (likely(tries_left))

   526   {

   527     udelay(10);

   528 

   529     ec_command_read(&cmd, node_address, 0x502, 10);

   530 

   531     if (unlikely(ec_simple_send_receive(master, &cmd) != 0))

   532       return -1;

   533 

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

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

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

   537       return -1;

   538     }

   539 

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

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

   542       break;

   543     }

   544 

   545     tries_left--;

   546   }

   547 

   548   if (unlikely(!tries_left)) {

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

   550            node_address);

   551     return -1;

   552   }

   553 

   554   return 0;

   555 }

   556 

   557 /*****************************************************************************/

   558 

   559 /**

   560    Ändert den Zustand eines Slaves (asynchron).

   561 

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

   563 

   564    @param master EtherCAT-Master

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

   566    @param state_and_ack Neuer Zustand, evtl. mit gesetztem

   567    Acknowledge-Flag

   568 

   569    @return 0 bei Erfolg, sonst < 0

   570 */

   571 

   572 int ec_state_change(ec_master_t *master, ec_slave_t *slave,

   573                     unsigned char state_and_ack)

   574 {

   575   ec_command_t cmd;

   576   unsigned char data[2];

   577   unsigned int tries_left;

   578 

   579   data[0] = state_and_ack;

   580   data[1] = 0x00;

   581 

   582   ec_command_write(&cmd, slave->station_address, 0x0120, 2, data);

   583 

   584   if (unlikely(ec_simple_send_receive(master, &cmd) != 0)) {

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

   586            state_and_ack);

   587     return -1;

   588   }

   589 

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

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

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

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

   594     return -1;

   595   }

   596 

   597   slave->requested_state = state_and_ack & 0x0F;

   598 

   599   tries_left = 100;

   600   while (likely(tries_left))

   601   {

   602     udelay(10);

   603 

   604     ec_command_read(&cmd, slave->station_address, 0x0130, 2);

   605 

   606     if (unlikely(ec_simple_send_receive(master, &cmd) != 0)) {

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

   608              " send!\n", state_and_ack);

   609       return -1;

   610     }

   611 

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

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

   614              " respond!\n", state_and_ack);

   615       return -1;

   616     }

   617 

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

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

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

   621       return -1;

   622     }

   623 

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

   625       // State change successful

   626       break;

   627     }

   628 

   629     tries_left--;

   630   }

   631 

   632   if (unlikely(!tries_left)) {

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

   634            " checking!\n", state_and_ack);

   635     return -1;

   636   }

   637 

   638   slave->current_state = state_and_ack & 0x0F;

   639 

   640   return 0;

   641 }

   642 

   643 /*****************************************************************************/

   644 

   645 /**

   646    Gibt Frame-Inhalte zwecks Debugging aus.

   647 

   648    @param master EtherCAT-Master

   649 */

   650 

   651 void ec_output_debug_data(const ec_master_t *master)

   652 {

   653   unsigned int i;

   654 

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

   656          master->tx_data_length);

   657 

   658   printk(KERN_DEBUG);

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

   660   {

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

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

   663   }

   664   printk("\n");

   665 

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

   667          master->rx_data_length);

   668 

   669   printk(KERN_DEBUG);

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

   671   {

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

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

   674   }

   675   printk("\n");

   676 }

   677 

   678 /*****************************************************************************/

   679 

   680 /**

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

   682 

   683    @param master EtherCAT-Master

   684 */

   685 

   686 void ec_output_lost_frames(ec_master_t *master)

   687 {

   688   unsigned long int t;

   689 

   690   if (master->frames_lost) {

   691     rdtscl(t);

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

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

   694       master->frames_lost = 0;

   695       master->t_lost_output = t;

   696     }

   697   }

   698 }

   699 

   700 /******************************************************************************

   701  *

   702  * Echtzeitschnittstelle

   703  *

   704  *****************************************************************************/

   705 

   706 /**

   707    Registriert einen Slave beim Master.

   708 

   709    @param master Der EtherCAT-Master

   710 

   711    @return 0 bei Erfolg, sonst < 0

   712 */

   713 

   714 void *EtherCAT_rt_register_slave(ec_master_t *master, unsigned int bus_index,

   715                               const char *vendor_name,

   716                               const char *product_name, unsigned int domain)

   717 {

   718   ec_slave_t *slave;

   719   ec_domain_t *dom;

   720   unsigned int j;

   721 

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

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

   724            master->bus_slaves_count);

   725     return NULL;

   726   }

   727 

   728   slave = master->bus_slaves + bus_index;

   729 

   730   if (slave->process_data) {

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

   732     return NULL;

   733   }

   734 

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

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

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

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

   739            slave->desc->product_name);

   740     return NULL;

   741   }

   742 

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

   744   dom = NULL;

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

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

   747       dom = master->domains + j;

   748     }

   749   }

   750 

   751   // Create process data domain

   752   if (!dom) {

   753     if (master->domain_count > EC_MAX_DOMAINS - 1) {

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

   755       return NULL;

   756     }

   757 

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

   759     ec_domain_init(dom);

   760     dom->number = domain;

   761     dom->logical_offset = master->domain_count * EC_FRAME_SIZE;

   762     master->domain_count++;

   763   }

   764 

   765   if (dom->data_size + slave->desc->process_data_size > EC_FRAME_SIZE - 14) {

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

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

   768            EC_FRAME_SIZE - 14);

   769     return NULL;

   770   }

   771 

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

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

   774 

   775   return slave->process_data;

   776 }

   777 

   778 /*****************************************************************************/

   779 

   780 /**

   781    Konfiguriert einen Slave und setzt den Operational-Zustand.

   782 

   783    Führt eine komplette Konfiguration eines Slaves durch,

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

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

   786 

   787    @param master EtherCAT-Master

   788    @param slave Zu aktivierender Slave

   789 

   790    @return 0 bei Erfolg, sonst < 0

   791 */

   792 

   793 int EtherCAT_rt_activate_slaves(ec_master_t *master)

   794 {

   795   unsigned int i;

   796   ec_slave_t *slave;

   797   ec_command_t cmd;

   798   const ec_slave_desc_t *desc;

   799   unsigned char fmmu[16];

   800   unsigned char data[256];

   801 

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

   803   {

   804     slave = master->bus_slaves + i;

   805     desc = slave->desc;

   806 

   807     if (unlikely(ec_state_change(master, slave, EC_SLAVE_STATE_INIT) != 0))

   808       return -1;

   809 

   810     // Resetting FMMU's

   811 

   812     memset(data, 0x00, 256);

   813 

   814     ec_command_write(&cmd, slave->station_address, 0x0600, 256, data);

   815 

   816     if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   817       return -1;

   818 

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

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

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

   822       return -1;

   823     }

   824 

   825     // Resetting Sync Manager channels

   826 

   827     if (desc->type != EC_NOSYNC_SLAVE)

   828     {

   829       memset(data, 0x00, 256);

   830 

   831       ec_command_write(&cmd, slave->station_address, 0x0800, 256, data);

   832 

   833       if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   834         return -1;

   835 

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

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

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

   839         return -1;

   840       }

   841     }

   842 

   843     // Init Mailbox communication

   844 

   845     if (desc->type == EC_MAILBOX_SLAVE)

   846     {

   847       if (desc->sm0)

   848       {

   849         ec_command_write(&cmd, slave->station_address, 0x0800, 8,

   850                          desc->sm0);

   851 

   852         if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   853           return -1;

   854 

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

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

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

   858           return -1;

   859         }

   860       }

   861 

   862       if (desc->sm1)

   863       {

   864         ec_command_write(&cmd, slave->station_address, 0x0808, 8,

   865                          desc->sm1);

   866 

   867         if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   868           return -1;

   869 

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

   871           printk(KERN_ERR "EtherCAT: Setting SM1 -"

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

   873                  slave->station_address);

   874           return -1;

   875         }

   876       }

   877     }

   878 

   879     // Change state to PREOP

   880 

   881     if (unlikely(ec_state_change(master, slave, EC_SLAVE_STATE_PREOP) != 0))

   882       return -1;

   883 

   884     // Set FMMU's

   885 

   886     if (desc->fmmu0)

   887     {

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

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

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

   891         return -1;

   892       }

   893 

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

   895 

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

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

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

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

   900 

   901       ec_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu);

   902 

   903       if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   904         return -1;

   905 

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

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

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

   909         return -1;

   910       }

   911     }

   912 

   913     // Set Sync Managers

   914 

   915     if (desc->type != EC_MAILBOX_SLAVE)

   916     {

   917       if (desc->sm0)

   918       {

   919         ec_command_write(&cmd, slave->station_address, 0x0800, 8,

   920                          desc->sm0);

   921 

   922         if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   923           return -1;

   924 

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

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

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

   928           return -1;

   929         }

   930       }

   931 

   932       if (desc->sm1)

   933       {

   934         ec_command_write(&cmd, slave->station_address, 0x0808, 8,

   935                          desc->sm1);

   936 

   937         if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   938           return -1;

   939 

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

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

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

   943           return -1;

   944         }

   945       }

   946     }

   947 

   948     if (desc->sm2)

   949     {

   950       ec_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2);

   951 

   952       if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   953         return -1;

   954 

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

   956         printk(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not"

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

   958         return -1;

   959       }

   960     }

   961 

   962     if (desc->sm3)

   963     {

   964       ec_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3);

   965 

   966       if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   967         return -1;

   968 

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

   970         printk(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not"

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

   972         return -1;

   973       }

   974     }

   975 

   976     // Change state to SAVEOP

   977     if (unlikely(ec_state_change(master, slave, EC_SLAVE_STATE_SAVEOP) != 0))

   978       return -1;

   979 

   980     // Change state to OP

   981     if (unlikely(ec_state_change(master, slave, EC_SLAVE_STATE_OP) != 0))

   982       return -1;

   983   }

   984 

   985   return 0;

   986 }

   987 

   988 /*****************************************************************************/

   989 

   990 /**

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

   992 

   993    @param master EtherCAT-Master

   994    @param slave Zu deaktivierender Slave

   995 

   996    @return 0 bei Erfolg, sonst < 0

   997 */

   998 

   999 int EtherCAT_rt_deactivate_slaves(ec_master_t *master)

  1000 {

  1001   ec_slave_t *slave;

  1002   unsigned int i;

  1003 

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

  1005   {

  1006     slave = master->bus_slaves + 1;

  1007 

  1008     if (unlikely(ec_state_change(master, slave, EC_SLAVE_STATE_INIT) != 0))

  1009       return -1;

  1010   }

  1011 

  1012   return 0;

  1013 }

  1014 

  1015 /*****************************************************************************/

  1016 

  1017 /**

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

  1019 

  1020    @param master     EtherCAT-Master

  1021           domain     Domäne

  1022           timeout_us Timeout in Mikrosekunden

  1023 

  1024    @return 0 bei Erfolg, sonst < 0

  1025 */

  1026 

  1027 int EtherCAT_rt_domain_cycle(ec_master_t *master, unsigned int domain,

  1028                              unsigned int timeout_us)

  1029 {

  1030   unsigned int i;

  1031   ec_domain_t *dom;

  1032   unsigned long start_ticks, end_ticks, timeout_ticks;

  1033 

  1034   ec_output_lost_frames(master); // Evtl. verlorene Frames ausgeben

  1035 

  1036   // Domäne bestimmen

  1037   dom = NULL;

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

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

  1040       dom = master->domains + i;

  1041       break;

  1042     }

  1043   }

  1044 

  1045   if (unlikely(!dom)) {

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

  1047     return -1;

  1048   }

  1049 

  1050   ec_command_logical_read_write(&dom->command, dom->logical_offset,

  1051                                 dom->data_size, dom->data);

  1052 

  1053   rdtscl(start_ticks); // Sendezeit nehmen

  1054 

  1055   if (unlikely(ec_simple_send(master, &dom->command) < 0)) {

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

  1057     return -1;

  1058   }

  1059 

  1060   timeout_ticks = timeout_us * cpu_khz / 1000;

  1061 

  1062   // Warten

  1063   do {

  1064     ec_device_call_isr(&master->device);

  1065     rdtscl(end_ticks); // Empfangszeit nehmen

  1066   }

  1067   while (unlikely(master->device.state == EC_DEVICE_STATE_SENT

  1068                   && end_ticks - start_ticks < timeout_ticks));

  1069 

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

  1071 

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

  1073     master->device.state = EC_DEVICE_STATE_READY;

  1074     master->frames_lost++;

  1075     ec_output_lost_frames(master);

  1076     return -1;

  1077   }

  1078 

  1079   if (unlikely(ec_simple_receive(master, &dom->command) < 0)) {

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

  1081     return -1;

  1082   }

  1083 

  1084   if (unlikely(dom->command.state != EC_COMMAND_STATE_RECEIVED)) {

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

  1086     return -1;

  1087   }

  1088 

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

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

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

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

  1093   }

  1094 

  1095   // Daten vom Kommando in den Prozessdatenspeicher kopieren

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

  1097 

  1098   return 0;

  1099 }

  1100 

  1101 /*****************************************************************************/

  1102 

  1103 EXPORT_SYMBOL(EtherCAT_rt_register_slave);

  1104 EXPORT_SYMBOL(EtherCAT_rt_activate_slaves);

  1105 EXPORT_SYMBOL(EtherCAT_rt_deactivate_slaves);

  1106 EXPORT_SYMBOL(EtherCAT_rt_domain_cycle);

  1107 

  1108 /*****************************************************************************/

  1109 

  1110 /* Emacs-Konfiguration

  1111 ;;; Local Variables: ***

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

  1113 ;;; End: ***

  1114 */