master/datagram_pair.c
branchredundancy
changeset 2367 8527429b6137
parent 2269 1d0711235a61
child 2368 dd84ef164869
equal deleted inserted replaced
2366:5682b63f826a 2367:8527429b6137
    32    EtherCAT datagram pair methods.
    32    EtherCAT datagram pair methods.
    33 */
    33 */
    34 
    34 
    35 /*****************************************************************************/
    35 /*****************************************************************************/
    36 
    36 
       
    37 #include <linux/slab.h>
       
    38 
       
    39 #include "master.h"
    37 #include "datagram_pair.h"
    40 #include "datagram_pair.h"
    38 
    41 
    39 /*****************************************************************************/
    42 /*****************************************************************************/
    40 
    43 
    41 /** Datagram pair constructor.
    44 /** Datagram pair constructor.
    42  */
    45  */
    43 void ec_datagram_pair_init(
    46 int ec_datagram_pair_init(
    44         ec_datagram_pair_t *pair /**< Datagram pair. */
    47         ec_datagram_pair_t *pair, /**< Datagram pair. */
       
    48         ec_domain_t *domain, /**< Parent domain. */
       
    49         uint32_t logical_offset,
       
    50         uint8_t *data,
       
    51         size_t data_size, /**< Data size. */
       
    52         const unsigned int used[] /**< input/output use count. */
    45         )
    53         )
    46 {
    54 {
    47     unsigned int i;
    55     unsigned int dev_idx;
       
    56     int ret;
    48 
    57 
    49     INIT_LIST_HEAD(&pair->list);
    58     INIT_LIST_HEAD(&pair->list);
       
    59     pair->domain = domain;
    50 
    60 
    51     for (i = 0; i < EC_NUM_DEVICES; i++) {
    61     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
    52         ec_datagram_init(&pair->datagrams[i]);
    62         ec_datagram_init(&pair->datagrams[dev_idx]);
       
    63         snprintf(pair->datagrams[dev_idx].name,
       
    64                 EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index,
       
    65                 logical_offset, dev_idx ? "backup" : "main");
    53     }
    66     }
       
    67 
       
    68     pair->expected_working_counter = 0U;
       
    69 
       
    70     /* backup datagram has its own memory */
       
    71     ret = ec_datagram_prealloc(&pair->datagrams[EC_DEVICE_BACKUP],
       
    72             data_size);
       
    73     if (ret) {
       
    74         goto out_datagrams;
       
    75     }
       
    76 
       
    77     if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) {
       
    78         EC_MASTER_ERR(domain->master,
       
    79                 "Failed to allocate domain send buffer!\n");
       
    80         ret = -ENOMEM;
       
    81         goto out_datagrams;
       
    82     }
       
    83 
       
    84     /* The ec_datagram_lxx() calls below can not fail, because either the
       
    85      * datagram has external memory or it is preallocated. */
       
    86 
       
    87     if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs
       
    88         ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],
       
    89                 logical_offset, data_size, data);
       
    90         ec_datagram_lrw(&pair->datagrams[EC_DEVICE_BACKUP],
       
    91                 logical_offset, data_size);
       
    92 
       
    93         // If LRW is used, output FMMUs increment the working counter by 2,
       
    94         // while input FMMUs increment it by 1.
       
    95         pair->expected_working_counter =
       
    96             used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT];
       
    97     } else if (used[EC_DIR_OUTPUT]) { // outputs only
       
    98         ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],
       
    99                 logical_offset, data_size, data);
       
   100         ec_datagram_lwr(&pair->datagrams[EC_DEVICE_BACKUP],
       
   101                 logical_offset, data_size);
       
   102 
       
   103         pair->expected_working_counter = used[EC_DIR_OUTPUT];
       
   104     } else { // inputs only (or nothing)
       
   105         ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],
       
   106                 logical_offset, data_size, data);
       
   107         ec_datagram_lrd(&pair->datagrams[EC_DEVICE_BACKUP],
       
   108                 logical_offset, data_size);
       
   109 
       
   110         pair->expected_working_counter = used[EC_DIR_INPUT];
       
   111     }
       
   112 
       
   113     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
       
   114         ec_datagram_zero(&pair->datagrams[dev_idx]);
       
   115     }
       
   116 
       
   117     return 0;
       
   118 
       
   119 out_datagrams:
       
   120     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
       
   121         ec_datagram_clear(&pair->datagrams[dev_idx]);
       
   122     }
       
   123 
       
   124     return ret;
    54 }
   125 }
    55 
   126 
    56 /*****************************************************************************/
   127 /*****************************************************************************/
    57 
   128 
    58 /** Datagram pair destructor.
   129 /** Datagram pair destructor.
    59  */
   130  */
    60 void ec_datagram_pair_clear(
   131 void ec_datagram_pair_clear(
    61         ec_datagram_pair_t *pair /**< Datagram pair. */
   132         ec_datagram_pair_t *pair /**< Datagram pair. */
    62         )
   133         )
    63 {
   134 {
    64     unsigned int i;
   135     unsigned int dev_idx;
    65 
   136 
    66     for (i = 0; i < EC_NUM_DEVICES; i++) {
   137     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
    67         ec_datagram_clear(&pair->datagrams[i]);
   138         ec_datagram_clear(&pair->datagrams[dev_idx]);
       
   139     }
       
   140 
       
   141     if (pair->send_buffer) {
       
   142         kfree(pair->send_buffer);
    68     }
   143     }
    69 }
   144 }
    70 
   145 
    71 /*****************************************************************************/
   146 /*****************************************************************************/
       
   147 
       
   148 /** Process received data.
       
   149  */
       
   150 unsigned int ec_datagram_pair_process(
       
   151         ec_datagram_pair_t *pair /**< Datagram pair. */
       
   152         )
       
   153 {
       
   154     unsigned int dev_idx, wc_sum = 0;
       
   155 
       
   156     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
       
   157         ec_datagram_t *datagram = &pair->datagrams[dev_idx];
       
   158 
       
   159         ec_datagram_output_stats(datagram);
       
   160 
       
   161         if (datagram->state == EC_DATAGRAM_RECEIVED) {
       
   162             wc_sum += datagram->working_counter;
       
   163         }
       
   164     }
       
   165 
       
   166     return wc_sum;
       
   167 }
       
   168 
       
   169 /*****************************************************************************/
       
   170 
       
   171 /** Process received data.
       
   172  */
       
   173 int ec_datagram_pair_data_changed(
       
   174         const ec_datagram_pair_t *pair,
       
   175         size_t offset,
       
   176         size_t size,
       
   177         ec_device_index_t dev_idx
       
   178         )
       
   179 {
       
   180     uint8_t *sent = pair->send_buffer + offset;
       
   181     uint8_t *recv = pair->datagrams[dev_idx].data + offset;
       
   182     size_t i;
       
   183 
       
   184     for (i = 0; i < size; i++) {
       
   185         if (recv[i] != sent[i]) {
       
   186             return 1;
       
   187         }
       
   188     }
       
   189 
       
   190     return 0;
       
   191 }
       
   192 
       
   193 /*****************************************************************************/