master/frame.c
changeset 77 677967864795
parent 73 9f4ea66d89a3
child 78 3d74183d6c6b
equal deleted inserted replaced
76:9dc136e3801c 77:677967864795
     9  *****************************************************************************/
     9  *****************************************************************************/
    10 
    10 
    11 #include <linux/slab.h>
    11 #include <linux/slab.h>
    12 #include <linux/delay.h>
    12 #include <linux/delay.h>
    13 
    13 
       
    14 #include "../include/EtherCAT_si.h"
    14 #include "frame.h"
    15 #include "frame.h"
    15 #include "master.h"
    16 #include "master.h"
    16 
    17 
    17 /*****************************************************************************/
    18 /*****************************************************************************/
    18 
    19 
    25 #define EC_FUNC_WRITE_FOOTER \
    26 #define EC_FUNC_WRITE_FOOTER \
    26     frame->data_length = length; \
    27     frame->data_length = length; \
    27     memcpy(frame->data, data, length);
    28     memcpy(frame->data, data, length);
    28 
    29 
    29 #define EC_FUNC_READ_FOOTER \
    30 #define EC_FUNC_READ_FOOTER \
    30     frame->data_length = length;
    31     frame->data_length = length; \
       
    32     memset(frame->data, 0x00, length);
    31 
    33 
    32 /*****************************************************************************/
    34 /*****************************************************************************/
    33 
    35 
    34 /**
    36 /**
    35    Initialisiert ein EtherCAT-NPRD-Kommando.
    37    Initialisiert ein EtherCAT-NPRD-Kommando.
   287 
   289 
   288     // Zeiger auf Socket-Buffer holen
   290     // Zeiger auf Socket-Buffer holen
   289     data = ec_device_prepare(&frame->master->device);
   291     data = ec_device_prepare(&frame->master->device);
   290 
   292 
   291     // EtherCAT frame header
   293     // EtherCAT frame header
   292     data[0] = command_size & 0xFF;
   294     EC_WRITE_U16(data, (command_size & 0x7FF) | 0x1000);
   293     data[1] = ((command_size & 0x700) >> 8) | 0x10;
       
   294     data += EC_FRAME_HEADER_SIZE;
   295     data += EC_FRAME_HEADER_SIZE;
   295 
   296 
   296     // EtherCAT command header
   297     // EtherCAT command header
   297     data[0] = frame->type;
   298     EC_WRITE_U8 (data,     frame->type);
   298     data[1] = frame->index;
   299     EC_WRITE_U8 (data + 1, frame->index);
   299     data[2] = frame->address.raw[0];
   300     EC_WRITE_U32(data + 2, frame->address.logical);
   300     data[3] = frame->address.raw[1];
   301     EC_WRITE_U16(data + 6, frame->data_length & 0x7FF);
   301     data[4] = frame->address.raw[2];
   302     EC_WRITE_U16(data + 8, 0x0000);
   302     data[5] = frame->address.raw[3];
       
   303     data[6] = frame->data_length & 0xFF;
       
   304     data[7] = (frame->data_length & 0x700) >> 8;
       
   305     data[8] = 0x00;
       
   306     data[9] = 0x00;
       
   307     data += EC_COMMAND_HEADER_SIZE;
   303     data += EC_COMMAND_HEADER_SIZE;
   308 
   304 
   309     if (likely(frame->type == ec_frame_type_apwr // Write commands
   305     // EtherCAT command data
   310                || frame->type == ec_frame_type_npwr
   306     memcpy(data, frame->data, frame->data_length);
   311                || frame->type == ec_frame_type_bwr
   307     data += frame->data_length;
   312                || frame->type == ec_frame_type_lrw)) {
       
   313         memcpy(data, frame->data, frame->data_length);
       
   314     }
       
   315     else { // Read commands
       
   316         memset(data, 0x00, frame->data_length);
       
   317     }
       
   318 
   308 
   319     // EtherCAT command footer
   309     // EtherCAT command footer
   320     data += frame->data_length;
   310     EC_WRITE_U16(data, frame->working_counter);
   321     data[0] = frame->working_counter & 0xFF;
       
   322     data[1] = (frame->working_counter & 0xFF00) >> 8;
       
   323     data += EC_COMMAND_FOOTER_SIZE;
   311     data += EC_COMMAND_FOOTER_SIZE;
   324 
   312 
   325     // Pad with zeros
   313     // Pad with zeros
   326     for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
   314     for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
   327              + frame->data_length + EC_COMMAND_FOOTER_SIZE;
   315              + frame->data_length + EC_COMMAND_FOOTER_SIZE;
   328          i < EC_MIN_FRAME_SIZE; i++) {
   316          i < EC_MIN_FRAME_SIZE; i++)
   329         *data++ = 0x00;
   317         EC_WRITE_U8(data++, 0x00);
   330     }
       
   331 
   318 
   332     // Send frame
   319     // Send frame
   333     ec_device_send(&frame->master->device, frame_size);
   320     ec_device_send(&frame->master->device, frame_size);
   334 
   321 
   335     return 0;
   322     return 0;
   369     }
   356     }
   370 
   357 
   371     data = ec_device_data(device);
   358     data = ec_device_data(device);
   372 
   359 
   373     // Länge des gesamten Frames prüfen
   360     // Länge des gesamten Frames prüfen
   374     frame_length = (data[0] & 0xFF) | ((data[1] & 0x07) << 8);
   361     frame_length = EC_READ_U16(data) & 0x07FF;
       
   362     data += EC_FRAME_HEADER_SIZE;
   375 
   363 
   376     if (unlikely(frame_length > received_length)) {
   364     if (unlikely(frame_length > received_length)) {
   377         printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"
   365         printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"
   378                " not match)!\n");
   366                " not match)!\n");
   379         ec_frame_print(frame);
   367         ec_frame_print(frame);
   380         return -1;
   368         return -1;
   381     }
   369     }
   382 
   370 
   383     // Command header
   371     // Command header
   384     data += EC_FRAME_HEADER_SIZE;
   372     command_type =  EC_READ_U8(data);
   385     command_type = data[0];
   373     command_index = EC_READ_U8(data + 1);
   386     command_index = data[1];
   374     data_length =   EC_READ_U16(data + 6) & 0x07FF;
   387     data_length = (data[6] & 0xFF) | ((data[7] & 0x07) << 8);
   375     data += EC_COMMAND_HEADER_SIZE;
   388 
   376 
   389     if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
   377     if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE
   390                  + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) {
   378                  + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) {
   391         printk(KERN_ERR "EtherCAT: Received frame with incomplete command"
   379         printk(KERN_ERR "EtherCAT: Received frame with incomplete command"
   392                " data!\n");
   380                " data!\n");
   397     if (unlikely(frame->type != command_type
   385     if (unlikely(frame->type != command_type
   398                  || frame->index != command_index
   386                  || frame->index != command_index
   399                  || frame->data_length != data_length))
   387                  || frame->data_length != data_length))
   400     {
   388     {
   401         printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");
   389         printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");
   402         ec_frame_print(frame);
   390         ec_frame_print(frame); // FIXME uninteressant...
   403         ec_device_call_isr(device); // Empfangenes "vergessen"
   391         ec_device_call_isr(device); // Empfangenes "vergessen"
   404         return -1;
   392         return -1;
   405     }
   393     }
   406 
   394 
   407     frame->state = ec_frame_received;
   395     frame->state = ec_frame_received;
   408 
   396 
   409     // Empfangene Daten in Kommandodatenspeicher kopieren
   397     // Empfangene Daten in Kommandodatenspeicher kopieren
   410     data += EC_COMMAND_HEADER_SIZE;
       
   411     memcpy(frame->data, data, data_length);
   398     memcpy(frame->data, data, data_length);
   412     data += data_length;
   399     data += data_length;
   413 
   400 
   414     // Working-Counter setzen
   401     // Working-Counter setzen
   415     frame->working_counter = (data[0] & 0xFF) | ((data[1] & 0xFF) << 8);
   402     frame->working_counter = EC_READ_U16(data);
   416 
   403 
   417     if (unlikely(frame->master->debug_level > 1)) {
   404     if (unlikely(frame->master->debug_level > 1)) {
   418         ec_frame_print(frame);
   405         ec_frame_print(frame);
   419     }
   406     }
   420 
   407