Loop through datagrams, then FMMUs; redundancy flag in domain state. redundancy
authorFlorian Pose <fp@igh-essen.com>
Fri, 16 Mar 2012 18:24:29 +0100
branchredundancy
changeset 2368 dd84ef164869
parent 2367 8527429b6137
child 2369 2536d60dea6e
Loop through datagrams, then FMMUs; redundancy flag in domain state.
include/ecrt.h
master/cdev.c
master/datagram_pair.c
master/datagram_pair.h
master/domain.c
master/domain.h
--- a/include/ecrt.h	Fri Mar 16 16:04:26 2012 +0100
+++ b/include/ecrt.h	Fri Mar 16 18:24:29 2012 +0100
@@ -38,6 +38,10 @@
  * for realtime modules that want to use EtherCAT. There are functions to
  * request a master, to map process data, to communicate with slaves via CoE
  * and to configure and activate the bus.
+ * 
+ * Changed since 1.5:
+ *
+ * - Added redundancy_active flag to ec_domain_state_t.
  *
  * Changes in version 1.5:
  *
@@ -304,6 +308,7 @@
 typedef struct {
     unsigned int working_counter; /**< Value of the last working counter. */
     ec_wc_state_t wc_state; /**< Working counter interpretation. */
+    unsigned int redundancy_active; /**< Redundant link is in use. */
 } ec_domain_state_t;
 
 /*****************************************************************************/
--- a/master/cdev.c	Fri Mar 16 16:04:26 2012 +0100
+++ b/master/cdev.c	Fri Mar 16 18:24:29 2012 +0100
@@ -553,7 +553,9 @@
 
     data.data_size = domain->data_size;
     data.logical_base_address = domain->logical_base_address;
-    data.working_counter = domain->working_counter;
+    data.working_counter =
+        domain->working_counter[EC_DEVICE_MAIN]
+        + domain->working_counter[EC_DEVICE_BACKUP];
     data.expected_working_counter = domain->expected_working_counter;
     data.fmmu_count = ec_domain_fmmu_count(domain);
 
--- a/master/datagram_pair.c	Fri Mar 16 16:04:26 2012 +0100
+++ b/master/datagram_pair.c	Fri Mar 16 18:24:29 2012 +0100
@@ -147,11 +147,13 @@
 
 /** Process received data.
  */
-unsigned int ec_datagram_pair_process(
-        ec_datagram_pair_t *pair /**< Datagram pair. */
+uint16_t ec_datagram_pair_process(
+        ec_datagram_pair_t *pair, /**< Datagram pair. */
+        uint16_t wc_sum[EC_NUM_DEVICES] /**< Working counter sums. */
         )
 {
-    unsigned int dev_idx, wc_sum = 0;
+    unsigned int dev_idx;
+    uint16_t pair_wc = 0;
 
     for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {
         ec_datagram_t *datagram = &pair->datagrams[dev_idx];
@@ -159,35 +161,12 @@
         ec_datagram_output_stats(datagram);
 
         if (datagram->state == EC_DATAGRAM_RECEIVED) {
-            wc_sum += datagram->working_counter;
+            pair_wc += datagram->working_counter;
+            wc_sum[dev_idx] += datagram->working_counter;
         }
     }
 
-    return wc_sum;
+    return pair_wc;
 }
 
 /*****************************************************************************/
-
-/** Process received data.
- */
-int ec_datagram_pair_data_changed(
-        const ec_datagram_pair_t *pair,
-        size_t offset,
-        size_t size,
-        ec_device_index_t dev_idx
-        )
-{
-    uint8_t *sent = pair->send_buffer + offset;
-    uint8_t *recv = pair->datagrams[dev_idx].data + offset;
-    size_t i;
-
-    for (i = 0; i < size; i++) {
-        if (recv[i] != sent[i]) {
-            return 1;
-        }
-    }
-
-    return 0;
-}
-
-/*****************************************************************************/
--- a/master/datagram_pair.h	Fri Mar 16 16:04:26 2012 +0100
+++ b/master/datagram_pair.h	Fri Mar 16 18:24:29 2012 +0100
@@ -61,9 +61,8 @@
         uint8_t *, size_t, const unsigned int []);
 void ec_datagram_pair_clear(ec_datagram_pair_t *);
 
-unsigned int ec_datagram_pair_process(ec_datagram_pair_t *);
-int ec_datagram_pair_data_changed(const ec_datagram_pair_t *,
-                    size_t, size_t, ec_device_index_t);
+uint16_t ec_datagram_pair_process(ec_datagram_pair_t *,
+        uint16_t[EC_NUM_DEVICES]);
 
 /*****************************************************************************/
 
--- a/master/domain.c	Fri Mar 16 16:04:26 2012 +0100
+++ b/master/domain.c	Fri Mar 16 18:24:29 2012 +0100
@@ -67,9 +67,11 @@
     domain->data_origin = EC_ORIG_INTERNAL;
     domain->logical_base_address = 0x00000000;
     INIT_LIST_HEAD(&domain->datagram_pairs);
-    domain->working_counter = 0x0000;
+    domain->working_counter[EC_DEVICE_MAIN] = 0x0000;
+    domain->working_counter[EC_DEVICE_BACKUP] = 0x0000;
     domain->expected_working_counter = 0x0000;
     domain->working_counter_changes = 0;
+    domain->redundancy_active = 0;
     domain->notify_jiffies = 0;
 }
 
@@ -351,6 +353,30 @@
     return NULL;
 }
 
+/*****************************************************************************/
+
+/** Process received data.
+ */
+int data_changed(
+        uint8_t *send_buffer,
+        const ec_datagram_t *datagram,
+        size_t offset,
+        size_t size
+        )
+{
+    uint8_t *sent = send_buffer + offset;
+    uint8_t *recv = datagram->data + offset;
+    size_t i;
+
+    for (i = 0; i < size; i++) {
+        if (recv[i] != sent[i]) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
 /******************************************************************************
  *  Application interface
  *****************************************************************************/
@@ -417,98 +443,118 @@
 
 void ecrt_domain_process(ec_domain_t *domain)
 {
-    uint16_t working_counter_sum;
-    ec_datagram_pair_t *datagram_pair = NULL;
-    ec_fmmu_config_t *fmmu;
+    uint16_t wc_sum[EC_NUM_DEVICES] = {};
+    ec_datagram_pair_t *pair;
+    ec_datagram_t *main_datagram, *backup_datagram;
     uint32_t logical_datagram_address;
-    unsigned int datagram_offset, datagram_pair_wc = 0;
     size_t datagram_size;
-    ec_datagram_t *main_datagram;
+    uint16_t datagram_pair_wc;
+    unsigned int datagram_offset;
+    ec_fmmu_config_t *fmmu =
+        list_first_entry(&domain->fmmu_configs, ec_fmmu_config_t, list);
+    unsigned int redundancy;
 
 #if DEBUG_REDUNDANCY
     EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index);
 #endif
 
-    working_counter_sum = 0;
-
-    if (!list_empty(&domain->datagram_pairs)) {
-        datagram_pair =
-            list_entry(domain->datagram_pairs.next, ec_datagram_pair_t, list);
-        main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN];
-
+    list_for_each_entry(pair, &domain->datagram_pairs, list) {
+
+        main_datagram = &pair->datagrams[EC_DEVICE_MAIN];
+        backup_datagram = &pair->datagrams[EC_DEVICE_BACKUP];
         logical_datagram_address = EC_READ_U32(main_datagram->address);
         datagram_size = main_datagram->data_size;
-        datagram_offset =
-            fmmu->logical_start_address - logical_datagram_address;
-        datagram_pair_wc = ec_datagram_pair_process(datagram_pair);
-        working_counter_sum += datagram_pair_wc;
+
 #if DEBUG_REDUNDANCY
         EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n",
                 main_datagram->name, logical_datagram_address);
 #endif
-    }
-
-    /* Go through all FMMU configs to detect data changes. */
-    list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
-#if DEBUG_REDUNDANCY
-        EC_MASTER_DBG(domain->master, 1, "fmmu log=%u size=%u dir=%u\n",
-                fmmu->logical_start_address, fmmu->data_size, fmmu->dir);
-#endif
-        if (fmmu->dir != EC_DIR_INPUT) {
-            continue;
-        }
-
-        logical_datagram_address =
-            EC_READ_U32(datagram_pair->datagrams[EC_DEVICE_MAIN].address);
-        datagram_size = datagram_pair->datagrams[EC_DEVICE_MAIN].data_size;
-        datagram_offset =
-            fmmu->logical_start_address - logical_datagram_address;
-        while (datagram_offset >= datagram_size) {
-
-            datagram_pair = list_entry(datagram_pair->list.next,
-                    ec_datagram_pair_t, list);
-            main_datagram = &datagram_pair->datagrams[EC_DEVICE_MAIN];
-
-            logical_datagram_address = EC_READ_U32(main_datagram->address);
-            datagram_size = main_datagram->data_size;
+
+        datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum);
+
+        /* Go through all FMMU configs to detect data changes. */
+        list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) {
+
+            if (fmmu->dir != EC_DIR_INPUT) {
+                continue;
+            }
+
+            if (fmmu->logical_start_address >=
+                    logical_datagram_address + datagram_size) {
+                // fmmu data contained in next datagram pair
+                break;
+            }
+
             datagram_offset =
                 fmmu->logical_start_address - logical_datagram_address;
-            datagram_pair_wc = ec_datagram_pair_process(datagram_pair);
-            working_counter_sum += datagram_pair_wc;
+
 #if DEBUG_REDUNDANCY
-            EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n",
-                    main_datagram->name, logical_datagram_address);
+            EC_MASTER_DBG(domain->master, 1,
+                    "input fmmu log=%u size=%u offset=%u\n",
+                    fmmu->logical_start_address, fmmu->data_size,
+                    datagram_offset);
+            if (domain->master->debug_level > 0) {
+                ec_print_data(pair->send_buffer + datagram_offset,
+                        fmmu->data_size);
+                ec_print_data(main_datagram->data + datagram_offset,
+                        fmmu->data_size);
+                ec_print_data(backup_datagram->data + datagram_offset,
+                        fmmu->data_size);
+            }
 #endif
-        }
-
+
+            if (data_changed(pair->send_buffer, main_datagram,
+                        datagram_offset, fmmu->data_size)) {
+                /* data changed on main link: no copying necessary. */
 #if DEBUG_REDUNDANCY
-        EC_MASTER_DBG(domain->master, 1, "input fmmu log=%u size=%u"
-                " using datagram %s offset=%u\n",
-                fmmu->logical_start_address, fmmu->data_size,
-                datagram_pair->datagrams[EC_DEVICE_MAIN].name,
-                datagram_offset);
+                EC_MASTER_DBG(domain->master, 1, "main changed\n");
 #endif
-
-        if (ec_datagram_pair_data_changed(datagram_pair,
-                    datagram_offset, fmmu->data_size, EC_DEVICE_MAIN)) {
-            /* data changed on main link. no copying necessary. */
-        } else if (ec_datagram_pair_data_changed(datagram_pair,
-                    datagram_offset, fmmu->data_size, EC_DEVICE_BACKUP)
-                || (datagram_pair_wc
-                == datagram_pair->expected_working_counter)) {
-            /* data changed on backup link or no change and complete WC.
-             * copy to main memory. */
-            uint8_t *target = datagram_pair->datagrams[EC_DEVICE_MAIN].data +
-                datagram_offset;
-            uint8_t *source = datagram_pair->datagrams[EC_DEVICE_BACKUP].data +
-                datagram_offset;
-            memcpy(target, source, fmmu->data_size);
-        }
-    }
-
-    if (working_counter_sum != domain->working_counter) {
+            } else if (data_changed(pair->send_buffer, backup_datagram,
+                        datagram_offset, fmmu->data_size)) {
+                /* data changed on backup link: copy to main memory. */
+#if DEBUG_REDUNDANCY
+                EC_MASTER_DBG(domain->master, 1, "backup changed\n");
+#endif
+                memcpy(main_datagram->data + datagram_offset,
+                        backup_datagram->data + datagram_offset,
+                        fmmu->data_size);
+            } else if (datagram_pair_wc == pair->expected_working_counter) {
+                /* no change, but WC complete: use main data. */
+#if DEBUG_REDUNDANCY
+                EC_MASTER_DBG(domain->master, 1, "no change but complete\n");
+#endif
+            } else {
+                /* no change and WC incomplete: mark WC as zero to avoid
+                 * data.dependent WC flickering. */
+                datagram_pair_wc = 0;
+#if DEBUG_REDUNDANCY
+                EC_MASTER_DBG(domain->master, 1,
+                        "no change and incomplete\n");
+#endif
+            }
+        }
+    }
+
+    redundancy = wc_sum[EC_DEVICE_BACKUP] > 0;
+    if (redundancy != domain->redundancy_active) {
+        if (redundancy) {
+            EC_MASTER_WARN(domain->master,
+                    "Domain %u: Redundant link in use!\n",
+                    domain->index);
+        } else {
+            EC_MASTER_INFO(domain->master,
+                    "Domain %u: Redundant link unused again.\n",
+                    domain->index);
+        }
+        domain->redundancy_active = redundancy;
+    }
+
+    if ((wc_sum[EC_DEVICE_MAIN] != domain->working_counter[EC_DEVICE_MAIN])
+            || (wc_sum[EC_DEVICE_BACKUP]
+                != domain->working_counter[EC_DEVICE_BACKUP])) {
         domain->working_counter_changes++;
-        domain->working_counter = working_counter_sum;
+        domain->working_counter[EC_DEVICE_MAIN] = wc_sum[EC_DEVICE_MAIN];
+        domain->working_counter[EC_DEVICE_BACKUP] = wc_sum[EC_DEVICE_BACKUP];
     }
 
     if (domain->working_counter_changes &&
@@ -516,14 +562,19 @@
         domain->notify_jiffies = jiffies;
         if (domain->working_counter_changes == 1) {
             EC_MASTER_INFO(domain->master, "Domain %u: Working counter"
-                    " changed to %u/%u.\n", domain->index,
-                    domain->working_counter,
-                    domain->expected_working_counter);
+                    " changed to %u/%u (%u+%u).\n", domain->index,
+                    domain->working_counter[EC_DEVICE_MAIN] +
+                    domain->working_counter[EC_DEVICE_BACKUP],
+                    domain->expected_working_counter,
+                    wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]);
         } else {
             EC_MASTER_INFO(domain->master, "Domain %u: %u working counter"
-                    " changes - now %u/%u.\n", domain->index,
-                    domain->working_counter_changes, domain->working_counter,
-                    domain->expected_working_counter);
+                    " changes - now %u/%u (%u+%u).\n", domain->index,
+                    domain->working_counter_changes,
+                    domain->working_counter[EC_DEVICE_MAIN] +
+                    domain->working_counter[EC_DEVICE_BACKUP],
+                    domain->expected_working_counter,
+                    wc_sum[EC_DEVICE_MAIN], wc_sum[EC_DEVICE_BACKUP]);
         }
         domain->working_counter_changes = 0;
     }
@@ -559,10 +610,12 @@
 
 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
 {
-    state->working_counter = domain->working_counter;
-
-    if (domain->working_counter) {
-        if (domain->working_counter == domain->expected_working_counter) {
+    state->working_counter =
+        domain->working_counter[EC_DEVICE_MAIN]
+        + domain->working_counter[EC_DEVICE_BACKUP];
+
+    if (state->working_counter) {
+        if (state->working_counter == domain->expected_working_counter) {
             state->wc_state = EC_WC_COMPLETE;
         } else {
             state->wc_state = EC_WC_INCOMPLETE;
@@ -570,6 +623,8 @@
     } else {
         state->wc_state = EC_WC_ZERO;
     }
+
+    state->redundancy_active = domain->redundancy_active;
 }
 
 /*****************************************************************************/
--- a/master/domain.h	Fri Mar 16 16:04:26 2012 +0100
+++ b/master/domain.h	Fri Mar 16 18:24:29 2012 +0100
@@ -66,10 +66,12 @@
     struct list_head datagram_pairs; /**< Datagrams pairs (main/backup) for
                                        process data exchange. */
 
-    uint16_t working_counter; /**< Last working counter value. */
+    uint16_t working_counter[EC_NUM_DEVICES]; /**< Last working counter
+                                                values. */
     uint16_t expected_working_counter; /**< Expected working counter. */
     unsigned int working_counter_changes; /**< Working counter changes
                                              since last notification. */
+    unsigned int redundancy_active; /**< Non-zero, if redundancy is in use. */
     unsigned long notify_jiffies; /**< Time of last notification. */
 };