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

[Benjamin Poirier]

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

Signed-off-by: Benjamin Poirier <address@hidden>
Cc: Igor V. Kovalenko <address@hidden>
Cc: Jason Wang <address@hidden>
Cc: Michael S. Tsirkin <address@hidden>
---
 hw/rtl8139.c |  123 +++++++++++++++++++++++++++++++++++++++++++++-------------
 1 files changed, 96 insertions(+), 27 deletions(-)

diff --git a/hw/rtl8139.c b/hw/rtl8139.c
index a22530c..35ccd3d 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"
@@ -68,6 +70,16 @@
 #if defined(RTL8139_CALCULATE_RXCRC)
 /* For crc32 */
 #include <zlib.h>
+
+static inline uLong rtl8139_crc32(uLong crc, const Bytef *buf, uInt len)
+{
+    return crc32(crc, buf, len);
+}
+#else
+static inline uLong rtl8139_crc32(uLong crc, const Bytef *buf, uInt len)
+{
+    return 0;
+}
 #endif
 
 #define SET_MASKED(input, mask, curr) \
@@ -77,6 +89,9 @@
 #define MOD2(input, size) \
     ( ( input ) & ( size - 1 )  )
 
+#define VLAN_TCI_LEN 2
+#define VLAN_HDR_LEN (ETHER_TYPE_LEN + VLAN_TCI_LEN)
+
 #if defined (DEBUG_RTL8139)
 #  define DEBUG_PRINT(x) do { printf x ; } while (0)
 #else
@@ -814,9 +829,11 @@ static int rtl8139_can_receive(VLANClientState *nc)
     }
 }
 
-static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, 
size_t size_, int do_interrupt)
+static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf,
+    size_t buf_size, int do_interrupt, const uint8_t *dot1q_buf)
 {
     RTL8139State *s = DO_UPCAST(NICState, nc, nc)->opaque;
+    int size_ = buf_size + (dot1q_buf ? VLAN_HDR_LEN : 0);
     int size = size_;
 
     uint32_t packet_header = 0;
@@ -935,7 +952,14 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, 
const uint8_t *buf, size_
 
     /* if too small buffer, then expand it */
     if (size < MIN_BUF_SIZE) {
-        memcpy(buf1, buf, size);
+        if (unlikely(dot1q_buf)) {
+            memcpy(buf1, buf, 2 * ETHER_ADDR_LEN);
+            memcpy(buf1 + 2 * ETHER_ADDR_LEN, dot1q_buf, VLAN_HDR_LEN);
+            memcpy(buf1 + 2 * ETHER_ADDR_LEN + VLAN_HDR_LEN, buf + 2 *
+                ETHER_ADDR_LEN, buf_size - 2 * ETHER_ADDR_LEN);
+        } else {
+            memcpy(buf1, buf, size);
+        }
         memset(buf1 + size, 0, MIN_BUF_SIZE - size);
         buf = buf1;
         size = MIN_BUF_SIZE;
@@ -1022,7 +1046,21 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, 
const uint8_t *buf, size_
         target_phys_addr_t rx_addr = rtl8139_addr64(rxbufLO, rxbufHI);
 
         /* receive/copy to target memory */
-        cpu_physical_memory_write( rx_addr, buf, size );
+        if (unlikely(dot1q_buf)) {
+            cpu_physical_memory_write(rx_addr, buf, 2 * ETHER_ADDR_LEN);
+            val = rtl8139_crc32(0, buf, 2 * ETHER_ADDR_LEN);
+            cpu_physical_memory_write(rx_addr + 2 * ETHER_ADDR_LEN, dot1q_buf,
+                VLAN_HDR_LEN);
+            val = rtl8139_crc32(val, dot1q_buf, VLAN_HDR_LEN);
+            cpu_physical_memory_write(rx_addr + 2 * ETHER_ADDR_LEN +
+                VLAN_HDR_LEN, buf + 2 * ETHER_ADDR_LEN, buf_size - 2 *
+                ETHER_ADDR_LEN);
+            val = rtl8139_crc32(val, buf + 2 * ETHER_ADDR_LEN, buf_size - 2 *
+                ETHER_ADDR_LEN);
+        } else {
+            cpu_physical_memory_write(rx_addr, buf, size);
+            val = rtl8139_crc32(0, buf, size);
+        }
 
         if (s->CpCmd & CPlusRxChkSum)
         {
@@ -1031,9 +1069,7 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, 
const uint8_t *buf, size_
 
         /* write checksum */
 #if defined (RTL8139_CALCULATE_RXCRC)
-        val = cpu_to_le32(crc32(0, buf, size));
-#else
-        val = 0;
+        val = cpu_to_le32(val);
 #endif
         cpu_physical_memory_write( rx_addr+size, (uint8_t *)&val, 4);
 
@@ -1133,13 +1169,24 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, 
const uint8_t *buf, size_
 
         rtl8139_write_buffer(s, (uint8_t *)&val, 4);
 
-        rtl8139_write_buffer(s, buf, size);
+        /* receive/copy to target memory */
+        if (unlikely(dot1q_buf)) {
+            rtl8139_write_buffer(s, buf, 2 * ETHER_ADDR_LEN);
+            val = rtl8139_crc32(0, buf, 2 * ETHER_ADDR_LEN);
+            rtl8139_write_buffer(s, dot1q_buf, VLAN_HDR_LEN);
+            val = rtl8139_crc32(val, dot1q_buf, VLAN_HDR_LEN);
+            rtl8139_write_buffer(s, buf + 2 * ETHER_ADDR_LEN, buf_size - 2 *
+                ETHER_ADDR_LEN);
+            val = rtl8139_crc32(val, buf + 2 * ETHER_ADDR_LEN, buf_size - 2 *
+                ETHER_ADDR_LEN);
+        } else {
+            rtl8139_write_buffer(s, buf, size);
+            val = rtl8139_crc32(0, buf, size);
+        }
 
         /* write checksum */
 #if defined (RTL8139_CALCULATE_RXCRC)
-        val = cpu_to_le32(crc32(0, buf, size));
-#else
-        val = 0;
+        val = cpu_to_le32(val);
 #endif
 
         rtl8139_write_buffer(s, (uint8_t *)&val, 4);
@@ -1165,7 +1212,7 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, 
const uint8_t *buf, size_
 
 static ssize_t rtl8139_receive(VLANClientState *nc, const uint8_t *buf, size_t 
size)
 {
-    return rtl8139_do_receive(nc, buf, size, 1);
+    return rtl8139_do_receive(nc, buf, size, 1, NULL);
 }
 
 static void rtl8139_reset_rxring(RTL8139State *s, uint32_t bufferSize)
@@ -1740,7 +1787,8 @@ static uint32_t rtl8139_RxConfig_read(RTL8139State *s)
     return ret;
 }
 
-static void rtl8139_transfer_frame(RTL8139State *s, const uint8_t *buf, int 
size, int do_interrupt)
+static void rtl8139_transfer_frame(RTL8139State *s, uint8_t *buf, int size,
+    int do_interrupt, const uint8_t *dot1q_buf)
 {
     if (!size)
     {
@@ -1751,11 +1799,22 @@ static void rtl8139_transfer_frame(RTL8139State *s, 
const uint8_t *buf, int size
     if (TxLoopBack == (s->TxConfig & TxLoopBack))
     {
         DEBUG_PRINT(("RTL8139: +++ transmit loopback mode\n"));
-        rtl8139_do_receive(&s->nic->nc, buf, size, do_interrupt);
+        rtl8139_do_receive(&s->nic->nc, buf, size, do_interrupt, dot1q_buf);
     }
     else
     {
-        qemu_send_packet(&s->nic->nc, buf, size);
+        if (dot1q_buf) {
+            struct iovec iov[] = {
+                { .iov_base = buf, .iov_len = ETHER_ADDR_LEN * 2 },
+                { .iov_base = (void *) dot1q_buf, .iov_len = VLAN_HDR_LEN },
+                { .iov_base = buf + ETHER_ADDR_LEN * 2,
+                    .iov_len = size - ETHER_ADDR_LEN * 2 },
+            };
+
+            qemu_sendv_packet(&s->nic->nc, iov, ARRAY_SIZE(iov));
+        } else {
+            qemu_send_packet(&s->nic->nc, buf, size);
+        }
     }
 }
 
@@ -1789,7 +1848,7 @@ static int rtl8139_transmit_one(RTL8139State *s, int 
descriptor)
     s->TxStatus[descriptor] |= TxHostOwns;
     s->TxStatus[descriptor] |= TxStatOK;
 
-    rtl8139_transfer_frame(s, txbuffer, txsize, 0);
+    rtl8139_transfer_frame(s, txbuffer, txsize, 0, NULL);
 
     DEBUG_PRINT(("RTL8139: +++ transmitted %d bytes from descriptor %d\n", 
txsize, descriptor));
 
@@ -1916,7 +1975,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);
@@ -1928,9 +1986,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 */
@@ -1954,9 +2009,9 @@ 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 bits 0...15 : VLAN tag */
+/* w1 add tag flag */
+#define CP_TX_TAGC (1<<17)
+/* w1 bits 0...15 : VLAN tag (big endian) */
 #define CP_TX_VLAN_TAG_MASK ((1<<16) - 1)
 /* w2 low  32bit of Rx buffer ptr */
 /* w3 high 32bit of Rx buffer ptr */
@@ -2056,13 +2111,12 @@ 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)
     {
+        uint16_t *dot1q_buffer;
+
         DEBUG_PRINT(("RTL8139: +++ C+ Tx mode : descriptor %d is last segment 
descriptor\n", descriptor));
 
         /* can transfer fully assembled packet */
@@ -2071,6 +2125,21 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
         int      saved_size    = s->cplus_txbuffer_offset;
         int      saved_buffer_len = s->cplus_txbuffer_len;
 
+        /* create vlan tag */
+        if (txdw1 & CP_TX_TAGC) {
+            /* the vlan tag is in BE byte order in the descriptor
+             * BE + le_to_cpu() + ~swap()~ = cpu */
+            printf("RTL8139: +++ C+ Tx mode : inserting vlan tag with "
+                "tci: %u\n", bswap16(txdw1 & CP_TX_VLAN_TAG_MASK));
+
+            dot1q_buffer = alloca(VLAN_HDR_LEN);
+            dot1q_buffer[0] = cpu_to_be16(ETHERTYPE_VLAN);
+            /* BE + le_to_cpu() + ~cpu_to_le()~ = BE */
+            dot1q_buffer[1] = cpu_to_le16(txdw1 & CP_TX_VLAN_TAG_MASK);
+        } else {
+            dot1q_buffer = NULL;
+        }
+
         /* reset the card space to protect from recursive call */
         s->cplus_txbuffer = NULL;
         s->cplus_txbuffer_offset = 0;
@@ -2228,7 +2297,7 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
 
                         int tso_send_size = ETH_HLEN + hlen + tcp_hlen + 
chunk_size;
                         DEBUG_PRINT(("RTL8139: +++ C+ mode TSO transferring 
packet size %d\n", tso_send_size));
-                        rtl8139_transfer_frame(s, saved_buffer, tso_send_size, 
0);
+                        rtl8139_transfer_frame(s, saved_buffer, tso_send_size, 
0, (uint8_t *) dot1q_buffer);
 
                         /* add transferred count to TCP sequence number */
                         p_tcp_hdr->th_seq = cpu_to_be32(chunk_size + 
be32_to_cpu(p_tcp_hdr->th_seq));
@@ -2301,7 +2370,7 @@ static int rtl8139_cplus_transmit_one(RTL8139State *s)
 
         DEBUG_PRINT(("RTL8139: +++ C+ mode transmitting %d bytes packet\n", 
saved_size));
 
-        rtl8139_transfer_frame(s, saved_buffer, saved_size, 1);
+        rtl8139_transfer_frame(s, saved_buffer, saved_size, 1, (uint8_t *) 
dot1q_buffer);
 
         /* restore card space if there was no recursion and reset offset */
         if (!s->cplus_txbuffer)
-- 
1.7.2.3