autobuild_mac80211_release/target/linux/mediatek/patches-5.4/8011-ovs-add-multicast-to-unicast-support.patch

diff --git a/net/openvswitch/actions.c b/net/openvswitch/actions.c
index 9e8a5c4..16f5187 100644
--- a/net/openvswitch/actions.c
+++ b/net/openvswitch/actions.c
@@ -919,6 +919,10 @@ static void do_output(struct datapath *dp, struct sk_buff *skb, int out_port,
 		      struct sw_flow_key *key)
 {
 	struct vport *vport = ovs_vport_rcu(dp, out_port);
+	struct multicast_data_base *mdb;
+	struct multicast_table *table;
+	struct multicast_table_entry *entry;
+	struct sk_buff *skb_cpy;
 
 	if (likely(vport)) {
 		u16 mru = OVS_CB(skb)->mru;
@@ -933,7 +937,28 @@ static void do_output(struct datapath *dp, struct sk_buff *skb, int out_port,
 
 		if (likely(!mru ||
 		           (skb->len <= mru + vport->dev->hard_header_len))) {
-			ovs_vport_send(vport, skb, ovs_key_mac_proto(key));
+			if (is_ipv4_multicast(skb) && !is_igmp(skb)) {
+				mdb = vport->mdb;
+				spin_lock(&mdb->tbl_lock);
+				list_for_each_entry(table, &mdb->list_head, mdb_node) {
+					if (table->group_addr.u.ip4 == key->ipv4.addr.dst) {
+						list_for_each_entry(entry, &table->entry_list, entry_node) {
+							skb_cpy = skb_copy(skb, GFP_ATOMIC);
+							if (!skb_cpy) {
+								kfree_skb(skb);
+								pr_err("%s(): error\n", __func__);
+								spin_unlock(&mdb->tbl_lock);
+								return;
+							}
+							memcpy(skb_cpy->data, entry->eth_addr, ETH_ALEN);
+							ovs_vport_send(vport, skb_cpy, ovs_key_mac_proto(key));
+						}
+					}
+				}
+				spin_unlock(&mdb->tbl_lock);
+				kfree_skb(skb);
+			} else
+				ovs_vport_send(vport, skb, ovs_key_mac_proto(key));
 		} else if (mru <= vport->dev->mtu) {
 			struct net *net = read_pnet(&dp->net);
 
diff --git a/net/openvswitch/datapath.c b/net/openvswitch/datapath.c
index 4f097bd..e6be550 100644
--- a/net/openvswitch/datapath.c
+++ b/net/openvswitch/datapath.c
@@ -11,6 +11,7 @@
 #include <linux/if_vlan.h>
 #include <linux/in.h>
 #include <linux/ip.h>
+#include <linux/igmp.h>
 #include <linux/jhash.h>
 #include <linux/delay.h>
 #include <linux/time.h>
@@ -530,6 +531,166 @@ static int queue_userspace_packet(struct datapath *dp, struct sk_buff *skb,
 	return err;
 }
 
+static int ovs_ip4_multicast_add_group(__be32 _group_addr,
+										const u8 *entry_addr,
+										struct vport *input_vport)
+{
+	struct multicast_data_base *mdb;
+	struct multicast_table *table;
+	struct multicast_table_entry *entry;
+
+	if (ipv4_is_local_multicast(_group_addr))
+		return 0;
+
+	mdb = input_vport->mdb;
+	spin_lock(&mdb->tbl_lock);
+	list_for_each_entry(table, &mdb->list_head, mdb_node) {
+		if (table->group_addr.u.ip4 == _group_addr) {
+			list_for_each_entry(entry, &table->entry_list, entry_node) {
+				if (!memcmp(entry->eth_addr, entry_addr, ETH_ALEN)) {
+					spin_unlock(&mdb->tbl_lock);
+					return 0;
+				}
+			}
+			entry = kzalloc(sizeof(struct multicast_table_entry), GFP_ATOMIC);
+			if (!entry) {
+				spin_unlock(&mdb->tbl_lock);
+				return -ENOMEM;
+			}
+
+			memcpy(entry->eth_addr, entry_addr, ETH_ALEN);
+			list_add(&entry->entry_node, &table->entry_list);
+			spin_unlock(&mdb->tbl_lock);
+			return 0;
+		}
+	}
+
+	table = kzalloc(sizeof(struct multicast_table), GFP_ATOMIC);
+	if (!table) {
+		spin_unlock(&mdb->tbl_lock);
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&table->entry_list);
+	entry = kzalloc(sizeof(struct multicast_table_entry), GFP_ATOMIC);
+	if (!entry) {
+		kfree(table);
+		spin_unlock(&mdb->tbl_lock);
+		return -ENOMEM;
+	}
+
+	memcpy(entry->eth_addr, entry_addr, ETH_ALEN);
+	list_add(&entry->entry_node, &table->entry_list);
+
+	table->group_addr.u.ip4 = _group_addr;
+	list_add(&table->mdb_node, &mdb->list_head);
+
+	spin_unlock(&mdb->tbl_lock);
+	return 0;
+}
+
+static int ovs_ip4_multicast_leave_group(__be32 _group_addr,
+										const u8 *entry_addr,
+										struct vport *input_vport)
+{
+	struct multicast_data_base *mdb;
+	struct multicast_table *table, *table_tmp;
+	struct multicast_table_entry *entry, *entry_tmp;
+
+	if (ipv4_is_local_multicast(_group_addr))
+		return 0;
+
+	mdb = input_vport->mdb;
+	spin_lock(&mdb->tbl_lock);
+	list_for_each_entry_safe(table, table_tmp, &mdb->list_head, mdb_node) {
+		if (table->group_addr.u.ip4 == _group_addr) {
+			list_for_each_entry_safe(entry, entry_tmp, &table->entry_list, entry_node) {
+				if (!memcmp(entry->eth_addr, entry_addr, ETH_ALEN)) {
+					list_del(&entry->entry_node);
+					kfree(entry);
+
+					if (list_empty(&table->entry_list)) {
+						list_del(&table->mdb_node);
+						kfree(table);
+					}
+					spin_unlock(&mdb->tbl_lock);
+					return 0;
+				}
+			}
+		}
+	}
+	spin_unlock(&mdb->tbl_lock);
+	return 0;
+}
+
+static int ovs_multicast_ipv4_rcv(struct sk_buff *skb, struct vport *input_vport)
+{
+	struct ethhdr *eth_hdr;
+	const u8 *dl_src;
+	__be32 group_addr;
+	struct iphdr *ip_header;
+	struct igmphdr *igmp_hdr;
+	int i;
+	struct igmpv3_report *igmpv3_hdr;
+	u16 group_num;
+	struct igmpv3_grec *grec;
+	u8 group_type;
+	u8 aux_data_len;
+	u16 num_of_source;
+	int err;
+
+	err = ip_mc_check_igmp(skb);
+	if (err < 0)
+		return 0;
+
+	eth_hdr = skb_eth_hdr(skb);
+	dl_src = eth_hdr->h_source;
+	ip_header = (struct iphdr *)(skb->data + 14);
+	igmp_hdr = (struct igmphdr *)((u8 *)ip_header + ip_header->ihl * 4);
+
+	switch (igmp_hdr->type) {
+	case IGMP_HOST_MEMBERSHIP_REPORT:
+	case IGMPV2_HOST_MEMBERSHIP_REPORT:
+		group_addr = igmp_hdr->group;
+		ovs_ip4_multicast_add_group(group_addr, dl_src, input_vport);
+		break;
+	case IGMP_HOST_LEAVE_MESSAGE:
+		group_addr = igmp_hdr->group;
+		ovs_ip4_multicast_leave_group(group_addr, dl_src, input_vport);
+		break;
+	case IGMPV3_HOST_MEMBERSHIP_REPORT:
+		igmpv3_hdr = (struct igmpv3_report *)igmp_hdr;
+		group_num = ntohs(igmpv3_hdr->ngrec);
+		grec = igmpv3_hdr->grec;
+		//group_num = ntohs(*(u16 *)(igmp_hdr + 6));
+		//group = igmp_hdr + 8;
+		for (i = 0; i < group_num; i++) {
+			group_type = grec->grec_type;
+			aux_data_len = grec->grec_auxwords;
+			num_of_source = ntohs(grec->grec_nsrcs);
+			group_addr = grec->grec_mca;
+
+			if (group_type == IGMPV3_MODE_IS_EXCLUDE ||
+				group_type == IGMPV3_CHANGE_TO_EXCLUDE ||
+				group_type == IGMPV3_ALLOW_NEW_SOURCES)
+				ovs_ip4_multicast_add_group(group_addr, dl_src, input_vport);
+
+			if (group_type == IGMPV3_MODE_IS_INCLUDE ||
+				group_type == IGMPV3_CHANGE_TO_INCLUDE ||
+				group_type == IGMPV3_BLOCK_OLD_SOURCES)
+				if (num_of_source == 0)
+					ovs_ip4_multicast_leave_group(group_addr, dl_src, input_vport);
+
+			grec += (8 + (num_of_source * 4) + aux_data_len);
+		}
+		break;
+	default:
+		pr_warning("%s(): error pkt\n", __func__);
+		break;
+	}
+	return 0;
+}
+
 static int ovs_packet_cmd_execute(struct sk_buff *skb, struct genl_info *info)
 {
 	struct ovs_header *ovs_header = info->userhdr;
@@ -604,6 +765,9 @@ static int ovs_packet_cmd_execute(struct sk_buff *skb, struct genl_info *info)
 	OVS_CB(packet)->input_vport = input_vport;
 	sf_acts = rcu_dereference(flow->sf_acts);
 
+	if (is_igmp(packet))
+		ovs_multicast_ipv4_rcv(packet, input_vport);
+
 	local_bh_disable();
 	err = ovs_execute_actions(dp, packet, sf_acts, &flow->key);
 	local_bh_enable();
@@ -2183,6 +2347,9 @@ static int ovs_vport_cmd_del(struct sk_buff *skb, struct genl_info *info)
 	struct datapath *dp;
 	struct vport *vport;
 	unsigned int new_headroom;
+	struct multicast_data_base *mdb;
+	struct multicast_table *table, *table_tmp;
+	struct multicast_table_entry *entry, *entry_tmp;
 	int err;
 
 	reply = ovs_vport_cmd_alloc_info();
@@ -2210,6 +2377,22 @@ static int ovs_vport_cmd_del(struct sk_buff *skb, struct genl_info *info)
 	if (netdev_get_fwd_headroom(vport->dev) == dp->max_headroom)
 		update_headroom = true;
 
+	mdb = vport->mdb;
+	spin_lock(&mdb->tbl_lock);
+	list_for_each_entry_safe(table, table_tmp, &mdb->list_head, mdb_node) {
+		list_for_each_entry_safe(entry, entry_tmp, &table->entry_list, entry_node) {
+			list_del(&entry->entry_node);
+			kfree(entry);
+
+			if (list_empty(&table->entry_list)) {
+				list_del(&table->mdb_node);
+				kfree(table);
+			}
+		}
+	}
+	spin_unlock(&mdb->tbl_lock);
+	kfree(mdb);
+
 	netdev_reset_rx_headroom(vport->dev);
 	ovs_dp_detach_port(vport);
 
diff --git a/net/openvswitch/datapath.h b/net/openvswitch/datapath.h
index 81e85dd..520532e 100644
--- a/net/openvswitch/datapath.h
+++ b/net/openvswitch/datapath.h
@@ -215,6 +215,31 @@ static inline struct datapath *get_dp(struct net *net, int dp_ifindex)
 	return dp;
 }
 
+#define IGMP_PROTOCOL_OFFSET		23
+/* support ipv4 for now */
+static inline bool is_ipv4_multicast(struct sk_buff *skb)
+{
+	struct ethhdr *eth_hdr = skb_eth_hdr(skb);
+
+	return eth_hdr->h_dest[0] == 0x01 && skb->protocol == htons(ETH_P_IP);
+}
+
+static inline bool is_igmp(struct sk_buff *skb)
+{
+	struct ethhdr *eth_hdr;
+
+	if (!skb)
+		return 0;
+
+	eth_hdr = skb_eth_hdr(skb);
+
+	if (eth_hdr->h_dest[0] == 0x01 &&
+		skb->protocol == htons(ETH_P_IP))
+		return (*(skb->data + IGMP_PROTOCOL_OFFSET) == IPPROTO_IGMP);
+	else
+		return 0;
+}
+
 extern struct notifier_block ovs_dp_device_notifier;
 extern struct genl_family dp_vport_genl_family;
 
diff --git a/net/openvswitch/vport.c b/net/openvswitch/vport.c
index 19af0ef..77bc923 100644
--- a/net/openvswitch/vport.c
+++ b/net/openvswitch/vport.c
@@ -141,6 +141,14 @@ struct vport *ovs_vport_alloc(int priv_size, const struct vport_ops *ops,
 		return ERR_PTR(-EINVAL);
 	}
 
+	vport->mdb = kzalloc(sizeof(struct multicast_data_base), GFP_KERNEL);
+	if (!vport->mdb) {
+		kfree(vport);
+		return ERR_PTR(-ENOMEM);
+	}
+	INIT_LIST_HEAD(&vport->mdb->list_head);
+	spin_lock_init(&vport->mdb->tbl_lock);
+
 	return vport;
 }
 EXPORT_SYMBOL_GPL(ovs_vport_alloc);
diff --git a/net/openvswitch/vport.h b/net/openvswitch/vport.h
index 1eb7495..eb69d6c 100644
--- a/net/openvswitch/vport.h
+++ b/net/openvswitch/vport.h
@@ -55,6 +55,30 @@ struct vport_portids {
 	u32 ids[];
 };
 
+struct ip_addr {
+	union {
+		__be32 ip4;
+		struct in6_addr ip6;
+	} u;
+};
+
+struct multicast_table_entry {
+	struct list_head entry_node;
+	u8 eth_addr[ETH_ALEN];
+};
+
+struct multicast_table {
+	struct list_head mdb_node;
+	struct list_head entry_list;
+	struct ip_addr group_addr;
+};
+
+struct multicast_data_base {
+	struct list_head list_head;
+	spinlock_t tbl_lock;
+};
+
+
 /**
  * struct vport - one port within a datapath
  * @dev: Pointer to net_device.
@@ -79,6 +103,8 @@ struct vport {
 
 	struct list_head detach_list;
 	struct rcu_head rcu;
+
+	struct multicast_data_base *mdb;
 };
 
 /**