23 #include "command.h"

    23 #include "frame.h"

    24 

    25 /*****************************************************************************/

    26 

    27 // Prototypen

    28 

    29 int ec_simple_send(ec_master_t *, ec_command_t *);

    30 int ec_simple_receive(ec_master_t *, ec_command_t *);

    31 void ec_output_debug_data(const ec_master_t *);

    32 int ec_sii_read(ec_master_t *, unsigned short, unsigned short, unsigned int *);

    33 void ec_output_lost_frames(ec_master_t *);

    41 void ec_master_init(ec_master_t *master

    31 void ec_master_init(ec_master_t *master /**< EtherCAT-Master */)

    42                     /**< Zeiger auf den zu initialisierenden EtherCAT-Master */

    32 {

    43                     )

    33     master->slaves = NULL;

    44 {

    34     master->slave_count = 0;

    45   master->bus_slaves = NULL;

    35     master->device_registered = 0;

    46   master->bus_slaves_count = 0;

    36     master->command_index = 0x00;

    47   master->device_registered = 0;

    37     master->domain_count = 0;

    48   master->command_index = 0x00;

    38     master->debug_level = 0;

    49   master->tx_data_length = 0;

    39     master->bus_time = 0;

    50   master->rx_data_length = 0;

    40     master->frames_lost = 0;

    51   master->domain_count = 0;

    41     master->t_lost_output = 0;

    52   master->debug_level = 0;

    53   master->bus_time = 0;

    54   master->frames_lost = 0;

    55   master->t_lost_output = 0;

    67 void ec_master_clear(ec_master_t *master

    53 void ec_master_clear(ec_master_t *master /**< EtherCAT-Master */)

    68                      /**< Zeiger auf den zu löschenden Master */

    54 {

    69                      )

    55     ec_master_reset(master);

    70 {

    56     ec_device_clear(&master->device);

    71   if (master->bus_slaves) {

    57 }

    72     kfree(master->bus_slaves);

    58 

    73     master->bus_slaves = NULL;

    59 /*****************************************************************************/

    74   }

    60 

    75 

    61 /**

    76   ec_device_clear(&master->device);

    62    Setzt den Master zurück in den Ausgangszustand.

    77 

    78   master->domain_count = 0;

    79 }

    80 

    81 /*****************************************************************************/

    82 

    83 /**

    84    Setzt den Master in den Ausgangszustand.

    94   if (master->bus_slaves) {

    72     unsigned int i;

    95     kfree(master->bus_slaves);

    73 

    96     master->bus_slaves = NULL;

    74     if (master->slaves) {

    97   }

    75         for (i = 0; i < master->slave_count; i++) {

    98 

    76             ec_slave_clear(master->slaves + i);

    99   master->bus_slaves_count = 0;

    77         }

   100   master->command_index = 0;

    78         kfree(master->slaves);

   101   master->tx_data_length = 0;

    79         master->slaves = NULL;

   102   master->rx_data_length = 0;

    80     }

   103   master->domain_count = 0;

    81     master->slave_count = 0;

   104   master->debug_level = 0;

    82 

   105   master->bus_time = 0;

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

   106   master->frames_lost = 0;

    84         ec_domain_clear(master->domains[i]);

   107   master->t_lost_output = 0;

    85         kfree(master->domains[i]);

    86     }

    87     master->domain_count = 0;

    88 

    89     master->command_index = 0;

    90     master->debug_level = 0;

    91     master->bus_time = 0;

    92     master->frames_lost = 0;

    93     master->t_lost_output = 0;

   121   if (!master->device_registered) {

   107     if (!master->device_registered) {

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

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

   123     return -1;

   109         return -1;

   124   }

   110     }

   125 

   111 

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

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

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

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

   128     return -1;

   114         return -1;

   129   }

   115     }

   130 

   116 

   131   return 0;

   117     return 0;

   142   if (!master->device_registered) {

   128     if (!master->device_registered) {

   143     printk(KERN_WARNING "EtherCAT: Warning -"

   129         printk(KERN_WARNING "EtherCAT: Warning -"

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

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

   145     return;

   131         return;

   146   }

   132     }

   147 

   133 

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

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

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

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

   150   }

   136     }

   151 }

   152 

   153 /*****************************************************************************/

   154 

   155 /**

   156    Sendet ein einzelnes Kommando in einem Frame und

   157    wartet auf dessen Empfang.

   158 

   159    \return 0 bei Erfolg, sonst < 0

   160 */

   161 

   162 int ec_simple_send_receive(ec_master_t *master,

   163                            /**< EtherCAT-Master */

   164                            ec_command_t *cmd

   165                            /**< Kommando zum Senden/Empfangen */

   166                            )

   167 {

   168   unsigned int tries_left;

   169 

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

   171     return -1;

   172 

   173   tries_left = 20;

   174 

   175   do

   176   {

   177     udelay(1);

   178     ec_device_call_isr(&master->device);

   179     tries_left--;

   180   }

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

   182 

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

   184     return -1;

   185 

   186   return 0;

   187 }

   188 

   189 /*****************************************************************************/

   190 

   191 /**

   192    Sendet ein einzelnes Kommando in einem Frame.

   193 

   194    \return 0 bei Erfolg, sonst < 0

   195 */

   196 

   197 int ec_simple_send(ec_master_t *master, /**< EtherCAT-Master */

   198                    ec_command_t *cmd /**< Kommando zum Senden */

   199                    )

   200 {

   201   unsigned int length, framelength, i;

   202 

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

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

   205   }

   206 

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

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

   209   }

   210 

   211   length = cmd->data_length + 12;

   212   framelength = length + 2;

   213 

   214   if (unlikely(framelength > EC_FRAME_SIZE)) {

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

   216     return -1;

   217   }

   218 

   219   if (framelength < 46) framelength = 46;

   220 

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

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

   223   }

   224 

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

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

   227 

   228   cmd->index = master->command_index;

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

   230 

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

   232     printk(KERN_DEBUG "EtherCAT: Sending command index 0x%X\n", cmd->index);

   233   }

   234 

   235   cmd->state = EC_COMMAND_STATE_SENT;

   236 

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

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

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

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

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

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

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

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

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

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

   247 

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

   249              || cmd->type == EC_COMMAND_NPWR

   250              || cmd->type == EC_COMMAND_BWR

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

   252   {

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

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

   255   }

   256   else // Read commands

   257   {

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

   259   }

   260 

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

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

   263 

   264   // Pad with zeros

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

   266 

   267   master->tx_data_length = framelength;

   268 

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

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

   271   }

   272 

   273   // Send frame

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

   275                               framelength) != 0)) {

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

   277     return -1;

   278   }

   279 

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

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

   282   }

   283 

   284   return 0;

   285 }

   286 

   287 /*****************************************************************************/

   288 

   289 /**

   290    Wartet auf den Empfang eines einzeln gesendeten

   291    Kommandos.

   292 

   293    \return 0 bei Erfolg, sonst < 0

   294 */

   295 

   296 int ec_simple_receive(ec_master_t *master, /**< EtherCAT-Master */

   297                       ec_command_t *cmd /**< Gesendetes Kommando */

   298                       )

   299 {

   300   unsigned int length;

   301   int ret;

   302   unsigned char command_type, command_index;

   303 

   304   if (unlikely((ret = ec_device_receive(&master->device,

   305                                         master->rx_data)) < 0))

   306     return -1;

   307 

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

   309 

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

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

   312            " header!\n");

   313     ec_output_debug_data(master);

   314     return -1;

   315   }

   316 

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

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

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

   320 

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

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

   323            " not match)!\n");

   324     ec_output_debug_data(master);

   325     return -1;

   326   }

   327 

   328   command_type = master->rx_data[2];

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

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

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

   332 

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

   334     printk(KERN_ERR "EtherCAT: Received frame with"

   335            " incomplete command data!\n");

   336     ec_output_debug_data(master);

   337     return -1;

   338   }

   339 

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

   341              && cmd->type == command_type

   342              && cmd->index == command_index

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

   344   {

   345     cmd->state = EC_COMMAND_STATE_RECEIVED;

   346 

   347     // Empfangene Daten in Kommandodatenspeicher kopieren

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

   349 

   350     // Working-Counter setzen

   351     cmd->working_counter

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

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

   354 

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

   356       ec_output_debug_data(master);

   357     }

   358   }

   359   else

   360   {

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

   362     ec_output_debug_data(master);

   363   }

   364 

   365   master->device.state = EC_DEVICE_STATE_READY;

   366 

   367   return 0;

   380   ec_command_t cmd;

   149     ec_frame_t frame;

   381   ec_slave_t *slave;

   150     ec_slave_t *slave;

   382   unsigned int i, j;

   151     ec_slave_ident_t *ident;

   383   unsigned char data[2];

   152     unsigned int i;

   384 

   153     unsigned char data[2];

   385   // Determine number of slaves on bus

   154 

   386 

   155     if (master->slaves || master->slave_count) {

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

   156         printk(KERN_ERR "EtherCAT: Slave scan already done!\n");

   388 

   157         return -1;

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

   158     }

   390     return -1;

   159 

   391 

   160     // Determine number of slaves on bus

   392   master->bus_slaves_count = cmd.working_counter;

   161 

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

   162     ec_frame_init_brd(&frame, master, 0x0000, 4);

   394 

   163     if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

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

   164 

   396 

   165     master->slave_count = frame.working_counter;

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

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

   398                                                     * sizeof(ec_slave_t),

   167 

   399                                                     GFP_KERNEL))) {

   168     if (!master->slave_count) return 0;

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

   169 

   401     return -1;

   170     if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   402   }

   171                                                       * sizeof(ec_slave_t),

   403 

   172                                                       GFP_KERNEL))) {

   404   // For every slave in the list

   173         printk(KERN_ERR "EtherCAT: Could not allocate memory for bus"

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

   174                " slaves!\n");

   406   {

   175         return -1;

   407     slave = master->bus_slaves + i;

   176     }

   408 

   177 

   409     ec_slave_init(slave);

   178     // Init slaves

   410 

   179     for (i = 0; i < master->slave_count; i++) {

   411     // Set ring position

   180         slave = master->slaves + i;

   412 

   181         ec_slave_init(slave, master);

   413     slave->ring_position = -i;

   182         slave->ring_position = i;

   414     slave->station_address = i + 1;

   183         slave->station_address = i + 1;

   415 

   184     }

   416     // Write station address

   185 

   417 

   186     // For every slave in the list

   418     data[0] = slave->station_address & 0x00FF;

   187     for (i = 0; i < master->slave_count; i++)

   419     data[1] = (slave->station_address & 0xFF00) >> 8;

   420 

   421     ec_command_position_write(&cmd, slave->ring_position, 0x0010, 2, data);

   422 

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

   424       return -1;

   425 

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

   427       printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing"

   428              " station address!\n", i);

   429       return -1;

   430     }

   431 

   432     // Read base data

   433 

   434     ec_command_read(&cmd, slave->station_address, 0x0000, 4);

   435 

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

   437       return -1;

   438 

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

   440       printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base"

   441              " data!\n", i);

   442       return -1;

   443     }

   444 

   445     // Get base data

   446 

   447     slave->base_type = cmd.data[0];

   448     slave->base_revision = cmd.data[1];

   449     slave->base_build = cmd.data[2] | (cmd.data[3] << 8);

   450 

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

   452 

   453     if (unlikely(ec_sii_read(master, slave->station_address, 0x0008,

   454                              &slave->sii_vendor_id) != 0)) {

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

   456       return -1;

   457     }

   458 

   459     if (unlikely(ec_sii_read(master, slave->station_address, 0x000A,

   460                              &slave->sii_product_code) != 0)) {

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

   462       return -1;

   463     }

   464 

   465     if (unlikely(ec_sii_read(master, slave->station_address, 0x000C,

   466                              &slave->sii_revision_number) != 0)) {

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

   468       return -1;

   469     }

   470 

   471     if (unlikely(ec_sii_read(master, slave->station_address, 0x000E,

   472                              &slave->sii_serial_number) != 0)) {

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

   474       return -1;

   475     }

   476 

   477     // Search for identification in "database"

   478 

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

   481       if (unlikely(slave_idents[j].vendor_id == slave->sii_vendor_id

   189         slave = master->slaves + i;

   482                    && slave_idents[j].product_code == slave->sii_product_code))

   190 

   483       {

   191         // Write station address

   484         slave->type = slave_idents[j].type;

   192         data[0] = slave->station_address & 0x00FF;

   485         break;

   193         data[1] = (slave->station_address & 0xFF00) >> 8;

   486       }

   194 

   487     }

   195         ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010, 2,

   488 

   196                            data);

   489     if (unlikely(!slave->type)) {

   197 

   490       printk(KERN_WARNING "EtherCAT: Unknown slave device (vendor 0x%08X, code"

   198         if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

   491              " 0x%08X) at position %i.\n", slave->sii_vendor_id,

   199 

   492              slave->sii_product_code, i);

   200         if (unlikely(frame.working_counter != 1)) {

   493       return 0;

   201             printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing"

   494     }

   202                    " station address!\n", i);

   495   }

   203             return -1;

   496 

   204         }

   497   return 0;

   205 

   498 }

   206         // Fetch all slave information

   499 

   207         if (ec_slave_fetch(slave)) return -1;

   500 /*****************************************************************************/

   208 

   501 

   209         // Search for identification in "database"

   502 /**

   210         ident = slave_idents;

   503    Liest Daten aus dem Slave-Information-Interface

   211         while (ident) {

   504    eines EtherCAT-Slaves.

   212             if (unlikely(ident->vendor_id == slave->sii_vendor_id

   505 

   213                          && ident->product_code == slave->sii_product_code)) {

   506    \return 0 bei Erfolg, sonst < 0

   214                 slave->type = ident->type;

   507 */

   215                 break;

   508 

   216             }

   509 int ec_sii_read(ec_master_t *master,

   217             ident++;

   510                 /**< EtherCAT-Master */

   218         }

   511                 unsigned short int node_address,

   219 

   512                 /**< Knotenadresse des Slaves */

   220         if (!slave->type) {

   513                 unsigned short int offset,

   221             printk(KERN_WARNING "EtherCAT: Unknown slave device (vendor"

   514                 /**< Adresse des zu lesenden SII-Registers */

   222                    " 0x%08X, code 0x%08X) at position %i.\n",

   515                 unsigned int *target

   223                    slave->sii_vendor_id, slave->sii_product_code, i);

   516                 /**< Zeiger auf einen 4 Byte großen Speicher zum Ablegen der

   224             return 0;

   517                    Daten */

   225         }

   518                 )

   226     }

   519 {

   227 

   520   ec_command_t cmd;

   228     return 0;

   521   unsigned char data[10];

   522   unsigned int tries_left;

   523 

   524   // Initiate read operation

   525 

   526   data[0] = 0x00;

   527   data[1] = 0x01;

   528   data[2] = offset & 0xFF;

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

   530   data[4] = 0x00;

   531   data[5] = 0x00;

   532 

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

   534 

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

   536     return -1;

   537 

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

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

   540            node_address);

   541     return -1;

   542   }

   543 

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

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

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

   547 

   548   tries_left = 100;

   549   while (likely(tries_left))

   550   {

   551     udelay(10);

   552 

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

   554 

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

   556       return -1;

   557 

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

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

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

   561       return -1;

   562     }

   563 

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

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

   566       break;

   567     }

   568 

   569     tries_left--;

   570   }

   571 

   572   if (unlikely(!tries_left)) {

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

   574            node_address);

   575     return -1;

   576   }

   577 

   578   return 0;

   579 }

   580 

   581 /*****************************************************************************/

   582 

   583 /**

   584    Ändert den Zustand eines Slaves.

   585 

   586    \return 0 bei Erfolg, sonst < 0

   587 */

   588 

   589 int ec_state_change(ec_master_t *master,

   590                     /**<EtherCAT-Master */

   591                     ec_slave_t *slave,

   592                     /**< Slave, dessen Zustand geändert werden soll */

   593                     unsigned char state_and_ack

   594                     /**< Neuer Zustand, evtl. mit gesetztem Acknowledge-Flag */

   595                     )

   596 {

   597   ec_command_t cmd;

   598   unsigned char data[2];

   599   unsigned int tries_left;

   600 

   601   data[0] = state_and_ack;

   602   data[1] = 0x00;

   603 

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

   605 

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

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

   608            state_and_ack);

   609     return -1;

   610   }

   611 

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

   613     printk(KERN_ERR "EtherCAT: Could not set state %02X - Slave %i did not"

   614            " respond!\n", state_and_ack, slave->ring_position * (-1));

   615     return -1;

   616   }

   617 

   618   tries_left = 100;

   619   while (likely(tries_left))

   620   {

   621     udelay(10);

   622 

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

   624 

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

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

   627              " send!\n", state_and_ack);

   628       return -1;

   629     }

   630 

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

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

   633              " not respond!\n", state_and_ack, slave->ring_position * (-1));

   634       return -1;

   635     }

   636 

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

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

   639              " state change (code %02X)!\n", state_and_ack,

   640              slave->ring_position * (-1), cmd.data[0]);

   641       return -1;

   642     }

   643 

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

   645       // State change successful

   646       break;

   647     }

   648 

   649     tries_left--;

   650   }

   651 

   652   if (unlikely(!tries_left)) {

   653     printk(KERN_ERR "EtherCAT: Could not check state %02X of slave %i -"

   654            " Timeout while checking!\n", state_and_ack,

   655            slave->ring_position * (-1));

   656     return -1;

   657   }

   658 

   659   return 0;

   660 }

   661 

   662 /*****************************************************************************/

   663 

   664 /**

   665    Gibt Frame-Inhalte zwecks Debugging aus.

   666 */

   667 

   668 void ec_output_debug_data(const ec_master_t *master /**< EtherCAT-Master */)

   669 {

   670   unsigned int i;

   671 

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

   673          master->tx_data_length);

   674 

   675   printk(KERN_DEBUG);

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

   677   {

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

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

   680   }

   681   printk("\n");

   682 

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

   684          master->rx_data_length);

   685 

   686   printk(KERN_DEBUG);

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

   688   {

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

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

   691   }

   692   printk("\n");

   703   unsigned long int t;

   239     unsigned long int t;

   704 

   240 

   705   if (master->frames_lost) {

   241     if (master->frames_lost) {

   706     rdtscl(t);

   242         rdtscl(t);

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

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

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

   244             printk(KERN_ERR "EtherCAT: %u frame(s) LOST!\n",

   709       master->frames_lost = 0;

   245                    master->frames_lost);

   710       master->t_lost_output = t;

   246             master->frames_lost = 0;

   711     }

   247             master->t_lost_output = t;

   712   }

   248         }

   249     }

   736   unsigned long first, second;

   273     unsigned long first, second;

   737   char *remainder, *remainder2;

   274     char *remainder, *remainder2;

   738   unsigned int i;

   275     unsigned int i;

   739   int coupler_idx, slave_idx;

   276     int coupler_idx, slave_idx;

   740   ec_slave_t *slave;

   277     ec_slave_t *slave;

   741 

   278 

   742   if (!address || address[0] == 0) return NULL;

   279     if (!address || address[0] == 0) return NULL;

   743 

   280 

   744   if (address[0] == '#') {

   281     if (address[0] == '#') {

   745     printk(KERN_ERR "EtherCAT: Bus ID - #<SSID> not implemented yet!\n");

   282         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - #<SSID> not implemented"

   283                " yet!\n", address);

   284         return NULL;

   285     }

   286 

   287     first = simple_strtoul(address, &remainder, 0);

   288     if (remainder == address) {

   289         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - First number empty!\n",

   290                address);

   291         return NULL;

   292     }

   293 

   294     if (!remainder[0]) { // absolute position

   295         if (first < master->slave_count) {

   296             return master->slaves + first;

   297         }

   298 

   299         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Absolute position"

   300                " illegal!\n", address);

   301     }

   302 

   303     else if (remainder[0] == ':') { // field position

   304 

   305         remainder++;

   306         second = simple_strtoul(remainder, &remainder2, 0);

   307 

   308         if (remainder2 == remainder) {

   309             printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Sencond number"

   310                    " empty!\n", address);

   311             return NULL;

   312         }

   313 

   314         if (remainder2[0]) {

   315             printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Illegal trailer"

   316                    " (2)!\n", address);

   317             return NULL;

   318         }

   319 

   320         coupler_idx = -1;

   321         slave_idx = 0;

   322         for (i = 0; i < master->slave_count; i++, slave_idx++) {

   323             slave = master->slaves + i;

   324             if (!slave->type) continue;

   325 

   326             if (strcmp(slave->type->vendor_name, "Beckhoff") == 0 &&

   327                 strcmp(slave->type->product_name, "EK1100") == 0) {

   328                 coupler_idx++;

   329                 slave_idx = 0;

   330             }

   331 

   332             if (coupler_idx == first && slave_idx == second) return slave;

   333         }

   334     }

   335 

   336     else {

   337         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Illegal trailer!\n",

   338                address);

   339     }

   340 

   747   }

   342 }

   748 

   343 

   749   first = simple_strtoul(address, &remainder, 0);

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

   750   if (remainder == address) {

   345 

   751     printk(KERN_ERR "EtherCAT: Bus ID - First number empty!\n");

   346 /**

   752     return NULL;

   347    Initialisiert eine Sync-Manager-Konfigurationsseite.

   753   }

   348 

   754 

   349    Der mit \a data referenzierte Speicher muss mindestens EC_SYNC_SIZE Bytes

   755   if (!remainder[0]) { // absolute position

   350    groß sein.

   756     if (first < master->bus_slaves_count) {

   351 */

   757       return master->bus_slaves + first;

   352 

   758     }

   353 void ec_sync_config(const ec_sync_t *sync, /**< Sync-Manager */

   759 

   354                     uint8_t *data /**> Zeiger auf Konfigurationsspeicher */

   760     printk(KERN_ERR "EtherCAT: Bus ID - Absolute position illegal!\n");

   355                     )

   761   }

   356 {

   762 

   357     data[0] = sync->physical_start_address & 0xFF;

   763   else if (remainder[0] == ':') { // field position

   358     data[1] = (sync->physical_start_address >> 8) & 0xFF;

   764 

   359     data[2] = sync->size & 0xFF;

   765     remainder++;

   360     data[3] = (sync->size >> 8) & 0xFF;

   766     second = simple_strtoul(remainder, &remainder2, 0);

   361     data[4] = sync->control_byte;

   767 

   362     data[5] = 0x00;

   768     if (remainder2 == remainder) {

   363     data[6] = 0x01; // enable

   769       printk(KERN_ERR "EtherCAT: Bus ID - Sencond number empty!\n");

   364     data[7] = 0x00;

   770       return NULL;

   365 }

   771     }

   366 

   772 

   367 /*****************************************************************************/

   773     if (remainder2[0]) {

   368 

   774       printk(KERN_ERR "EtherCAT: Bus ID - Illegal trailer (2)!\n");

   369 /**

   775       return NULL;

   370    Initialisiert eine FMMU-Konfigurationsseite.

   776     }

   371 

   777 

   372    Der mit \a data referenzierte Speicher muss mindestens EC_FMMU_SIZE Bytes

   778     coupler_idx = -1;

   373    groß sein.

   779     slave_idx = 0;

   374 */

   780     for (i = 0; i < master->bus_slaves_count; i++, slave_idx++) {

   375 

   781       slave = master->bus_slaves + i;

   376 void ec_fmmu_config(const ec_fmmu_t *fmmu, /**< Sync-Manager */

   782       if (!slave->type) continue;

   377                     uint8_t *data /**> Zeiger auf Konfigurationsspeicher */

   783 

   378                     )

   784       if (strcmp(slave->type->vendor_name, "Beckhoff") == 0 &&

   379 {

   785           strcmp(slave->type->product_name, "EK1100") == 0) {

   380     data[0] = fmmu->logical_start_address & 0xFF;

   786         coupler_idx++;

   381     data[1] = (fmmu->logical_start_address >> 8) & 0xFF;

   787         slave_idx = 0;

   382     data[2] = (fmmu->logical_start_address >> 16) & 0xFF;

   788       }

   383     data[3] = (fmmu->logical_start_address >> 24) & 0xFF;

   789 

   384     data[4] = fmmu->sync->size & 0xFF;

   790       if (coupler_idx == first && slave_idx == second) return slave;

   385     data[5] = (fmmu->sync->size >> 8) & 0xFF;

   791     }

   386     data[6] = 0x00; // Logical start bit

   792   }

   387     data[7] = 0x07; // Logical end bit

   793 

   388     data[8] = fmmu->sync->physical_start_address & 0xFF;

   794   else {

   389     data[9] = (fmmu->sync->physical_start_address >> 8) & 0xFF;

   795     printk(KERN_ERR "EtherCAT: Bus ID - Illegal trailer!\n");

   390     data[10] = 0x00; // Physical start bit

   796   }

   391     data[11] = (fmmu->sync->control_byte & 0x04) ? 0x02 : 0x01;

   797 

   392     data[12] = 0x01; // Enable

   798   return NULL;

   393     data[13] = 0x00; // res.

   394     data[14] = 0x00; // res.

   395     data[15] = 0x00; // res.

   808    Registriert einen Slave beim Master.

   405    Registriert eine neue Domäne.

   809 

   406 

   810    \return Zeiger auf den Slave bei Erfolg, sonst NULL

   407    \return Zeiger auf die Domäne bei Erfolg, sonst NULL.

   811 */

   408 */

   812 

   409 

   813 ec_slave_t *EtherCAT_rt_register_slave(ec_master_t *master,

   410 ec_domain_t *EtherCAT_rt_master_register_domain(ec_master_t *master,

   814                                        /**< EtherCAT-Master */

   411                                                 /**< Domäne */

   815                                        const char *address,

   412                                                 ec_domain_mode_t mode,

   816                                        /**< ASCII-Addresse des Slaves, siehe

   413                                                 /**< Modus */

   817                                           auch ec_address() */

   414                                                 unsigned int timeout_us

   818                                        const char *vendor_name,

   415                                                 /**< Timeout */

   819                                        /**< Herstellername */

   416                                                 )

   820                                        const char *product_name,

   417 {

   821                                        /**< Produktname */

   418     ec_domain_t *domain;

   822                                        int domain

   419 

   823                                        /**< Domäne */

   420     if (master->domain_count >= EC_MASTER_MAX_DOMAINS) {

   824                                        )

   421         printk(KERN_ERR "EtherCAT: Maximum number of domains reached!\n");

   825 {

   422         return NULL;

   826   ec_slave_t *slave;

   423     }

   827   const ec_slave_type_t *type;

   424 

   828   ec_domain_t *dom;

   425     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {

   829   unsigned int j;

   426         printk(KERN_ERR "EthertCAT: Error allocating domain memory!\n");

   830 

   427         return NULL;

   831   if (domain < 0) {

   428     }

   832     printk(KERN_ERR "EtherCAT: Invalid domain: %i\n", domain);

   429 

   833     return NULL;

   430     ec_domain_init(domain, master, mode, timeout_us);

   834   }

   431     master->domains[master->domain_count] = domain;

   835 

   836   if ((slave = ec_address(master, address)) == NULL) {

   837     printk(KERN_ERR "EtherCAT: Illegal address: \"%s\"\n", address);

   838     return NULL;

   839   }

   840 

   841   if (slave->registered) {

   842     printk(KERN_ERR "EtherCAT: Slave \"%s\" (position %i) has already been"

   843            " registered!\n", address, slave->ring_position * (-1));

   844     return NULL;

   845   }

   846 

   847   if (!slave->type) {

   848     printk(KERN_ERR "EtherCAT: Slave \"%s\" (position %i) has unknown type!\n",

   849            address, slave->ring_position * (-1));

   850     return NULL;

   851   }

   852 

   853   type = slave->type;

   854 

   855   if (strcmp(vendor_name, type->vendor_name) ||

   856       strcmp(product_name, type->product_name)) {

   857     printk(KERN_ERR "Invalid Slave Type! Requested: \"%s %s\", found: \"%s"

   858            " %s\".\n", vendor_name, product_name, type->vendor_name,

   859            type->product_name);

   860     return NULL;

   861   }

   862 

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

   864   dom = NULL;

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

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

   867       dom = master->domains + j;

   868       break;

   869     }

   870   }

   871 

   872   // Create process data domain

   873   if (!dom) {

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

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

   876       return NULL;

   877     }

   878 

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

   880     ec_domain_init(dom);

   881     dom->number = domain;

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

   884   }

   433 

   885 

   434     return domain;

   886   if (dom->data_size + type->process_data_size > EC_FRAME_SIZE - 14) {

   435 }

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

   888            dom->number, dom->data_size + type->process_data_size,

   889            EC_FRAME_SIZE - 14);

   890     return NULL;

   891   }

   892 

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

   894   slave->logical_address = dom->data_size;

   895   slave->registered = 1;

   896 

   897   dom->data_size += type->process_data_size;

   898 

   899   return slave;

   900 }

   901 

   902 /*****************************************************************************/

   903 

   904 /**

   905    Registriert eine ganze Liste von Slaves beim Master.

   906 

   907    \return 0 bei Erfolg, sonst < 0

   908 */

   909 

   910 int EtherCAT_rt_register_slave_list(ec_master_t *master,

   911                                     /**< EtherCAT-Master */

   912                                     const ec_slave_init_t *slaves,

   913                                     /**< Array von Slave-Initialisierungs-

   914                                        strukturen */

   915                                     unsigned int count

   916                                     /**< Anzahl der Strukturen in \a slaves */

   917                                     )

   918 {

   919   unsigned int i;

   920 

   921   for (i = 0; i < count; i++)

   922   {

   923     if ((*(slaves[i].slave_ptr) =

   924          EtherCAT_rt_register_slave(master, slaves[i].address,

   925                                     slaves[i].vendor_name,

   926                                     slaves[i].product_name,

   927                                     slaves[i].domain)) == NULL)

   928       return -1;

   929   }

   930 

   931   return 0;

   932 }

   933 

   934 /*****************************************************************************/

   946 int EtherCAT_rt_activate_slaves(ec_master_t *master /**< EtherCAT-Master */)

   447 int EtherCAT_rt_master_activate(ec_master_t *master /**< EtherCAT-Master */)

   947 {

   448 {

   948   unsigned int i;

   449     unsigned int i, j;

   949   ec_slave_t *slave;

   450     ec_slave_t *slave;

   950   ec_command_t cmd;

   451     ec_frame_t frame;

   951   const ec_slave_type_t *type;

   452     const ec_sync_t *sync;

   952   unsigned char fmmu[16];

   453     const ec_slave_type_t *type;

   953   unsigned char data[256];

   454     const ec_fmmu_t *fmmu;

   954 

   455     uint8_t data[256];

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

   456     uint32_t domain_offset;

   956   {

   457 

   957     slave = master->bus_slaves + i;

   458     // Domains erstellen

   958 

   459     domain_offset = 0;

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

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

   960       return -1;

   461         ec_domain_t *domain = master->domains[i];

   961 

   462         if (ec_domain_alloc(domain, domain_offset)) {

   962     // Check if slave was registered...

   463             printk(KERN_INFO "EtherCAT: Failed to allocate domain %i!\n", i);

   963     if (!slave->registered) {

   464             return -1;

   964       printk(KERN_INFO "EtherCAT: Slave %i was not registered.\n", i);

   465         }

   965       continue;

   466         printk(KERN_INFO "EtherCAT: Domain %i - Allocated %i bytes (%i"

   966     }

   467                " Frame(s))\n", i, domain->data_size,

   967 

   468                domain->data_size / EC_MAX_FRAME_SIZE + 1);

   968     type = slave->type;

   469         domain_offset += domain->data_size;

   969 

   470     }

   970     // Resetting FMMU's

   471 

   971 

   472     // Slaves aktivieren

   972     memset(data, 0x00, 256);

   473     for (i = 0; i < master->slave_count; i++)

   973 

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

   975 

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

   977       return -1;

   978 

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

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

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

   982       return -1;

   983     }

   984 

   985     // Resetting Sync Manager channels

   986 

   987     if (type->features != EC_NOSYNC_SLAVE)

   989       memset(data, 0x00, 256);

   475         slave = master->slaves + i;

   990 

   476 

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

   477         // Change state to INIT

   992 

   478         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_INIT)))

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

   479             return -1;

   994         return -1;

   480 

   995 

   481         // Check if slave was registered...

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

   482         if (!slave->type || !slave->registered) {

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

   483             printk(KERN_INFO "EtherCAT: Slave %i was not registered.\n", i);

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

   484             continue;

   999         return -1;

   485         }

  1000       }

   486 

  1001     }

   487         type = slave->type;

  1002 

   488 

  1003     // Init Mailbox communication

   489         // Resetting FMMU's

  1004 

   490         if (slave->base_fmmu_count) {

  1005     if (type->features == EC_MAILBOX_SLAVE)

   491             memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);

   492             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600,

   493                                EC_FMMU_SIZE * slave->base_fmmu_count, data);

   494             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

   495             if (unlikely(frame.working_counter != 1)) {

   496                 printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %i did"

   497                        " not respond!\n", slave->ring_position);

   498                 return -1;

   499             }

   500         }

   501 

   502         // Resetting Sync Manager channels

   503         if (slave->base_sync_count) {

   504             memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);

   505             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800,

   506                                EC_SYNC_SIZE * slave->base_sync_count, data);

   507             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

   508             if (unlikely(frame.working_counter != 1)) {

   509                 printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %i did not"

   510                        " respond!\n", slave->ring_position);

   511                 return -1;

   512             }

   513         }

   514 

   515         // Set Sync Managers

   516         for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++)

   517         {

   518             sync = type->sync_managers[j];

   519 

   520             ec_sync_config(sync, data);

   521             ec_frame_init_npwr(&frame, master, slave->station_address,

   522                                0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, data);

   523 

   524             if (unlikely(ec_frame_send_receive(&frame))) return -1;

   525 

   526             if (unlikely(frame.working_counter != 1)) {

   527                 printk(KERN_ERR "EtherCAT: Setting sync manager %i - Slave"

   528                        " %i did not respond!\n", j, slave->ring_position);

   529                 return -1;

   530             }

   531         }

   532 

   533         // Change state to PREOP

   534         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_PREOP)))

   535             return -1;

   536 

   537         // Set FMMUs

   538         for (j = 0; j < slave->fmmu_count; j++)

   539         {

   540             fmmu = &slave->fmmus[j];

   541 

   542             ec_fmmu_config(fmmu, data);

   543             ec_frame_init_npwr(&frame, master, slave->station_address,

   544                                0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE, data);

   545 

   546             if (unlikely(ec_frame_send_receive(&frame))) return -1;

   547 

   548             if (unlikely(frame.working_counter != 1)) {

   549                 printk(KERN_ERR "EtherCAT: Setting FMMU %i - Slave %i did not"

   550                        " respond!\n", j, slave->ring_position);

   551                 return -1;

   552             }

   553         }

   554 

   555         // Change state to SAVEOP

   556         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_SAVEOP)))

   557             return -1;

   558 

   559         // Change state to OP

   560         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_OP)))

   561             return -1;

   562     }

   563 

   564     return 0;

   565 }

   566 

   567 /*****************************************************************************/

   568 

   569 /**

   570    Setzt alle Slaves zurück in den Init-Zustand.

   571 

   572    \return 0 bei Erfolg, sonst < 0

   573 */

   574 

   575 int EtherCAT_rt_master_deactivate(ec_master_t *master /**< EtherCAT-Master */)

   576 {

   577     ec_slave_t *slave;

   578     unsigned int i;

   579 

   580     for (i = 0; i < master->slave_count; i++)

  1007       if (type->sm0)

   582         slave = master->slaves + i;

  1008       {

   583 

  1009         ec_command_write(&cmd, slave->station_address, 0x0800, 8, type->sm0);

   584         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_INIT) != 0))

  1010 

   585             return -1;

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

   586     }

  1012           return -1;

   587 

  1013 

   588     return 0;

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