Prefer EEPROM sync manager information for config.
authorFlorian Pose <fp@igh-essen.com>
Mon, 17 Jul 2006 13:01:38 +0000
changeset 298 d491d1f84ebc
parent 297 ac2a057a8ef2
child 299 911f0aed65a9
Prefer EEPROM sync manager information for config.
master/fsm.c
master/master.c
master/master.h
master/slave.c
master/slave.h
--- a/master/fsm.c	Mon Jul 17 12:58:47 2006 +0000
+++ b/master/fsm.c	Mon Jul 17 13:01:38 2006 +0000
@@ -1163,7 +1163,7 @@
                     EC_ERR("Invalid sync manager configuration found!");
                     return;
                 }
-                ec_eeprom_sync_config(eeprom_sync,
+                ec_eeprom_sync_config(eeprom_sync, slave,
                                       datagram->data + EC_SYNC_SIZE
                                       * eeprom_sync->index);
             }
@@ -1176,14 +1176,14 @@
             mbox_sync.length = slave->sii_rx_mailbox_size;
             mbox_sync.control_register = 0x26;
             mbox_sync.enable = 1;
-            ec_eeprom_sync_config(&mbox_sync, datagram->data);
+            ec_eeprom_sync_config(&mbox_sync, slave, datagram->data);
 
             mbox_sync.physical_start_address =
                 slave->sii_tx_mailbox_offset;
             mbox_sync.length = slave->sii_tx_mailbox_size;
             mbox_sync.control_register = 0x22;
             mbox_sync.enable = 1;
-            ec_eeprom_sync_config(&mbox_sync,
+            ec_eeprom_sync_config(&mbox_sync, slave,
                                   datagram->data + EC_SYNC_SIZE);
         }
 
--- a/master/master.c	Mon Jul 17 12:58:47 2006 +0000
+++ b/master/master.c	Mon Jul 17 13:01:38 2006 +0000
@@ -772,11 +772,16 @@
 */
 
 void ec_eeprom_sync_config(const ec_eeprom_sync_t *sync, /**< sync manager */
+                           const ec_slave_t *slave, /**< EtherCAT slave */
                            uint8_t *data /**> configuration memory */
                            )
 {
+    size_t sync_size;
+
+    sync_size = ec_slave_calc_eeprom_sync_size(slave, sync);
+
     EC_WRITE_U16(data,     sync->physical_start_address);
-    EC_WRITE_U16(data + 2, sync->length);
+    EC_WRITE_U16(data + 2, sync_size);
     EC_WRITE_U8 (data + 4, sync->control_register);
     EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
     EC_WRITE_U16(data + 6, sync->enable ? 0x0001 : 0x0000); // enable
@@ -1130,7 +1135,8 @@
             if (ec_datagram_npwr(datagram, slave->station_address, 0x0800,
                                 EC_SYNC_SIZE * slave->base_sync_count))
                 return -1;
-            memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
+            memset(datagram->data, 0x00,
+                   EC_SYNC_SIZE * slave->base_sync_count);
             if (unlikely(ec_master_simple_io(master, datagram))) {
                 EC_ERR("Resetting sync managers failed on slave %i!\n",
                        slave->ring_position);
@@ -1139,11 +1145,27 @@
         }
 
         // configure sync managers
-        if (type) { // known slave type, take type's SM information
+
+        // does the slave provide sync manager information?
+        if (!list_empty(&slave->eeprom_syncs)) {
+            list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) {
+                if (ec_datagram_npwr(datagram, slave->station_address,
+                                     0x800 + eeprom_sync->index *
+                                     EC_SYNC_SIZE,
+                                     EC_SYNC_SIZE)) return -1;
+                ec_eeprom_sync_config(eeprom_sync, slave, datagram->data);
+                if (unlikely(ec_master_simple_io(master, datagram))) {
+                    EC_ERR("Setting sync manager %i failed on slave %i!\n",
+                           eeprom_sync->index, slave->ring_position);
+                    return -1;
+                }
+            }
+        }
+        else if (type) { // known slave type, take type's SM information
             for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) {
                 sync = type->sync_managers[j];
                 if (ec_datagram_npwr(datagram, slave->station_address,
-                                    0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE))
+                                     0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE))
                     return -1;
                 ec_sync_config(sync, slave, datagram->data);
                 if (unlikely(ec_master_simple_io(master, datagram))) {
@@ -1153,54 +1175,34 @@
                 }
             }
         }
-        else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox
-            // does the device supply SM configurations in its EEPROM?
-	    if (!list_empty(&slave->eeprom_syncs)) {
-		list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) {
-		    EC_INFO("Sync manager %i...\n", eeprom_sync->index);
-		    if (ec_datagram_npwr(datagram, slave->station_address,
-                                 0x800 + eeprom_sync->index *
-                                 EC_SYNC_SIZE,
-                                 EC_SYNC_SIZE)) return -1;
-		    ec_eeprom_sync_config(eeprom_sync, datagram->data);
-		    if (unlikely(ec_master_simple_io(master, datagram))) {
-			EC_ERR("Setting sync manager %i failed on slave %i!\n",
-			       eeprom_sync->index, slave->ring_position);
-			return -1;
-		    }
-		}
-            }
-	    else { // no sync manager information; guess mailbox settings
-		mbox_sync.physical_start_address =
-                    slave->sii_rx_mailbox_offset;
-		mbox_sync.length = slave->sii_rx_mailbox_size;
-		mbox_sync.control_register = 0x26;
-		mbox_sync.enable = 1;
-		if (ec_datagram_npwr(datagram, slave->station_address,
-                             0x800,EC_SYNC_SIZE)) return -1;
-		ec_eeprom_sync_config(&mbox_sync, datagram->data);
-		if (unlikely(ec_master_simple_io(master, datagram))) {
-		    EC_ERR("Setting sync manager 0 failed on slave %i!\n",
-			   slave->ring_position);
-		    return -1;
-		}
-
-		mbox_sync.physical_start_address =
-                    slave->sii_tx_mailbox_offset;
-		mbox_sync.length = slave->sii_tx_mailbox_size;
-		mbox_sync.control_register = 0x22;
-		mbox_sync.enable = 1;
-		if (ec_datagram_npwr(datagram, slave->station_address,
-                             0x808, EC_SYNC_SIZE)) return -1;
-		ec_eeprom_sync_config(&mbox_sync, datagram->data);
-		if (unlikely(ec_master_simple_io(master, datagram))) {
+        else { // no sync manager information; guess mailbox settings
+            mbox_sync.physical_start_address =
+                slave->sii_rx_mailbox_offset;
+            mbox_sync.length = slave->sii_rx_mailbox_size;
+            mbox_sync.control_register = 0x26;
+            mbox_sync.enable = 1;
+            if (ec_datagram_npwr(datagram, slave->station_address,
+                                 0x800,EC_SYNC_SIZE)) return -1;
+            ec_eeprom_sync_config(&mbox_sync, slave, datagram->data);
+            if (unlikely(ec_master_simple_io(master, datagram))) {
+                EC_ERR("Setting sync manager 0 failed on slave %i!\n",
+                       slave->ring_position);
+                return -1;
+            }
+
+            mbox_sync.physical_start_address =
+                slave->sii_tx_mailbox_offset;
+            mbox_sync.length = slave->sii_tx_mailbox_size;
+            mbox_sync.control_register = 0x22;
+            mbox_sync.enable = 1;
+            if (ec_datagram_npwr(datagram, slave->station_address,
+                                 0x808, EC_SYNC_SIZE)) return -1;
+            ec_eeprom_sync_config(&mbox_sync, slave, datagram->data);
+            if (unlikely(ec_master_simple_io(master, datagram))) {
 		    EC_ERR("Setting sync manager 1 failed on slave %i!\n",
 			   slave->ring_position);
 		    return -1;
-		}
-	    }
-	    EC_INFO("Mailbox configured for unknown slave %i\n",
-		    slave->ring_position);
+            }
         }
 
         // change state to PREOP
--- a/master/master.h	Mon Jul 17 12:58:47 2006 +0000
+++ b/master/master.h	Mon Jul 17 13:01:38 2006 +0000
@@ -152,7 +152,8 @@
 // misc.
 void ec_master_clear_slaves(ec_master_t *);
 void ec_sync_config(const ec_sync_t *, const ec_slave_t *, uint8_t *);
-void ec_eeprom_sync_config(const ec_eeprom_sync_t *, uint8_t *);
+void ec_eeprom_sync_config(const ec_eeprom_sync_t *, const ec_slave_t *,
+                           uint8_t *);
 void ec_fmmu_config(const ec_fmmu_t *, const ec_slave_t *, uint8_t *);
 void ec_master_output_stats(ec_master_t *);
 
--- a/master/slave.c	Mon Jul 17 12:58:47 2006 +0000
+++ b/master/slave.c	Mon Jul 17 13:01:38 2006 +0000
@@ -1494,6 +1494,37 @@
     return size;
 }
 
+/*****************************************************************************/
+
+/**
+   Calculates the size of a sync manager by evaluating PDO sizes.
+   \return sync manager size
+*/
+
+uint16_t ec_slave_calc_eeprom_sync_size(const ec_slave_t *slave,
+                                        /**< EtherCAT slave */
+                                        const ec_eeprom_sync_t *sync
+                                        /**< sync manager */
+                                        )
+{
+    ec_eeprom_pdo_t *pdo;
+    ec_eeprom_pdo_entry_t *pdo_entry;
+    uint16_t size;
+
+    if (sync->length) return sync->length;
+
+    size = 0;
+    list_for_each_entry(pdo, &slave->eeprom_pdos, list) {
+        if (pdo->sync_manager != sync->index) continue;
+
+        list_for_each_entry(pdo_entry, &pdo->entries, list) {
+            size += pdo_entry->bit_length / 8;
+        }
+    }
+
+    return size;
+}
+
 /******************************************************************************
  *  Realtime interface
  *****************************************************************************/
--- a/master/slave.h	Mon Jul 17 12:58:47 2006 +0000
+++ b/master/slave.h	Mon Jul 17 13:01:38 2006 +0000
@@ -326,6 +326,9 @@
 
 // misc.
 size_t ec_slave_calc_sync_size(const ec_slave_t *, const ec_sync_t *);
+uint16_t ec_slave_calc_eeprom_sync_size(const ec_slave_t *,
+                                        const ec_eeprom_sync_t *);
+
 void ec_slave_print(const ec_slave_t *, unsigned int);
 int ec_slave_check_crc(ec_slave_t *);