Inplace I/O: let input Pdos use the same bus space as Output Pdos
authorMartin Troxler <martin.troxler@komaxgroup.com>
Thu, 29 Apr 2010 16:33:32 +0200
changeset 1981 c14b6bb14fdf
parent 1980 a89e2bedf004
child 1982 7421bca39b6d
Inplace I/O: let input Pdos use the same bus space as Output Pdos
include/ecrt.h
lib/slave_config.c
master/cdev.c
master/datagram.c
master/datagram.h
master/domain.c
master/domain.h
master/fmmu_config.c
master/fmmu_config.h
master/ioctl.h
master/master.c
master/slave_config.c
master/slave_config.h
--- a/include/ecrt.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/include/ecrt.h	Thu Apr 29 16:33:32 2010 +0200
@@ -78,6 +78,9 @@
  * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations,
  *   and ecrt_master_read_idn() and ecrt_master_write_idn() to read/write IDNs
  *   ad-hoc via the user-space library.
+ * - Added support for overlapping PDOs which allows inputs to use the same
+ *   space as outputs on the frame. This reduces the frame length.
+ *
  *
  * @{
  */
@@ -891,6 +894,17 @@
                                      */
         );
 
+/** Configure wether a slave allows overlapping PDOs.
+ *
+ * Overlapping PDOs allows inputs to use the same space as outputs on the frame.
+ * This reduces the frame length.
+ */
+void ecrt_slave_config_overlapping_pdos(
+        ec_slave_config_t *sc, /**< Slave configuration. */
+        uint8_t allow_overlapping_pdos /**< Allow overlapping PDOs */
+        );
+
+
 /** Add a PDO to a sync manager's PDO assignment.
  *
  * \see ecrt_slave_config_pdos()
--- a/lib/slave_config.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/lib/slave_config.c	Thu Apr 29 16:33:32 2010 +0200
@@ -87,6 +87,23 @@
 
 /*****************************************************************************/
 
+void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc,
+        uint8_t allow_overlapping_pdos)
+{
+    ec_ioctl_config_t data;
+
+    memset(&data, 0x00, sizeof(ec_ioctl_config_t));
+    data.config_index = sc->index;
+    data.allow_overlapping_pdos = allow_overlapping_pdos;
+
+    if (ioctl(sc->master->fd, EC_IOCTL_SC_OVERLAPPING_IO, &data) == -1) {
+        fprintf(stderr, "Failed to config overlapping PDOs: %s\n",
+                strerror(errno));
+    }
+}
+
+/*****************************************************************************/
+
 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
         uint8_t sync_index, uint16_t pdo_index)
 {
--- a/master/cdev.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/cdev.c	Thu Apr 29 16:33:32 2010 +0200
@@ -2031,6 +2031,50 @@
     return ret;
 }
 
+
+/*****************************************************************************/
+
+/** Configure wether a slave allows overlapping PDOs.
+ */
+int ec_cdev_ioctl_sc_allow_overlapping_pdos(
+        ec_master_t *master, /**< EtherCAT master. */
+        unsigned long arg, /**< ioctl() argument. */
+        ec_cdev_priv_t *priv /**< Private data structure of file handle. */
+        )
+{
+    ec_ioctl_config_t data;
+    ec_slave_config_t *sc;
+    int ret = 0;
+
+    if (unlikely(!priv->requested)) {
+        ret = -EPERM;
+        goto out_return;
+    }
+
+    if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+        ret = -EFAULT;
+        goto out_return;
+    }
+
+    if (down_interruptible(&master->master_sem)) {
+        ret = -EINTR;
+        goto out_return;
+    }
+
+    if (!(sc = ec_master_get_config(master, data.config_index))) {
+        ret = -ENOENT;
+        goto out_up;
+    }
+
+    ecrt_slave_config_overlapping_pdos(sc,
+            data.allow_overlapping_pdos);
+
+out_up:
+    up(&master->master_sem);
+out_return:
+    return ret;
+}
+
 /*****************************************************************************/
 
 /** Add a PDO to the assignment.
@@ -3725,6 +3769,10 @@
             if (!(filp->f_mode & FMODE_WRITE))
                 return -EPERM;
             return ec_cdev_ioctl_sc_watchdog(master, arg, priv);
+        case EC_IOCTL_SC_OVERLAPPING_IO:
+            if (!(filp->f_mode & FMODE_WRITE))
+                return -EPERM;
+            return ec_cdev_ioctl_sc_allow_overlapping_pdos(master,arg,priv);
         case EC_IOCTL_SC_ADD_PDO:
             if (!(filp->f_mode & FMODE_WRITE))
                 return -EPERM;
--- a/master/datagram.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/datagram.c	Thu Apr 29 16:33:32 2010 +0200
@@ -94,6 +94,7 @@
     datagram->data_origin = EC_ORIG_INTERNAL;
     datagram->mem_size = 0;
     datagram->data_size = 0;
+    datagram->domain = NULL;
     datagram->index = 0x00;
     datagram->working_counter = 0x0000;
     datagram->state = EC_DATAGRAM_INIT;
--- a/master/datagram.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/datagram.h	Thu Apr 29 16:33:32 2010 +0200
@@ -94,6 +94,7 @@
     ec_origin_t data_origin; /**< Origin of the \a data memory. */
     size_t mem_size; /**< Datagram \a data memory size. */
     size_t data_size; /**< Size of the data in \a data. */
+    ec_domain_t *domain; /**< Owning domain (may be null for non-domain datagrams) */
     uint8_t index; /**< Index (set by master). */
     uint16_t working_counter; /**< Working counter. */
     ec_datagram_state_t state; /**< State. */
--- a/master/domain.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/domain.c	Thu Apr 29 16:33:32 2010 +0200
@@ -60,6 +60,7 @@
     domain->index = index;
     INIT_LIST_HEAD(&domain->fmmu_configs);
     domain->data_size = 0;
+    domain->tx_size = 0;
     domain->data = NULL;
     domain->data_origin = EC_ORIG_INTERNAL;
     domain->logical_base_address = 0x00000000;
@@ -113,6 +114,7 @@
     fmmu->domain = domain;
 
     domain->data_size += fmmu->data_size;
+    domain->tx_size += fmmu->tx_size;
     list_add_tail(&fmmu->list, &domain->fmmu_configs);
 
     if (domain->master->debug_level)
@@ -178,6 +180,7 @@
 
     ec_datagram_zero(datagram);
     list_add_tail(&datagram->list, &domain->datagrams);
+    datagram->domain = domain;
     return 0;
 }
 
@@ -236,6 +239,7 @@
     list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
         // Correct logical FMMU address
         fmmu->logical_start_address += base_address;
+        fmmu->domain_address += base_address;
 
         // Increment Input/Output counter to determine datagram types
         // and calculate expected working counters
@@ -247,7 +251,7 @@
 
         // If the current FMMU's data do not fit in the current datagram,
         // allocate a new one.
-        if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {
+        if (datagram_size + fmmu->tx_size > EC_MAX_DATA_SIZE) {
             ret = ec_domain_add_datagram(domain,
                     domain->logical_base_address + datagram_offset,
                     datagram_size, domain->data + datagram_offset,
@@ -265,7 +269,7 @@
             }
         }
 
-        datagram_size += fmmu->data_size;
+        datagram_size += fmmu->tx_size;
     }
 
     // Allocate last datagram, if data are left (this is also the case if the
--- a/master/domain.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/domain.h	Thu Apr 29 16:33:32 2010 +0200
@@ -59,6 +59,7 @@
 
     struct list_head fmmu_configs; /**< FMMU configurations contained. */
     size_t data_size; /**< Size of the process data. */
+    size_t tx_size; /**< Size of the transmitted data. */
     uint8_t *data; /**< Memory for the process data. */
     ec_origin_t data_origin; /**< Origin of the \a data memory. */
     uint32_t logical_base_address; /**< Logical offset address of the
--- a/master/fmmu_config.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/fmmu_config.c	Thu Apr 29 16:33:32 2010 +0200
@@ -43,14 +43,12 @@
 
 /** FMMU configuration constructor.
  *
- * Inits an FMMU configuration, sets the logical start address and adds the
- * process data size for the mapped PDOs of the given direction to the domain
- * data size.
+ * Inits an FMMU configuration and the process data size forthe mapped PDOs
+ * of the given direction to the domain data size.
  */
 void ec_fmmu_config_init(
         ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
         ec_slave_config_t *sc, /**< EtherCAT slave configuration. */
-        ec_domain_t *domain, /**< EtherCAT domain. */
         uint8_t sync_index, /**< Sync manager index to use. */
         ec_direction_t dir /**< PDO direction. */
         )
@@ -59,14 +57,32 @@
     fmmu->sc = sc;
     fmmu->sync_index = sync_index;
     fmmu->dir = dir;
-
-    fmmu->logical_start_address = domain->data_size;
     fmmu->data_size = ec_pdo_list_total_size(
             &sc->sync_configs[sync_index].pdos);
+}
 
+
+/*****************************************************************************/
+
+/** Sets FMMU domain
+ *
+ * Sets the logical start address and the size of the transmitted data
+ */
+void ec_fmmu_config_domain(
+        ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
+        ec_domain_t *domain, /**< EtherCAT domain. */
+        uint32_t logical_start_address, /**< FMMU logical start address. */
+        size_t tx_size /**< Size of transmitted data */
+        )
+{
+    fmmu->domain = domain;
+    fmmu->domain_address = domain->data_size;
+    fmmu->logical_start_address = logical_start_address;
+    fmmu->tx_size = tx_size;
     ec_domain_add_fmmu_config(domain, fmmu);
 }
 
+
 /*****************************************************************************/
 
 /** Initializes an FMMU configuration page.
@@ -80,9 +96,9 @@
         )
 {
     if (fmmu->sc->master->debug_level) {
-        EC_DBG("FMMU: LogAddr 0x%08X, Size %3u, PhysAddr 0x%04X, SM%u, "
-                "Dir %s\n", fmmu->logical_start_address, fmmu->data_size,
-               sync->physical_start_address, fmmu->sync_index,
+        EC_DBG("FMMU: LogAddr 0x%08X, DomAddr 0x%08X, Size %3u, Tx %3u, PhysAddr 0x%04X, SM%u, "
+                "Dir %s\n", fmmu->logical_start_address, fmmu->domain_address, fmmu->data_size,
+                fmmu->tx_size, sync->physical_start_address, fmmu->sync_index,
                fmmu->dir == EC_DIR_INPUT ? "in" : "out");
     }
 
--- a/master/fmmu_config.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/fmmu_config.h	Thu Apr 29 16:33:32 2010 +0200
@@ -50,13 +50,18 @@
     uint8_t sync_index; /**< Index of sync manager to use. */
     ec_direction_t dir; /**< FMMU direction. */
     uint32_t logical_start_address; /**< Logical start address. */
+    size_t tx_size; /**< Transmitted (bus) size. */
+    uint32_t domain_address;    /** Domain start address */
     unsigned int data_size; /**< Covered PDO size. */
 } ec_fmmu_config_t;
 
 /*****************************************************************************/
 
 void ec_fmmu_config_init(ec_fmmu_config_t *, ec_slave_config_t *,
-        ec_domain_t *, uint8_t, ec_direction_t);
+        uint8_t, ec_direction_t);
+
+void ec_fmmu_config_domain(ec_fmmu_config_t *, ec_domain_t *,
+        uint32_t , size_t);
 
 void ec_fmmu_config_page(const ec_fmmu_config_t *, const ec_sync_t *,
         uint8_t *);
--- a/master/ioctl.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/ioctl.h	Thu Apr 29 16:33:32 2010 +0200
@@ -135,6 +135,7 @@
 #define EC_IOCTL_VOE_DATA             EC_IOWR(0x45, ec_ioctl_voe_t)
 #define EC_IOCTL_SET_SEND_INTERVAL     EC_IOW(0x46, size_t)
 #define EC_IOCTL_MASTER_SC_STATE        EC_IOR(0x47, ec_master_state_t)
+#define EC_IOCTL_SC_OVERLAPPING_IO      EC_IOW(0x48, ec_ioctl_config_t)
 
 /*****************************************************************************/
 
@@ -455,6 +456,7 @@
     } syncs[EC_MAX_SYNC_MANAGERS];
     uint16_t watchdog_divider;
     uint16_t watchdog_intervals;
+    uint8_t allow_overlapping_pdos;
     uint32_t sdo_count;
     int32_t slave_position;
     uint16_t dc_assign_activate;
--- a/master/master.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/master.c	Thu Apr 29 16:33:32 2010 +0200
@@ -902,7 +902,7 @@
 {
     ec_datagram_t *datagram, *next;
     size_t datagram_size;
-    uint8_t *frame_data, *cur_data;
+    uint8_t *frame_data, *cur_data, *frame_datagram_data;
     void *follows_word;
 #ifdef EC_HAVE_CYCLES
     cycles_t cycles_start, cycles_sent, cycles_end;
@@ -910,6 +910,7 @@
     unsigned long jiffies_sent;
     unsigned int frame_count, more_datagrams_waiting;
     struct list_head sent_datagrams;
+    ec_fmmu_config_t* domain_fmmu;
 
 #ifdef EC_HAVE_CYCLES
     cycles_start = get_cycles();
@@ -959,7 +960,28 @@
             cur_data += EC_DATAGRAM_HEADER_SIZE;
 
             // EtherCAT datagram data
-            memcpy(cur_data, datagram->data, datagram->data_size);
+            frame_datagram_data = cur_data;
+            if (datagram->domain) {
+                unsigned int datagram_address = EC_READ_U32(datagram->address);
+                int i = 0;
+                uint8_t *domain_data = datagram->data;
+                list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
+                    if (domain_fmmu->dir == EC_DIR_OUTPUT ) {
+                        unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
+                        memcpy(frame_datagram_data+frame_offset, domain_data, domain_fmmu->data_size);
+                        if (unlikely(master->debug_level > 1)) {
+                            EC_DBG("sending dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n",
+                                   datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
+                            ec_print_data(domain_data, domain_fmmu->data_size);
+                        }
+                    }
+                    domain_data += domain_fmmu->data_size;
+                    ++i;
+                }
+            }
+            else {
+                memcpy(frame_datagram_data, datagram->data, datagram->data_size);
+            }
             cur_data += datagram->data_size;
 
             // EtherCAT datagram footer
@@ -1031,8 +1053,9 @@
     size_t frame_size, data_size;
     uint8_t datagram_type, datagram_index;
     unsigned int cmd_follows, matched;
-    const uint8_t *cur_data;
+    const uint8_t *cur_data, *frame_datagram_data;
     ec_datagram_t *datagram;
+    ec_fmmu_config_t* domain_fmmu;
 
     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
         if (master->debug_level) {
@@ -1113,9 +1136,29 @@
             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
             continue;
         }
-
-        // copy received data into the datagram memory
-        memcpy(datagram->data, cur_data, data_size);
+        frame_datagram_data = cur_data;
+        if (datagram->domain) {
+            size_t datagram_address = EC_READ_U32(datagram->address);
+            int i = 0;
+            uint8_t *domain_data = datagram->data;
+            list_for_each_entry(domain_fmmu, &datagram->domain->fmmu_configs, list) {
+                if (domain_fmmu->dir == EC_DIR_INPUT ) {
+                    unsigned int frame_offset = domain_fmmu->logical_start_address-datagram_address;
+                    memcpy(domain_data, frame_datagram_data+frame_offset, domain_fmmu->data_size);
+                    if (unlikely(master->debug_level > 1)) {
+                        EC_DBG("receiving dg 0x%02X fmmu %u fp=%u dp=%u size=%u\n",
+                               datagram->index, i,frame_offset,domain_data-datagram->data,domain_fmmu->data_size);
+                        ec_print_data(domain_data, domain_fmmu->data_size);
+                    }
+                }
+                domain_data += domain_fmmu->data_size;
+                ++i;
+            }
+        }
+        else {
+            // copy received data into the datagram memory
+            memcpy(datagram->data, frame_datagram_data, data_size);
+        }
         cur_data += data_size;
 
         // set the datagram's working counter
--- a/master/slave_config.c	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/slave_config.c	Thu Apr 29 16:33:32 2010 +0200
@@ -70,7 +70,7 @@
     sc->product_code = product_code;
     sc->watchdog_divider = 0; // use default
     sc->watchdog_intervals = 0; // use default
-
+    sc->allow_overlapping_pdos = 0; // default not allowed
     sc->slave = NULL;
 
     for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)
@@ -164,12 +164,15 @@
 {
     unsigned int i;
     ec_fmmu_config_t *fmmu;
+    ec_fmmu_config_t *prev_fmmu;
+    uint32_t fmmu_logical_start_address;
+    size_t tx_size;
 
     // FMMU configuration already prepared?
     for (i = 0; i < sc->used_fmmus; i++) {
         fmmu = &sc->fmmu_configs[i];
         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
-            return fmmu->logical_start_address;
+            return fmmu->domain_address;
     }
 
     if (sc->used_fmmus == EC_MAX_FMMUS) {
@@ -178,13 +181,25 @@
         return -EOVERFLOW;
     }
 
-    fmmu = &sc->fmmu_configs[sc->used_fmmus++];
+    fmmu = &sc->fmmu_configs[sc->used_fmmus];
 
     down(&sc->master->master_sem);
-    ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);
+    ec_fmmu_config_init(fmmu, sc, sync_index, dir);
+    fmmu_logical_start_address = domain->tx_size;
+    tx_size = fmmu->data_size;
+    if (sc->allow_overlapping_pdos && sc->used_fmmus > 0) {
+        prev_fmmu = &sc->fmmu_configs[sc->used_fmmus-1];
+        if (fmmu->dir != prev_fmmu->dir) {
+            prev_fmmu->tx_size = max(fmmu->data_size,prev_fmmu->data_size);
+            tx_size = 0;
+            fmmu_logical_start_address = prev_fmmu->logical_start_address;
+        }
+    }
+    ec_fmmu_config_domain(fmmu,domain,fmmu_logical_start_address,tx_size);
     up(&sc->master->master_sem);
 
-    return fmmu->logical_start_address;
+    ++sc->used_fmmus;
+    return fmmu->domain_address;
 }
 
 /*****************************************************************************/
@@ -465,6 +480,18 @@
 
 /*****************************************************************************/
 
+void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc,
+        uint8_t allow_overlapping_pdos )
+{
+    if (sc->master->debug_level)
+        EC_DBG("%s(sc = 0x%p, allow_overlapping_pdos = %u)\n",
+                __func__, sc, allow_overlapping_pdos);
+
+    sc->allow_overlapping_pdos = allow_overlapping_pdos;
+}
+
+/*****************************************************************************/
+
 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
         uint8_t sync_index, uint16_t pdo_index)
 {
@@ -1005,6 +1032,7 @@
 
 EXPORT_SYMBOL(ecrt_slave_config_sync_manager);
 EXPORT_SYMBOL(ecrt_slave_config_watchdog);
+EXPORT_SYMBOL(ecrt_slave_config_overlapping_pdos);
 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add);
 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear);
 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add);
--- a/master/slave_config.h	Thu Apr 29 14:05:15 2010 +0200
+++ b/master/slave_config.h	Thu Apr 29 16:33:32 2010 +0200
@@ -62,7 +62,8 @@
                                  intervals (see spec. reg. 0x0400). */
     uint16_t watchdog_intervals; /**< Process data watchdog intervals (see
                                    spec. reg. 0x0420). */
-
+    uint8_t allow_overlapping_pdos;	/**< Allow input PDOs use the same frame space
+                                      as output PDOs. */
     ec_slave_t *slave; /**< Slave pointer. This is \a NULL, if the slave is
                          offline. */