diff mbox

[v4,2/2] rtl8139: add vlan tag insertion

Message ID 1299105380-24826-3-git-send-email-benjamin.poirier@gmail.com
State New
Headers show

Commit Message

Benjamin Poirier March 2, 2011, 10:36 p.m. UTC
Add support to the emulated hardware to insert vlan tags in packets
going from the guest to the network.

Signed-off-by: Benjamin Poirier <benjamin.poirier@gmail.com>
Cc: Igor V. Kovalenko <igor.v.kovalenko@gmail.com>
Cc: Jason Wang <jasowang@redhat.com>
Cc: Michael S. Tsirkin <mst@redhat.com>
---
 hw/rtl8139.c |  102 ++++++++++++++++++++++++++++++++++++++++++----------------
 1 files changed, 74 insertions(+), 28 deletions(-)

Comments

Michael S. Tsirkin March 3, 2011, 7:47 a.m. UTC | #1
On Wed, Mar 02, 2011 at 05:36:20PM -0500, Benjamin Poirier wrote:
> Add support to the emulated hardware to insert vlan tags in packets
> going from the guest to the network.
> 
> Signed-off-by: Benjamin Poirier <benjamin.poirier@gmail.com>
> Cc: Igor V. Kovalenko <igor.v.kovalenko@gmail.com>
> Cc: Jason Wang <jasowang@redhat.com>
> Cc: Michael S. Tsirkin <mst@redhat.com>
> ---
>  hw/rtl8139.c |  102 ++++++++++++++++++++++++++++++++++++++++++----------------
>  1 files changed, 74 insertions(+), 28 deletions(-)
> 
> diff --git a/hw/rtl8139.c b/hw/rtl8139.c
> index 6e770bd..0ea79e4 100644
> --- a/hw/rtl8139.c
> +++ b/hw/rtl8139.c
> @@ -832,11 +832,14 @@ 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;
> +    /* size_ is the total length of argument buffers */
> +    int size_ = buf_size + (dot1q_buf ? VLAN_HDR_LEN : 0);
> +    /* size is the length of the buffer passed to the driver */
>      int size = size_;
> -    const uint8_t *dot1q_buf;
>      const uint8_t *next_part;
>      size_t next_part_size;
>  
> @@ -1018,10 +1021,13 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
>          /* next_part starts right after the vlan header (if any), at the
>           * ethertype for the payload */
>          next_part = &buf[ETHER_ADDR_LEN * 2];
> -        if (s->CpCmd & CPlusRxVLAN && be16_to_cpup((uint16_t *)
> -                &buf[ETHER_ADDR_LEN * 2]) == ETHERTYPE_VLAN) {
> -            dot1q_buf = &buf[ETHER_ADDR_LEN * 2];
> -            next_part += VLAN_HDR_LEN;
> +        if (s->CpCmd & CPlusRxVLAN && (dot1q_buf || be16_to_cpup((uint16_t *)
> +                    &buf[ETHER_ADDR_LEN * 2]) == ETHERTYPE_VLAN)) {
> +            if (!dot1q_buf) {
> +                /* the tag is in the buffer */
> +                dot1q_buf = &buf[ETHER_ADDR_LEN * 2];
> +                next_part += VLAN_HDR_LEN;
> +            }
>              size -= VLAN_HDR_LEN;
>  
>              rxdw1 &= ~CP_RX_VLAN_TAG_MASK;
> @@ -1034,9 +1040,8 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
>          } else {
>              /* reset VLAN tag flag */
>              rxdw1 &= ~CP_RX_TAVA;
> -            dot1q_buf = NULL;
>          }
> -        next_part_size = buf + size_ - next_part;
> +        next_part_size = buf + buf_size - next_part;
>  
>          /* if too small buffer, then expand it */
>          if (size < MIN_BUF_SIZE) {
> @@ -1164,10 +1169,18 @@ 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)) {

unlikely kind of iffy here

> +                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;
> +            dot1q_buf = NULL;
>          }
>  
>          /* begin ring receiver mode */
> @@ -1196,8 +1209,19 @@ static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
>          rtl8139_write_buffer(s, (uint8_t *)&val, 4);
>  
>          /* receive/copy to target memory */
> -        rtl8139_write_buffer(s, buf, size);
> -        val = rtl8139_crc32(0, buf, size);
> +        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)
> @@ -1227,7 +1251,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)
> @@ -1802,7 +1826,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)
>      {
> @@ -1813,11 +1838,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);
> +        }
>      }
>  }
>  
> @@ -1851,7 +1887,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));
>  
> @@ -1978,7 +2014,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);
> @@ -1990,9 +2025,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 */
> @@ -2016,9 +2048,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 */
> @@ -2118,13 +2150,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 */
> @@ -2133,6 +2164,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));

left over debugging output

> +
> +            dot1q_buffer = alloca(VLAN_HDR_LEN);

Don't use alloca.  Just
 uint8_t dot1q_buffer[VLAN_HDR_LEN]
or something like that.

> +            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;
> @@ -2290,7 +2336,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);

This line's way too long.

>  
>                          /* 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));
> @@ -2363,7 +2409,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
diff mbox

Patch

diff --git a/hw/rtl8139.c b/hw/rtl8139.c
index 6e770bd..0ea79e4 100644
--- a/hw/rtl8139.c
+++ b/hw/rtl8139.c
@@ -832,11 +832,14 @@  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;
+    /* size_ is the total length of argument buffers */
+    int size_ = buf_size + (dot1q_buf ? VLAN_HDR_LEN : 0);
+    /* size is the length of the buffer passed to the driver */
     int size = size_;
-    const uint8_t *dot1q_buf;
     const uint8_t *next_part;
     size_t next_part_size;
 
@@ -1018,10 +1021,13 @@  static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
         /* next_part starts right after the vlan header (if any), at the
          * ethertype for the payload */
         next_part = &buf[ETHER_ADDR_LEN * 2];
-        if (s->CpCmd & CPlusRxVLAN && be16_to_cpup((uint16_t *)
-                &buf[ETHER_ADDR_LEN * 2]) == ETHERTYPE_VLAN) {
-            dot1q_buf = &buf[ETHER_ADDR_LEN * 2];
-            next_part += VLAN_HDR_LEN;
+        if (s->CpCmd & CPlusRxVLAN && (dot1q_buf || be16_to_cpup((uint16_t *)
+                    &buf[ETHER_ADDR_LEN * 2]) == ETHERTYPE_VLAN)) {
+            if (!dot1q_buf) {
+                /* the tag is in the buffer */
+                dot1q_buf = &buf[ETHER_ADDR_LEN * 2];
+                next_part += VLAN_HDR_LEN;
+            }
             size -= VLAN_HDR_LEN;
 
             rxdw1 &= ~CP_RX_VLAN_TAG_MASK;
@@ -1034,9 +1040,8 @@  static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
         } else {
             /* reset VLAN tag flag */
             rxdw1 &= ~CP_RX_TAVA;
-            dot1q_buf = NULL;
         }
-        next_part_size = buf + size_ - next_part;
+        next_part_size = buf + buf_size - next_part;
 
         /* if too small buffer, then expand it */
         if (size < MIN_BUF_SIZE) {
@@ -1164,10 +1169,18 @@  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;
+            dot1q_buf = NULL;
         }
 
         /* begin ring receiver mode */
@@ -1196,8 +1209,19 @@  static ssize_t rtl8139_do_receive(VLANClientState *nc, const uint8_t *buf, size_
         rtl8139_write_buffer(s, (uint8_t *)&val, 4);
 
         /* receive/copy to target memory */
-        rtl8139_write_buffer(s, buf, size);
-        val = rtl8139_crc32(0, buf, size);
+        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)
@@ -1227,7 +1251,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)
@@ -1802,7 +1826,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)
     {
@@ -1813,11 +1838,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);
+        }
     }
 }
 
@@ -1851,7 +1887,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));
 
@@ -1978,7 +2014,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);
@@ -1990,9 +2025,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 */
@@ -2016,9 +2048,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 */
@@ -2118,13 +2150,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 */
@@ -2133,6 +2164,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;
@@ -2290,7 +2336,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));
@@ -2363,7 +2409,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)