diff --git a/platform/src/packet_io.cpp b/platform/src/packet_io.cpp
index 3fb22a4..345de21 100644
--- a/platform/src/packet_io.cpp
+++ b/platform/src/packet_io.cpp
@@ -430,22 +430,27 @@ static void overwrite_vlan_id(struct vlan_hdr *vlan_hdr, uint16_t vlan_id)
 
 void vlan_encapsulate(marsio_buff_t *mbuff, int vlan_id, int replace_orig_vlan_header)
 {
+    struct ethhdr *old_eth_hdr = (struct ethhdr *)marsio_buff_mtod(mbuff);
+
     if (replace_orig_vlan_header == 0)
     {
     append:
-        struct ethhdr *eth_hdr = (struct ethhdr *)marsio_buff_prepend(mbuff, sizeof(struct vlan_hdr));
-        struct vlan_hdr *vlan_hdr = (struct vlan_hdr *)((char *)eth_hdr + sizeof(struct ethhdr));
+        struct ethhdr *new_eth_hdr = (struct ethhdr *)marsio_buff_prepend(mbuff, sizeof(struct vlan_hdr));
+        char *dst = (char *)new_eth_hdr;
+        char *src = (char *)old_eth_hdr;
+        memcpy(dst, src, 4);
+        memcpy(dst + 4, src + 4, 4);
+        memcpy(dst + 8, src + 8, 4);
+        new_eth_hdr->h_proto = htons(ETH_P_8021Q);
 
-        memmove(eth_hdr, (char *)eth_hdr + sizeof(struct vlan_hdr), sizeof(struct ethhdr));
-        build_vlan_header(vlan_hdr, vlan_id, ntohs(eth_hdr->h_proto));
-        eth_hdr->h_proto = htons(ETH_P_8021Q);
+        struct vlan_hdr *vlan_hdr = (struct vlan_hdr *)((char *)new_eth_hdr + sizeof(struct ethhdr));
+        build_vlan_header(vlan_hdr, vlan_id, ntohs(old_eth_hdr->h_proto));
         return;
     }
     else
     {
-        struct ethhdr *eth_hdr = (struct ethhdr *)marsio_buff_mtod(mbuff);
-        int next_proto = eth_hdr->h_proto;
-        char *start_layer = (char *)eth_hdr + sizeof(struct ethhdr);
+        uint16_t next_proto = old_eth_hdr->h_proto;
+        char *start_layer = (char *)old_eth_hdr + sizeof(struct ethhdr);
         char *next_layer = start_layer;
 
         while (next_proto == htons(ETH_P_8021Q) || next_proto == htons(ETH_P_8021AD))
@@ -477,7 +482,7 @@ void vlan_encapsulate(marsio_buff_t *mbuff, int vlan_id, int replace_orig_vlan_h
             struct ethhdr *new_eth_hdr = (struct ethhdr *)((char *)vlan_hdr - sizeof(struct ethhdr));
 
             overwrite_vlan_id(vlan_hdr, vlan_id);
-            memmove(new_eth_hdr, (char *)eth_hdr, sizeof(struct ethhdr));
+            memmove(new_eth_hdr, (char *)old_eth_hdr, sizeof(struct ethhdr));
             new_eth_hdr->h_proto = htons(ETH_P_8021Q);
 
             marsio_buff_adj(mbuff, offset - sizeof(struct vlan_hdr));