[Qemu-devel] [PATCH 1/2] rtl8139: add vlan tag insertion

[Benjamin Poirier <benjamin.poirier@polymtl.ca>]

Add support to the emulated hardware to add vlan tags in packets going
from the guest to the network.

Signed-off-by: Benjamin Poirier <benjamin.poirier@polymtl.ca>
Cc: Igor V. Kovalenko <igor.v.kovalenko@gmail.com>
---
 hw/rtl8139.c |   46 +++++++++++++++++++++++++++++++++++-----------
 1 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/hw/rtl8139.c b/hw/rtl8139.c
index d92981d..ac294da 100644
--- a/hw/rtl8139.c
+++ b/hw/rtl8139.c
@@ -47,6 +47,8 @@
  *                                  Darwin)
  */
 
+#include <net/ethernet.h>
+
 #include "hw.h"
 #include "pci.h"
 #include "qemu-timer.h"
@@ -58,6 +60,10 @@
 
 #define PCI_FREQUENCY 33000000L
 
+/* bytes in VLAN tag */
+#define VLAN_TCI_LEN 2
+#define VLAN_HDR_LEN (ETHER_TYPE_LEN + VLAN_TCI_LEN)
+
 /* debug RTL8139 card C+ mode only */
 //#define DEBUG_RTL8139CP 1
 
@@ -1913,7 +1919,6 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
 
     cpu_physical_memory_read(cplus_tx_ring_desc,    (uint8_t *)&val, 4);
     txdw0 = le32_to_cpu(val);
-    /* TODO: implement VLAN tagging support, VLAN tag data is read to txdw1 */
     cpu_physical_memory_read(cplus_tx_ring_desc+4,  (uint8_t *)&val, 4);
     txdw1 = le32_to_cpu(val);
     cpu_physical_memory_read(cplus_tx_ring_desc+8,  (uint8_t *)&val, 4);
@@ -1925,9 +1930,6 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
            descriptor,
            txdw0, txdw1, txbufLO, txbufHI));
 
-    /* TODO: the following discard cast should clean clang analyzer output */
-    (void)txdw1;
-
 /* w0 ownership flag */
 #define CP_TX_OWN (1<<31)
 /* w0 end of ring flag */
@@ -1951,8 +1953,8 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
 /* w0 bits 0...15 : buffer size */
 #define CP_TX_BUFFER_SIZE (1<<16)
 #define CP_TX_BUFFER_SIZE_MASK (CP_TX_BUFFER_SIZE - 1)
-/* w1 tag available flag */
-#define CP_RX_TAGC (1<<17)
+/* w1 add tag flag */
+#define CP_TX_TAGC (1<<17)
 /* w1 bits 0...15 : VLAN tag */
 #define CP_TX_VLAN_TAG_MASK ((1<<16) - 1)
 /* w2 low  32bit of Rx buffer ptr */
@@ -1978,12 +1980,22 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
 
     DEBUG_PRINT(("RTL8139: +++ C+ Tx mode : transmitting from descriptor %d\n", descriptor));
 
+    int vlan_extra_size = 0;
     if (txdw0 & CP_TX_FS)
     {
         DEBUG_PRINT(("RTL8139: +++ C+ Tx mode : descriptor %d is first segment descriptor\n", descriptor));
 
+        DEBUG_PRINT(("RTL8139: +++ C+ Tx mode : add vlan tag: %u tci: %u\n",
+                !!(txdw1 & CP_TX_TAGC), bswap16(txdw1 &
+                    CP_TX_VLAN_TAG_MASK)));
+
         /* reset internal buffer offset */
         s->cplus_txbuffer_offset = 0;
+
+        if (txdw1 & CP_TX_TAGC)
+        {
+            vlan_extra_size = VLAN_HDR_LEN;
+        }
     }
 
     int txsize = txdw0 & CP_TX_BUFFER_SIZE_MASK;
@@ -1992,14 +2004,15 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
     /* make sure we have enough space to assemble the packet */
     if (!s->cplus_txbuffer)
     {
-        s->cplus_txbuffer_len = CP_TX_BUFFER_SIZE;
+        s->cplus_txbuffer_len = CP_TX_BUFFER_SIZE + VLAN_HDR_LEN;
         s->cplus_txbuffer = qemu_malloc(s->cplus_txbuffer_len);
         s->cplus_txbuffer_offset = 0;
 
         DEBUG_PRINT(("RTL8139: +++ C+ mode transmission buffer allocated space %d\n", s->cplus_txbuffer_len));
     }
 
-    while (s->cplus_txbuffer && s->cplus_txbuffer_offset + txsize >= s->cplus_txbuffer_len)
+    while (s->cplus_txbuffer && s->cplus_txbuffer_offset + txsize +
+        vlan_extra_size >= s->cplus_txbuffer_len)
     {
         s->cplus_txbuffer_len += CP_TX_BUFFER_SIZE;
         s->cplus_txbuffer = qemu_realloc(s->cplus_txbuffer, s->cplus_txbuffer_len);
@@ -2025,6 +2038,20 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
     DEBUG_PRINT(("RTL8139: +++ C+ mode transmit reading %d bytes from host memory at %016" PRIx64 " to offset %d\n",
                  txsize, (uint64_t)tx_addr, s->cplus_txbuffer_offset));
 
+    if (vlan_extra_size && txsize >= 2 * ETHER_ADDR_LEN)
+    {
+        /* copy addresses */
+        cpu_physical_memory_read(tx_addr, s->cplus_txbuffer, 2 *
+            ETHER_ADDR_LEN);
+        tx_addr += 2 * ETHER_ADDR_LEN;
+        txsize -= 2 * ETHER_ADDR_LEN;
+        /* insert vlan tag */
+        *(uint16_t *)(s->cplus_txbuffer + 2 * ETHER_ADDR_LEN) =
+            cpu_to_be16(ETHERTYPE_VLAN);
+        *(uint16_t *)(s->cplus_txbuffer + 2 * ETHER_ADDR_LEN + ETHER_TYPE_LEN)
+            = cpu_to_le16(txdw1 & CP_TX_VLAN_TAG_MASK);
+        s->cplus_txbuffer_offset += 2 * ETHER_ADDR_LEN + VLAN_HDR_LEN;
+    }
     cpu_physical_memory_read(tx_addr, s->cplus_txbuffer + s->cplus_txbuffer_offset, txsize);
     s->cplus_txbuffer_offset += txsize;
 
@@ -2053,9 +2080,6 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
     /* update ring data */
     val = cpu_to_le32(txdw0);
     cpu_physical_memory_write(cplus_tx_ring_desc,    (uint8_t *)&val, 4);
-    /* TODO: implement VLAN tagging support, VLAN tag data is read to txdw1 */
-//    val = cpu_to_le32(txdw1);
-//    cpu_physical_memory_write(cplus_tx_ring_desc+4,  &val, 4);
 
     /* Now decide if descriptor being processed is holding the last segment of packet */
     if (txdw0 & CP_TX_LS)
-- 
1.7.2.3