Introduced configured_[tr]x_mailbox(size|offset) members of slave. 1.4-foe
authorFlorian Pose <fp@igh-essen.com>
Tue, 20 Jan 2009 09:55:32 +0000
branch1.4-foe
changeset 1711 2b017fcc1c6d
parent 1710 4522459bb5a4
child 1712 45ff77031255
Introduced configured_[tr]x_mailbox(size|offset) members of slave.
master/ethernet.c
master/fsm_coe.c
master/fsm_foe.c
master/fsm_pdo.c
master/fsm_slave_config.c
master/fsm_slave_scan.c
master/mailbox.c
master/slave.c
master/slave.h
--- a/master/ethernet.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/ethernet.c	Tue Jan 20 09:55:32 2009 +0000
@@ -146,7 +146,7 @@
     // so the MTU is left on the Ethernet standard value and fragmenting
     // is done "manually".
 #if 0
-    eoe->dev->mtu = slave->sii.rx_mailbox_size - ETH_HLEN - 10;
+    eoe->dev->mtu = slave->configured_rx_mailbox_size - ETH_HLEN - 10;
 #endif
 
     // connect the net_device to the kernel
@@ -231,11 +231,11 @@
 
     remaining_size = eoe->tx_frame->skb->len - eoe->tx_offset;
 
-    if (remaining_size <= eoe->slave->sii.std_tx_mailbox_size - 10) {
+    if (remaining_size <= eoe->slave->configured_tx_mailbox_size - 10) {
         current_size = remaining_size;
         last_fragment = 1;
     } else {
-        current_size = ((eoe->slave->sii.std_tx_mailbox_size - 10) / 32) * 32;
+        current_size = ((eoe->slave->configured_tx_mailbox_size - 10) / 32) * 32;
         last_fragment = 0;
     }
 
@@ -695,7 +695,7 @@
     ec_eoe_frame_t *frame;
 
 #if 0
-    if (skb->len > eoe->slave->sii.tx_mailbox_size - 10) {
+    if (skb->len > eoe->slave->configured_tx_mailbox_size - 10) {
         EC_WARN("EoE TX frame (%u octets) exceeds MTU. dropping.\n", skb->len);
         dev_kfree_skb(skb);
         eoe->stats.tx_dropped++;
--- a/master/fsm_coe.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/fsm_coe.c	Tue Jan 20 09:55:32 2009 +0000
@@ -1089,7 +1089,7 @@
         }
 	}
     else { // request->data_size > 4, use normal transfer type
-	    if (slave->sii.std_rx_mailbox_size < 6 + 10 + request->data_size) {
+	    if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) {
 	        EC_ERR("SDO fragmenting not supported yet!\n");
 	        fsm->state = ec_fsm_coe_error;
 	        return;
--- a/master/fsm_foe.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/fsm_foe.c	Tue Jan 20 09:55:32 2009 +0000
@@ -214,12 +214,12 @@
 
     remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset;
 
-    if (remaining_size < fsm->slave->sii.std_tx_mailbox_size
+    if (remaining_size < fsm->slave->configured_tx_mailbox_size
             - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {
         current_size = remaining_size;
         fsm->tx_last_packet = 1;
     } else {
-        current_size = fsm->slave->sii.std_tx_mailbox_size
+        current_size = fsm->slave->configured_tx_mailbox_size
             - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;
     }
 
@@ -789,10 +789,10 @@
 
     fsm->rx_last_packet =
         (rec_size + EC_MBOX_HEADER_SIZE + EC_FOE_HEADER_SIZE
-         != fsm->slave->sii.std_rx_mailbox_size);
+         != fsm->slave->configured_rx_mailbox_size);
 
     if (fsm->rx_last_packet ||
-            (slave->sii.std_rx_mailbox_size - EC_MBOX_HEADER_SIZE
+            (slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE
              - EC_FOE_HEADER_SIZE + fsm->rx_buffer_offset) <= fsm->rx_buffer_size) {
         // either it was the last packet or a new packet will fit into the delivered buffer
 #ifdef	myDEBUG
@@ -812,7 +812,7 @@
     	printk ("       rx_buffer_size  = %d\n", fsm->rx_buffer_size);
     	printk ("       rx_buffer_offset= %d\n", fsm->rx_buffer_offset);
     	printk ("       rec_size        = %d\n", rec_size);
-    	printk ("       rx_mailbox_size = %d\n", slave->sii.std_rx_mailbox_size);
+    	printk ("       rx_mailbox_size = %d\n", slave->configured_rx_mailbox_size);
     	printk ("       rx_last_packet  = %d\n", fsm->rx_last_packet);
 //    	fsm->state = ec_fsm_state_wait_next_read;
     	fsm->request->result = FOE_READY;
--- a/master/fsm_pdo.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/fsm_pdo.c	Tue Jan 20 09:55:32 2009 +0000
@@ -225,8 +225,8 @@
     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
-        EC_ERR("Failed to read number of assigned PDOs for SM%u.\n",
-                fsm->sync_index);
+        EC_ERR("Failed to read number of assigned PDOs for SM%u"
+                " of slave %u.\n", fsm->sync_index, fsm->slave->ring_position);
         fsm->state = ec_fsm_pdo_state_error;
         return;
     }
--- a/master/fsm_slave_config.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/fsm_slave_config.c	Tue Jan 20 09:55:32 2009 +0000
@@ -373,6 +373,10 @@
         ec_sync_page(&sync, 0, slave->sii.boot_rx_mailbox_size,
                 EC_DIR_INVALID, // use default direction
                 datagram->data);
+        slave->configured_rx_mailbox_offset =
+            slave->sii.boot_rx_mailbox_offset;
+        slave->configured_rx_mailbox_size =
+            slave->sii.boot_rx_mailbox_size;
 
         ec_sync_init(&sync, slave);
         sync.physical_start_address = slave->sii.boot_tx_mailbox_offset;
@@ -381,6 +385,11 @@
         ec_sync_page(&sync, 1, slave->sii.boot_tx_mailbox_size,
                 EC_DIR_INVALID, // use default direction
                 datagram->data + EC_SYNC_PAGE_SIZE);
+        slave->configured_tx_mailbox_offset =
+            slave->sii.boot_tx_mailbox_offset;
+        slave->configured_tx_mailbox_size =
+            slave->sii.boot_tx_mailbox_size;
+
     } else if (slave->sii.sync_count >= 2) { // mailbox configuration provided
         ec_datagram_fpwr(datagram, slave->station_address, 0x0800,
                 EC_SYNC_PAGE_SIZE * slave->sii.sync_count);
@@ -393,6 +402,15 @@
                     EC_DIR_INVALID, // use default direction
                     datagram->data + EC_SYNC_PAGE_SIZE * i);
         }
+
+        slave->configured_rx_mailbox_offset =
+            slave->sii.syncs[0].physical_start_address;
+        slave->configured_rx_mailbox_size =
+            slave->sii.syncs[0].default_length;
+        slave->configured_tx_mailbox_offset =
+            slave->sii.syncs[1].physical_start_address;
+        slave->configured_tx_mailbox_size =
+            slave->sii.syncs[1].default_length;
     } else { // no mailbox sync manager configurations provided
         ec_sync_t sync;
 
@@ -412,6 +430,10 @@
         ec_sync_page(&sync, 0, slave->sii.std_rx_mailbox_size,
                 EC_DIR_INVALID, // use default direction
                 datagram->data);
+        slave->configured_rx_mailbox_offset =
+            slave->sii.std_rx_mailbox_offset;
+        slave->configured_rx_mailbox_size =
+            slave->sii.std_rx_mailbox_size;
 
         ec_sync_init(&sync, slave);
         sync.physical_start_address = slave->sii.std_tx_mailbox_offset;
@@ -420,6 +442,10 @@
         ec_sync_page(&sync, 1, slave->sii.std_tx_mailbox_size,
                 EC_DIR_INVALID, // use default direction
                 datagram->data + EC_SYNC_PAGE_SIZE);
+        slave->configured_tx_mailbox_offset =
+            slave->sii.boot_tx_mailbox_offset;
+        slave->configured_tx_mailbox_size =
+            slave->sii.boot_tx_mailbox_size;
     }
 
     fsm->retries = EC_FSM_RETRIES;
--- a/master/fsm_slave_scan.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/fsm_slave_scan.c	Tue Jan 20 09:55:32 2009 +0000
@@ -48,6 +48,7 @@
 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);
+void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);
 
 void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);
@@ -586,17 +587,33 @@
         )
 {
     ec_slave_t *slave = fsm->slave;
-
-    if ((slave->current_state & EC_SLAVE_STATE_MASK) < EC_SLAVE_STATE_PREOP) {
-        if (slave->master->debug_level)
-            EC_DBG("Slave %u is not in the state to do mailbox com, setting"
-                    " to PREOP.\n", slave->ring_position);
+    uint8_t current_state = slave->current_state & EC_SLAVE_STATE_MASK;
+
+    if (current_state != EC_SLAVE_STATE_PREOP
+            && current_state != EC_SLAVE_STATE_SAFEOP
+            && current_state != EC_SLAVE_STATE_OP) {
+        if (slave->master->debug_level) {
+            char str[EC_STATE_STRING_SIZE];
+            ec_state_string(current_state, str, 0);
+            EC_DBG("Slave %u is not in the state to do mailbox com (%s),"
+                    " setting to PREOP.\n", slave->ring_position, str);
+        }
+
         fsm->state = ec_fsm_slave_scan_state_preop;
         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
         ec_fsm_slave_config_start(fsm->fsm_slave_config, slave);
         ec_fsm_slave_config_exec(fsm->fsm_slave_config);
     } else {
-        ec_fsm_slave_scan_enter_pdos(fsm);
+        if (slave->master->debug_level)
+            EC_DBG("Reading mailbox syncmanager configuration of slave %u.\n",
+                    slave->ring_position);
+
+        /* Scan current sync manager configuration to get configured mailbox
+         * sizes. */
+        ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800,
+                EC_SYNC_PAGE_SIZE * 2);
+        fsm->retries = EC_FSM_RETRIES;
+        fsm->state = ec_fsm_slave_scan_state_sync;
     }
 }
 
@@ -621,6 +638,55 @@
 
 /*****************************************************************************/
 
+/** Slave scan state: SYNC.
+ */
+void ec_fsm_slave_scan_state_sync(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+        return;
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        fsm->state = ec_fsm_slave_scan_state_error;
+        EC_ERR("Failed to receive sync manager configuration datagram"
+                " from slave %u (datagram state %u).\n",
+               slave->ring_position, datagram->state);
+        return;
+    }
+
+    if (datagram->working_counter != 1) {
+        fsm->slave->error_flag = 1;
+        fsm->state = ec_fsm_slave_scan_state_error;
+        EC_ERR("Failed to read DL status from slave %u: ",
+               slave->ring_position);
+        ec_datagram_print_wc_error(datagram);
+        return;
+    }
+
+    slave->configured_rx_mailbox_offset = EC_READ_U16(datagram->data);
+    slave->configured_rx_mailbox_size = EC_READ_U16(datagram->data + 2);
+    slave->configured_tx_mailbox_offset = EC_READ_U16(datagram->data + 8);
+    slave->configured_tx_mailbox_size = EC_READ_U16(datagram->data + 10);
+
+    if (slave->master->debug_level) {
+        EC_DBG("Mailbox configuration of slave %u:\n", slave->ring_position);
+        EC_DBG(" RX offset=0x%04x size=%u\n",
+                slave->configured_rx_mailbox_offset,
+                slave->configured_rx_mailbox_size);
+        EC_DBG(" TX offset=0x%04x size=%u\n",
+                slave->configured_tx_mailbox_offset,
+                slave->configured_tx_mailbox_size);
+    }
+
+    ec_fsm_slave_scan_enter_pdos(fsm);
+}
+
+/*****************************************************************************/
+
 /** Enter slave scan state PDOS.
  */
 void ec_fsm_slave_scan_enter_pdos(
--- a/master/mailbox.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/mailbox.c	Tue Jan 20 09:55:32 2009 +0000
@@ -52,7 +52,6 @@
                                     )
 {
     size_t total_size;
-    uint16_t mailbox_offset, mailbox_size;
 
     if (unlikely(!slave->sii.mailbox_protocols)) {
         EC_ERR("Slave %u does not support mailbox communication!\n",
@@ -62,21 +61,15 @@
 
     total_size = size + 6;
 
-    if (slave->current_state != EC_SLAVE_STATE_BOOT) {
-        mailbox_offset = slave->sii.std_rx_mailbox_offset;
-        mailbox_size = slave->sii.std_rx_mailbox_size;
-    } else {
-        mailbox_offset = slave->sii.boot_rx_mailbox_offset;
-        mailbox_size = slave->sii.boot_rx_mailbox_size;
-    }
-
-    if (unlikely(total_size > mailbox_size)) {
-        EC_ERR("Data size does not fit in mailbox!\n");
+    if (unlikely(total_size > slave->configured_rx_mailbox_size)) {
+        EC_ERR("Data size (%u) does not fit in mailbox (%u)!\n",
+                total_size, slave->configured_rx_mailbox_size);
         return NULL;
     }
 
     if (ec_datagram_fpwr(datagram, slave->station_address,
-                         mailbox_offset, mailbox_size))
+                         slave->configured_rx_mailbox_offset,
+                         slave->configured_rx_mailbox_size))
         return NULL;
 
     EC_WRITE_U16(datagram->data,     size); // mailbox service data length
@@ -128,18 +121,9 @@
                                 ec_datagram_t *datagram /**< datagram */
                                 )
 {
-    uint16_t mailbox_offset, mailbox_size;
-
-    if (slave->current_state != EC_SLAVE_STATE_BOOT) {
-        mailbox_offset = slave->sii.std_tx_mailbox_offset;
-        mailbox_size = slave->sii.std_tx_mailbox_size;
-    } else {
-        mailbox_offset = slave->sii.boot_tx_mailbox_offset;
-        mailbox_size = slave->sii.boot_tx_mailbox_size;
-    }
-
     if (ec_datagram_fprd(datagram, slave->station_address,
-                         mailbox_offset, mailbox_size))
+                         slave->configured_tx_mailbox_offset,
+                         slave->configured_tx_mailbox_size))
         return -1;
 
     return 0;
@@ -177,22 +161,13 @@
                              )
 {
     size_t data_size;
-    uint16_t mailbox_offset, mailbox_size;
-
-    if (slave->current_state != EC_SLAVE_STATE_BOOT) {
-        mailbox_offset = slave->sii.std_tx_mailbox_offset;
-        mailbox_size = slave->sii.std_tx_mailbox_size;
-    } else {
-        mailbox_offset = slave->sii.boot_tx_mailbox_offset;
-        mailbox_size = slave->sii.boot_tx_mailbox_size;
-    }
 
     data_size = EC_READ_U16(datagram->data);
 
-    if (data_size > mailbox_size - 6) {
+    if (data_size > slave->configured_tx_mailbox_size - 6) {
         EC_ERR("Corrupt mailbox response received from slave %u!\n",
                slave->ring_position);
-        ec_print_data(datagram->data, mailbox_size);
+        ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
         return NULL;
     }
 
--- a/master/slave.c	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/slave.c	Tue Jan 20 09:55:32 2009 +0000
@@ -74,6 +74,10 @@
     slave->current_state = EC_SLAVE_STATE_UNKNOWN;
     slave->error_flag = 0;
     slave->force_config = 0;
+    slave->configured_rx_mailbox_offset = 0x0000;
+    slave->configured_rx_mailbox_size = 0x0000;
+    slave->configured_tx_mailbox_offset = 0x0000;
+    slave->configured_tx_mailbox_size = 0x0000;
 
     slave->base_type = 0;
     slave->base_revision = 0;
--- a/master/slave.h	Tue Jan 20 08:04:50 2009 +0000
+++ b/master/slave.h	Tue Jan 20 09:55:32 2009 +0000
@@ -117,6 +117,10 @@
     ec_slave_state_t current_state; /**< Current application state. */
     unsigned int error_flag; /**< Stop processing after an error. */
     unsigned int force_config; /**< Force (re-)configuration. */
+    uint16_t configured_rx_mailbox_offset;
+    uint16_t configured_rx_mailbox_size;
+    uint16_t configured_tx_mailbox_offset;
+    uint16_t configured_tx_mailbox_size;
 
     // base data
     uint8_t base_type; /**< Slave type. */