EoE frame queuing, receiving of fragmented frames; no TCP possible yet.
authorFlorian Pose <fp@igh-essen.com>
Fri, 05 May 2006 07:20:10 +0000
changeset 214 8beb86af5ed0
parent 213 8d723b9833a9
child 215 b021aa6eee38
EoE frame queuing, receiving of fragmented frames; no TCP possible yet.
master/ethernet.c
master/ethernet.h
master/globals.h
master/master.c
master/master.h
--- a/master/ethernet.c	Wed May 03 08:01:20 2006 +0000
+++ b/master/ethernet.c	Fri May 05 07:20:10 2006 +0000
@@ -39,6 +39,20 @@
 
 /*****************************************************************************/
 
+/**
+   Queued frame structure.
+*/
+
+typedef struct
+{
+    struct list_head queue; /**< list item */
+    struct sk_buff *skb; /**< socket buffer */
+}
+ec_eoe_frame_t;
+
+/*****************************************************************************/
+
+void ec_eoe_flush(ec_eoe_t *);
 void ec_eoedev_init(struct net_device *);
 int ec_eoedev_open(struct net_device *);
 int ec_eoedev_stop(struct net_device *);
@@ -58,12 +72,16 @@
     int result;
 
     eoe->slave = slave;
-    eoe->rx_state = EC_EOE_IDLE;
+    eoe->state = EC_EOE_RX_START;
     eoe->opened = 0;
     eoe->skb = NULL;
+    eoe->expected_fragment = 0;
     INIT_LIST_HEAD(&eoe->tx_queue);
+    eoe->tx_queue_active = 0;
     eoe->queued_frames = 0;
     eoe->tx_queue_lock = SPIN_LOCK_UNLOCKED;
+    eoe->tx_frame_number = 0xFF;
+    memset(&eoe->stats, 0, sizeof(struct net_device_stats));
 
     if (!(eoe->dev =
           alloc_netdev(sizeof(ec_eoe_t *), "eoe%d", ec_eoedev_init))) {
@@ -75,6 +93,8 @@
     priv = netdev_priv(eoe->dev);
     *priv = eoe;
 
+    //eoe->dev->mtu = slave->sii_rx_mailbox_size - ETH_HLEN - 10;
+
     // connect the net_device to the kernel
     if ((result = register_netdev(eoe->dev))) {
         EC_ERR("Unable to register net_device: error %i\n", result);
@@ -102,6 +122,31 @@
         unregister_netdev(eoe->dev);
         free_netdev(eoe->dev);
     }
+
+    // empty transmit queue
+    ec_eoe_flush(eoe);
+}
+
+/*****************************************************************************/
+
+/**
+   Empties the transmit queue.
+*/
+
+void ec_eoe_flush(ec_eoe_t *eoe)
+{
+    ec_eoe_frame_t *frame, *next;
+
+    spin_lock_bh(&eoe->tx_queue_lock);
+
+    list_for_each_entry_safe(frame, next, &eoe->tx_queue, queue) {
+        list_del(&frame->queue);
+        dev_kfree_skb(frame->skb);
+        kfree(frame);
+    }
+    eoe->queued_frames = 0;
+
+    spin_unlock_bh(&eoe->tx_queue_lock);
 }
 
 /*****************************************************************************/
@@ -114,90 +159,214 @@
 {
     uint8_t *data;
     ec_master_t *master;
-    size_t rec_size;
+    size_t rec_size, data_size;
+    off_t offset;
     uint8_t fragment_number, frame_number, last_fragment, time_appended;
     uint8_t fragment_offset, frame_type;
+    ec_eoe_frame_t *frame;
+    unsigned int wakeup = 0;
 
     if (!eoe->opened) return;
 
     master = eoe->slave->master;
 
-    if (eoe->rx_state == EC_EOE_IDLE) {
-        ec_slave_mbox_prepare_check(eoe->slave);
-        ec_master_queue_command(master, &eoe->slave->mbox_command);
-        eoe->rx_state = EC_EOE_CHECKING;
-        return;
-    }
-
-    if (eoe->rx_state == EC_EOE_CHECKING) {
-        if (eoe->slave->mbox_command.state != EC_CMD_RECEIVED) {
-            master->stats.eoe_errors++;
-            eoe->rx_state = EC_EOE_IDLE;
-            return;
-        }
-        if (!ec_slave_mbox_check(eoe->slave)) {
-            eoe->rx_state = EC_EOE_IDLE;
-            return;
-        }
-        ec_slave_mbox_prepare_fetch(eoe->slave);
-        ec_master_queue_command(master, &eoe->slave->mbox_command);
-        eoe->rx_state = EC_EOE_FETCHING;
-        return;
-    }
-
-    if (eoe->rx_state == EC_EOE_FETCHING) {
-        if (eoe->slave->mbox_command.state != EC_CMD_RECEIVED) {
-            master->stats.eoe_errors++;
-            eoe->rx_state = EC_EOE_IDLE;
-            return;
-        }
-        if (!(data = ec_slave_mbox_fetch(eoe->slave, 0x02, &rec_size))) {
-            master->stats.eoe_errors++;
-            eoe->rx_state = EC_EOE_IDLE;
-            return;
-        }
-
-        frame_type = EC_READ_U16(data) & 0x000F;
-
-        if (frame_type == 0x00) { // EoE Fragment Request
-            last_fragment = (EC_READ_U16(data) >> 8) & 0x0001;
-            time_appended = (EC_READ_U16(data) >> 9) & 0x0001;
-            fragment_number = EC_READ_U16(data + 2) & 0x003F;
-            fragment_offset = (EC_READ_U16(data + 2) >> 6) & 0x003F;
-            frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F;
-
-#if 0
-            EC_DBG("EOE fragment req received, fragment: %i, offset: %i,"
-                   " frame %i%s%s, %i bytes\n", fragment_number,
-                   fragment_offset, frame_number,
-                   last_fragment ? ", last fragment" : "",
-                   time_appended ? ", + timestamp" : "",
-                   time_appended ? rec_size - 8 : rec_size - 4);
-
-#if 0
-            EC_DBG("");
-            for (i = 0; i < rec_size - 4; i++) {
-                printk("%02X ", data[i + 4]);
-                if ((i + 1) % 16 == 0) {
-                    printk("\n");
-                    EC_DBG("");
+    switch (eoe->state) {
+        case EC_EOE_RX_START:
+            ec_slave_mbox_prepare_check(eoe->slave);
+            ec_master_queue_command(master, &eoe->slave->mbox_command);
+            eoe->state = EC_EOE_RX_CHECK;
+            break;
+
+        case EC_EOE_RX_CHECK:
+            if (eoe->slave->mbox_command.state != EC_CMD_RECEIVED) {
+                eoe->stats.rx_errors++;
+                eoe->state = EC_EOE_TX_START;
+                break;
+            }
+            if (!ec_slave_mbox_check(eoe->slave)) {
+                eoe->state = EC_EOE_TX_START;
+                break;
+            }
+            ec_slave_mbox_prepare_fetch(eoe->slave);
+            ec_master_queue_command(master, &eoe->slave->mbox_command);
+            eoe->state = EC_EOE_RX_FETCH;
+            break;
+
+        case EC_EOE_RX_FETCH:
+            if (eoe->slave->mbox_command.state != EC_CMD_RECEIVED) {
+                eoe->stats.rx_errors++;
+                eoe->state = EC_EOE_TX_START;
+                break;
+            }
+            if (!(data = ec_slave_mbox_fetch(eoe->slave, 0x02, &rec_size))) {
+                eoe->stats.rx_errors++;
+                eoe->state = EC_EOE_TX_START;
+                break;
+            }
+
+            frame_type = EC_READ_U16(data) & 0x000F;
+
+            if (frame_type == 0x00) { // EoE Fragment Request
+                last_fragment = (EC_READ_U16(data) >> 8) & 0x0001;
+                time_appended = (EC_READ_U16(data) >> 9) & 0x0001;
+                fragment_number = EC_READ_U16(data + 2) & 0x003F;
+                fragment_offset = (EC_READ_U16(data + 2) >> 6) & 0x003F;
+                frame_number = (EC_READ_U16(data + 2) >> 12) & 0x000F;
+
+                EC_DBG("EoE RX fragment %i, offset %i, frame %i%s%s,"
+                       " %i octets\n", fragment_number, fragment_offset,
+                       frame_number,
+                       last_fragment ? ", last fragment" : "",
+                       time_appended ? ", + timestamp" : "",
+                       time_appended ? rec_size - 8 : rec_size - 4);
+
+                data_size = time_appended ? rec_size - 8 : rec_size - 4;
+
+                if (!fragment_number) {
+                    if (eoe->skb) {
+                        EC_WARN("EoE RX freeing old socket buffer...\n");
+                        dev_kfree_skb(eoe->skb);
+                    }
+
+                    // new socket buffer
+                    if (!(eoe->skb = dev_alloc_skb(fragment_offset * 32))) {
+                        if (printk_ratelimit())
+                            EC_WARN("EoE RX low on mem. frame dropped.\n");
+                        eoe->stats.rx_dropped++;
+                        eoe->state = EC_EOE_TX_START;
+                        break;
+                    }
+                    eoe->skb_offset = 0;
+                    eoe->skb_size = fragment_offset * 32;
+                    eoe->expected_fragment = 0;
                 }
-            }
-            printk("\n");
-#endif
-#endif
-
-            ec_eoedev_rx(eoe->dev, data + 4,
-                         time_appended ? rec_size - 8 : rec_size - 4);
-        }
-        else {
-#if 1
-            EC_DBG("other frame received.\n");
-#endif
-        }
-
-        eoe->rx_state = EC_EOE_IDLE;
-        return;
+                else {
+                    if (!eoe->skb) {
+                        eoe->stats.rx_dropped++;
+                        eoe->state = EC_EOE_TX_START;
+                        break;
+                    }
+
+                    offset = fragment_offset * 32;
+                    if (offset != eoe->skb_offset ||
+                        offset + data_size > eoe->skb_size ||
+                        fragment_number != eoe->expected_fragment) {
+                        eoe->stats.rx_errors++;
+                        eoe->state = EC_EOE_TX_START;
+                        dev_kfree_skb(eoe->skb);
+                        eoe->skb = NULL;
+                        break;
+                    }
+                }
+
+                // copy fragment into socket buffer
+                memcpy(skb_put(eoe->skb, data_size), data + 4, data_size);
+                eoe->skb_offset += data_size;
+
+                if (last_fragment) {
+                    // update statistics
+                    eoe->stats.rx_packets++;
+                    eoe->stats.rx_bytes += eoe->skb->len;
+
+                    EC_DBG("EoE RX frame completed with %u octets.\n",
+                           eoe->skb->len);
+
+                    // pass socket buffer to network stack
+                    eoe->skb->dev = eoe->dev;
+                    eoe->skb->protocol = eth_type_trans(eoe->skb, eoe->dev);
+                    eoe->skb->ip_summed = CHECKSUM_NONE;
+                    if (netif_rx(eoe->skb)) {
+                        EC_WARN("EoE RX netif_rx failed.\n");
+                    }
+                    eoe->skb = NULL;
+
+                    eoe->state = EC_EOE_TX_START;
+                }
+                else {
+                    eoe->expected_fragment++;
+                    EC_DBG("EoE RX expecting fragment %i\n",
+                           eoe->expected_fragment);
+                    eoe->state = EC_EOE_RX_START;
+                }
+            }
+            else {
+                EC_DBG("other frame received.\n");
+                eoe->stats.rx_dropped++;
+                eoe->state = EC_EOE_TX_START;
+            }
+            break;
+
+        case EC_EOE_TX_START:
+            spin_lock_bh(&eoe->tx_queue_lock);
+
+            if (!eoe->queued_frames || list_empty(&eoe->tx_queue)) {
+                spin_unlock_bh(&eoe->tx_queue_lock);
+                eoe->state = EC_EOE_RX_START;
+                break;
+            }
+
+            // take the first frame out of the queue
+            frame = list_entry(eoe->tx_queue.next, ec_eoe_frame_t, queue);
+            list_del(&frame->queue);
+            if (!eoe->tx_queue_active &&
+                eoe->queued_frames == EC_EOE_TX_QUEUE_SIZE / 2) {
+                netif_wake_queue(eoe->dev);
+                eoe->tx_queue_active = 1;
+                wakeup = 1;
+            }
+            eoe->queued_frames--;
+            spin_unlock_bh(&eoe->tx_queue_lock);
+
+            EC_DBG("EoE TX Sending frame with %i octets."
+                   " (%i frames queued).\n",
+                   frame->skb->len, eoe->queued_frames);
+
+            if (wakeup) EC_DBG("waking up TX queue...\n");
+
+            if (!(data = ec_slave_mbox_prepare_send(eoe->slave, 0x02,
+                                                    frame->skb->len + 4))) {
+                dev_kfree_skb(frame->skb);
+                kfree(frame);
+                eoe->stats.tx_errors++;
+                eoe->state = EC_EOE_RX_START;
+                break;
+            }
+
+            eoe->tx_frame_number++;
+            eoe->tx_frame_number %= 16;
+
+            EC_WRITE_U16(data, 0x0100); // eoe fragment req.
+            EC_WRITE_U16(data + 2, (eoe->tx_frame_number & 0x0F) << 12);
+
+            memcpy(data + 4, frame->skb->data, frame->skb->len);
+            ec_master_queue_command(master, &eoe->slave->mbox_command);
+
+            eoe->last_tx_bytes = frame->skb->len;
+            dev_kfree_skb(frame->skb);
+            kfree(frame);
+
+            eoe->state = EC_EOE_TX_SENT;
+            break;
+
+        case EC_EOE_TX_SENT:
+            if (eoe->slave->mbox_command.state != EC_CMD_RECEIVED) {
+                eoe->stats.tx_errors++;
+                eoe->state = EC_EOE_RX_START;
+                break;
+            }
+            if (eoe->slave->mbox_command.working_counter != 1) {
+                eoe->stats.tx_errors++;
+                eoe->state = EC_EOE_RX_START;
+                break;
+            }
+
+            eoe->stats.tx_packets++;
+            eoe->stats.tx_bytes += eoe->last_tx_bytes;
+            eoe->state = EC_EOE_RX_START;
+            break;
+
+        default:
+            break;
     }
 }
 
@@ -210,7 +379,7 @@
 void ec_eoe_print(const ec_eoe_t *eoe)
 {
     EC_INFO("  EoE slave %i\n", eoe->slave->ring_position);
-    EC_INFO("    RX State %i\n", eoe->rx_state);
+    EC_INFO("    State %i\n", eoe->state);
     EC_INFO("    Assigned device: %s (%s)\n", eoe->dev->name,
             eoe->opened ? "opened" : "closed");
 }
@@ -249,8 +418,10 @@
 int ec_eoedev_open(struct net_device *dev /**< EoE net_device */)
 {
     ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+    ec_eoe_flush(eoe);
     eoe->opened = 1;
     netif_start_queue(dev);
+    eoe->tx_queue_active = 1;
     EC_INFO("%s (slave %i) opened.\n", dev->name, eoe->slave->ring_position);
     return 0;
 }
@@ -265,7 +436,9 @@
 {
     ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
     netif_stop_queue(dev);
+    eoe->tx_queue_active = 0;
     eoe->opened = 0;
+    ec_eoe_flush(eoe);
     EC_INFO("%s (slave %i) stopped.\n", dev->name, eoe->slave->ring_position);
     return 0;
 }
@@ -281,64 +454,44 @@
                 )
 {
     ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
+    ec_eoe_frame_t *frame;
+
+    if (skb->len + 10 > eoe->slave->sii_tx_mailbox_size) {
+        EC_WARN("EoE TX frame (%i octets) exceeds MTU. dropping.\n", skb->len);
+        dev_kfree_skb(skb);
+        eoe->stats.tx_dropped++;
+        return 0;
+    }
+
+    if (!(frame =
+          (ec_eoe_frame_t *) kmalloc(sizeof(ec_eoe_frame_t), GFP_ATOMIC))) {
+        if (printk_ratelimit())
+            EC_WARN("EoE TX: low on mem. frame dropped.\n");
+        return 1;
+    }
+
+    frame->skb = skb;
 
     spin_lock_bh(&eoe->tx_queue_lock);
-
-#if 0
-    eoe->stats.tx_packets++;
-    dev_kfree_skb(skb);
-    EC_INFO("EoE device sent %i octets.\n", skb->len);
-#endif
-
-    if (eoe->queued_frames == EC_EOE_TX_QUEUE_SIZE)
+    list_add_tail(&frame->queue, &eoe->tx_queue);
+    eoe->queued_frames++;
+    if (eoe->queued_frames == EC_EOE_TX_QUEUE_SIZE) {
         netif_stop_queue(dev);
-
+        eoe->tx_queue_active = 0;
+    }
     spin_unlock_bh(&eoe->tx_queue_lock);
 
+    EC_DBG("EoE TX queued frame with %i octets (%i frames queued).\n",
+           skb->len, eoe->queued_frames);
+    if (!eoe->tx_queue_active)
+        EC_WARN("EoE TX queue is now full.\n");
+
     return 0;
 }
 
 /*****************************************************************************/
 
 /**
-   Receives data from the bus and passes it to the network stack.
-*/
-
-void ec_eoedev_rx(struct net_device *dev, /**< EoE net_device */
-                  const uint8_t *data, /**< pointer to the data */
-                  size_t size /**< size of the received data */
-                 )
-{
-    ec_eoe_t *eoe = *((ec_eoe_t **) netdev_priv(dev));
-
-    // allocate socket buffer
-    if (!(eoe->skb = dev_alloc_skb(size + 2))) {
-        if (printk_ratelimit())
-            EC_WARN("EoE RX: low on mem. frame dropped.\n");
-        eoe->stats.rx_dropped++;
-        return;
-    }
-
-    // copy received data to socket buffer
-    memcpy(skb_put(eoe->skb, size), data, size);
-
-    // set socket buffer fields
-    eoe->skb->dev = dev;
-    eoe->skb->protocol = eth_type_trans(eoe->skb, dev);
-    eoe->skb->ip_summed = CHECKSUM_UNNECESSARY;
-
-    // update statistics
-    eoe->stats.rx_packets++;
-    eoe->stats.rx_bytes += size;
-
-    // pass socket buffer to network stack
-    netif_rx(eoe->skb);
-    eoe->skb = NULL;
-}
-
-/*****************************************************************************/
-
-/**
    Gets statistics about the virtual network device.
 */
 
--- a/master/ethernet.h	Wed May 03 08:01:20 2006 +0000
+++ b/master/ethernet.h	Fri May 05 07:20:10 2006 +0000
@@ -44,9 +44,13 @@
 
 typedef enum
 {
-    EC_EOE_IDLE,     /**< Idle. The next step ist to check for data. */
-    EC_EOE_CHECKING, /**< Checking frame was sent. */
-    EC_EOE_FETCHING  /**< There is new data. Fetching frame was sent. */
+    EC_EOE_RX_START, /**< start receiving and check for data. */
+    EC_EOE_RX_CHECK, /**< checking frame was sent. */
+    EC_EOE_RX_FETCH, /**< there is new data; fetching frame was sent. */
+    EC_EOE_TX_START, /**< start sending a queued frame. */
+    EC_EOE_TX_SENT,  /**< queued frame was sent; start checking. */
+    EC_EOE_TX_CHECK, /**< check mailbox for acknowledgement. */
+    EC_EOE_TX_FETCH, /**< receive mailbox response */
 }
 ec_eoe_state_t;
 
@@ -62,14 +66,20 @@
 {
     struct list_head list; /**< list item */
     ec_slave_t *slave; /**< pointer to the corresponding slave */
-    ec_eoe_state_t rx_state; /**< state of the state machine */
+    ec_eoe_state_t state; /**< state of the state machine */
     struct net_device *dev; /**< net_device for virtual ethernet device */
     uint8_t opened; /**< net_device is opened */
     struct sk_buff *skb; /**< current rx socket buffer */
+    off_t skb_offset; /**< current write pointer in the socket buffer */
+    size_t skb_size; /**< size of the allocated socket buffer memory */
+    uint8_t expected_fragment; /**< expected fragment */
     struct net_device_stats stats; /**< device statistics */
     struct list_head tx_queue; /**< queue for frames to send */
+    unsigned int tx_queue_active; /**< kernel netif queue started */
     unsigned int queued_frames; /**< number of frames in the queue */
     spinlock_t tx_queue_lock; /**< spinlock for the send queue */
+    uint8_t tx_frame_number; /**< Number of the transmitted frame */
+    size_t last_tx_bytes; /**< number of bytes currently transmitted */
 }
 ec_eoe_t;
 
--- a/master/globals.h	Wed May 03 08:01:20 2006 +0000
+++ b/master/globals.h	Fri May 05 07:20:10 2006 +0000
@@ -50,7 +50,7 @@
 #define EC_MAX_FMMUS 16
 
 /** size of the EoE tx queue */
-#define EC_EOE_TX_QUEUE_SIZE 10
+#define EC_EOE_TX_QUEUE_SIZE 100
 
 /******************************************************************************
  *  EtherCAT protocol
--- a/master/master.c	Wed May 03 08:01:20 2006 +0000
+++ b/master/master.c	Fri May 05 07:20:10 2006 +0000
@@ -215,7 +215,6 @@
     master->stats.delayed = 0;
     master->stats.corrupted = 0;
     master->stats.unmatched = 0;
-    master->stats.eoe_errors = 0;
     master->stats.t_last = 0;
 
     master->mode = EC_MASTER_MODE_IDLE;
@@ -622,10 +621,6 @@
             EC_WARN("%i command(s) UNMATCHED!\n", master->stats.unmatched);
             master->stats.unmatched = 0;
         }
-        if (master->stats.eoe_errors) {
-            EC_WARN("%i EOE ERROR(S)!\n", master->stats.eoe_errors);
-            master->stats.eoe_errors = 0;
-        }
         master->stats.t_last = t_now;
     }
 }
@@ -860,7 +855,7 @@
     master->release_cb(master->cb_data);
 
  restart_timer:
-    master->eoe_timer.expires += HZ / 4;
+    master->eoe_timer.expires += HZ / 1000;
     add_timer(&master->eoe_timer);
 }
 
--- a/master/master.h	Wed May 03 08:01:20 2006 +0000
+++ b/master/master.h	Fri May 05 07:20:10 2006 +0000
@@ -64,7 +64,6 @@
     unsigned int delayed; /**< delayed commands */
     unsigned int corrupted; /**< corrupted frames */
     unsigned int unmatched; /**< unmatched commands */
-    unsigned int eoe_errors; /**< Ethernet-over-EtherCAT errors */
     cycles_t t_last; /**< time of last output */
 }
 ec_stats_t;