--- zzzz-none-000/linux-4.4.271/drivers/net/usb/cdc_mbim.c	2021-06-03 06:22:09.000000000 +0000
+++ maple-fsgw-759/linux-4.4.271/drivers/net/usb/cdc_mbim.c	2023-12-20 10:37:21.000000000 +0000
@@ -27,6 +27,19 @@
 /* alternative VLAN for IP session 0 if not untagged */
 #define MBIM_IPS0_VID	4094
 
+#define AVM_VLAN_MAC_MAPPING
+
+#ifdef AVM_VLAN_MAC_MAPPING
+
+#define AVM_VLAN_ID_MAX 3
+static unsigned char avm_vlan_mac_addr[AVM_VLAN_ID_MAX+1][ETH_ALEN];
+
+inline bool avm_is_mac_empty(unsigned char mac_addr[ETH_ALEN]) {
+	/* check first two bytes of mac address */
+	return !((uint16_t *)mac_addr)[0];
+}
+#endif
+
 /* driver specific data - must match cdc_ncm usage */
 struct cdc_mbim_state {
 	struct cdc_ncm_ctx *ctx;
@@ -176,6 +189,11 @@
 		goto err;
 	}
 
+#ifdef AVM_VLAN_MAC_MAPPING
+	/* clear old mapping */
+	memset(avm_vlan_mac_addr, 0, sizeof avm_vlan_mac_addr);
+#endif
+
 	/* can't let usbnet use the interrupt endpoint */
 	dev->status = NULL;
 	info->subdriver = subdriver;
@@ -244,6 +262,12 @@
 		if (vlan_get_tag(skb, &tci) < 0 && skb->len > VLAN_ETH_HLEN &&
 		    __vlan_get_tag(skb, &tci) == 0) {
 			is_ip = is_ip_proto(vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
+#ifdef AVM_VLAN_MAC_MAPPING
+			/* save mac_addr of first incoming VLAN packet */
+			if (tci && (tci <= AVM_VLAN_ID_MAX) && avm_is_mac_empty(avm_vlan_mac_addr[tci])) {
+				memcpy(avm_vlan_mac_addr[tci], eth_hdr(skb)->h_source, ETH_ALEN);
+			}
+#endif
 			skb_pull(skb, VLAN_ETH_HLEN);
 		} else {
 			is_ip = is_ip_proto(eth_hdr(skb)->h_proto);
@@ -395,6 +419,11 @@
 	skb_reset_mac_header(skb);
 	eth_hdr(skb)->h_proto = proto;
 	eth_zero_addr(eth_hdr(skb)->h_source);
+#ifdef AVM_VLAN_MAC_MAPPING
+	if (tci && (tci <= AVM_VLAN_ID_MAX)) {
+		memcpy(eth_hdr(skb)->h_dest, avm_vlan_mac_addr[tci], ETH_ALEN);
+	} else
+#endif
 	memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
 
 	/* add datagram */