From 6546c316644dd1c04088242f3274ef6a1579e3c4 Mon Sep 17 00:00:00 2001
From: wanfeng <wanfeng@kylinos.cn>
Date: Fri, 28 Jun 2024 10:56:51 +0800
Subject: [PATCH] add MCAST_MSFILTER to setsockopt for IGMPv3

---
 src/api/sockets.c | 137 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 137 insertions(+)

diff --git a/src/api/sockets.c b/src/api/sockets.c
index a030f4b..7a67650 100644
--- a/src/api/sockets.c
+++ b/src/api/sockets.c
@@ -3940,6 +3940,27 @@ lwip_setsockopt_impl(int s, int level, int optname, const void *optval, socklen_
           if (NETCONNTYPE_GROUP(netconn_type(sock->conn)) == NETCONN_RAW) {
             err = mcast_sock_block_unblock_source_group(&sock->conn->pcb.raw->ipmc, optname, (const struct group_source_req *)optval);
           } else
+#endif /* LWIP_RAW */
+          {
+            done_socket(sock);
+            return ENOPROTOOPT;
+          }
+          break;
+        case MCAST_MSFILTER:
+          LWIP_SOCKOPT_CHECK_OPTLEN_CONN_PCB(sock, optlen, struct group_filter);
+          if (((const struct group_filter *)optval)->gf_group.ss_family != AF_INET) {
+            done_socket(sock);
+            return EINVAL;
+          }
+#if LWIP_UDP
+          if (NETCONNTYPE_GROUP(netconn_type(sock->conn)) == NETCONN_UDP) {
+            err = mcast_sock_set_groupfilter(&sock->conn->pcb.udp->ipmc, optname, (const struct group_filter *)optval);
+          } else
+#endif /* LWIP_UDP */
+#if LWIP_RAW
+          if (NETCONNTYPE_GROUP(netconn_type(sock->conn)) == NETCONN_RAW) {
+            err = mcast_sock_set_groupfilter(&sock->conn->pcb.raw->ipmc, optname, (const struct group_filter *)optval);
+          } else
 #endif /* LWIP_RAW */
           {
             done_socket(sock);
@@ -4729,6 +4750,78 @@ lwip_socket_drop_registered_mld6_memberships(int s)
 }
 #endif /* LWIP_IPV6_MLD */
 
+/** Set filter to a multicast group (group_filter)
+ *
+ * @param ipmc multicast filter control block
+ * @param netif the network interface which group we already join.
+ * @param multi_addr the address of the group to add source
+ * @param gf filter
+ * @return lwIP error definitions
+ */
+err_t
+mcast_set_groupfilter_netif(struct ip_mc *ipmc, struct netif *netif, const ip_addr_t *multi_addr, const struct group_filter *gf)
+{
+#if LWIP_IPV4 && LWIP_IGMP
+  if (IP_IS_V4(multi_addr)) {
+    struct ip4_mc *mc;
+    struct igmp_src *src, *head = NULL;
+    u32_t i;
+
+    mc = mcast_ip4_mc_find(ipmc, netif, ip_2_ip4(multi_addr), NULL);
+    if (mc == NULL) {
+      return ERR_VAL;
+    }
+    mc->num_src = 0;
+    for (i = 0; i < gf->gf_numsrc; i++) {
+      src = (struct igmp_src *)mem_malloc(sizeof(struct igmp_src));
+      if (src == NULL) {
+        mcast_ip4_mc_src_remove(head);
+        return ERR_MEM;
+      }
+      inet_addr_to_ip4addr(&src->src_addr, &(((struct sockaddr_in *)&(gf->gf_slist[i]))->sin_addr));
+      src->next = head;
+      head = src;
+      mc->num_src++;
+    }
+
+    mc->fmode = (u8_t)gf->gf_fmode;
+    mcast_ip4_mc_src_remove(mc->src);
+    mc->src = head;
+    IP4_MC_TRIGGER_CALL(netif, ip_2_ip4(multi_addr)); /* trigger a report */
+  } else
+#endif /* LWIP_IPV4 && LWIP_IGMP */
+  {
+#if LWIP_IPV6 && LWIP_IPV6_MLD
+    struct ip6_mc *mc;
+    struct mld6_src *src, *head = NULL;
+    u32_t i;
+
+    mc = mcast_ip6_mc_find(ipmc, netif, ip_2_ip6(multi_addr), NULL);
+    if (mc == NULL) {
+      return ERR_VAL;
+    }
+    mc->num_src = 0;
+    for (i = 0; i < gf->gf_numsrc; i++) {
+      src = (struct mld6_src *)mem_malloc(sizeof(struct mld6_src));
+      if (src == NULL) {
+        mcast_ip6_mc_src_remove(head);
+        return ERR_MEM;
+      }
+      inet6_addr_to_ip6addr(&src->src_addr, &(((struct sockaddr_in6 *)&(gf->gf_slist[i]))->sin6_addr));
+      src->next = head;
+      head = src;
+      mc->num_src++;
+    }
+
+    mc->fmode = (u8_t)gf->gf_fmode;
+    mcast_ip6_mc_src_remove(mc->src);
+    mc->src = head;
+    IP6_MC_TRIGGER_CALL(netif, ip_2_ip6(multi_addr)); /* trigger a report */
+#endif /* LWIP_IPV6 && LWIP_IPV6_MLD */
+  }
+  return ERR_OK;
+}
+
 /** Remove multicast filter when pcb remove
  *
  * @param ipmc multicast filter control block
@@ -5199,6 +5292,50 @@ mcast_sock_block_unblock_source_group(struct ip_mc *ipmc, int optname, const str
   return err_to_errno(err);
 }
 
+/**
+ * setsockopt() MCAST_MSFILTER command
+ */
+int
+mcast_sock_set_groupfilter(struct ip_mc *ipmc, int optname, const struct group_filter *gf)
+{
+  struct netif *netif;
+  ip_addr_t multi_addr;
+
+  LWIP_UNUSED_ARG(optname);
+
+  if (gf->gf_numsrc > LWIP_MCAST_SRC_TBL_SIZE) {
+    return ERR_VAL;
+  }
+  if (gf->gf_group.ss_family == AF_INET) {
+    inet_addr_to_ip4addr(ip_2_ip4(&multi_addr), &(((struct sockaddr_in *)&(gf->gf_group))->sin_addr));
+    IP_SET_TYPE_VAL(multi_addr, IPADDR_TYPE_V4);
+    if (!ip4_addr_ismulticast(ip_2_ip4(&multi_addr))) {
+      return EADDRNOTAVAIL;
+    }
+
+  } else if (gf->gf_group.ss_family == AF_INET6) {
+    inet6_addr_to_ip6addr(ip_2_ip6(&multi_addr), &(((struct sockaddr_in6 *)&(gf->gf_group))->sin6_addr));
+    IP_SET_TYPE_VAL(multi_addr, IPADDR_TYPE_V6);
+    if (!ip6_addr_ismulticast(ip_2_ip6(&multi_addr))) {
+      return EADDRNOTAVAIL;
+    }
+
+  } else {
+    return EADDRNOTAVAIL;
+  }
+
+  if (gf->gf_interface) {
+    netif = netif_get_by_index((u8_t)gf->gf_interface);
+  } else {
+    netif = netif_default; /* To default network interface */
+  }
+  if (netif == NULL) {
+    return ENXIO;
+  }
+
+  return err_to_errno(mcast_set_groupfilter_netif(ipmc, netif, &multi_addr, gf));
+}
+
 /**
  * setsockopt() IPV6_JOIN_GROUP / IPV6_LEAVE_GROUP command
  */
-- 
2.25.1