From a4a51288e841fc066cc6ab079c63eb1f113fa2ab Mon Sep 17 00:00:00 2001
From: wanfeng <[email protected]>
Date: Tue, 16 Apr 2024 15:51:09 +0800
Subject: [PATCH] add MCAST_BLOCK_SOURCE to setsockopt for mldv2

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

diff --git a/src/api/sockets.c b/src/api/sockets.c
index 50fa335..6a3ec91 100644
--- a/src/api/sockets.c
+++ b/src/api/sockets.c
@@ -3993,6 +3993,29 @@ lwip_setsockopt_impl(int s, int level, int optname, const void *optval, socklen_
           if (NETCONNTYPE_GROUPV6(netconn_type(sock->conn)) == NETCONN_RAW_IPV6) {
             err = mcast_sock_join_leave_source_group(s, &sock->conn->pcb.raw->ipmc, optname, (const struct group_source_req *)optval);
           } else
+#endif /* LWIP_RAW */
+          {
+            done_socket(sock);
+            return ENOPROTOOPT;
+          }
+          break;
+        case MCAST_BLOCK_SOURCE:
+        case MCAST_UNBLOCK_SOURCE:
+          LWIP_SOCKOPT_CHECK_OPTLEN_CONN_PCB(sock, optlen, struct group_source_req);
+          if ((((const struct group_source_req *)optval)->gsr_group.ss_family != AF_INET6) ||
+              (((const struct group_source_req *)optval)->gsr_source.ss_family != AF_INET6)) {
+            done_socket(sock);
+            return EINVAL;
+          }
+#if LWIP_UDP
+          if (NETCONNTYPE_GROUPV6(netconn_type(sock->conn)) == NETCONN_UDP_IPV6) {
+            err = mcast_sock_block_unblock_source_group(&sock->conn->pcb.udp->ipmc, optname, (const struct group_source_req *)optval);
+          } else
+#endif /* LWIP_UDP */
+#if LWIP_RAW
+          if (NETCONNTYPE_GROUPV6(netconn_type(sock->conn)) == NETCONN_RAW_IPV6) {
+            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);
@@ -4963,6 +4986,57 @@ out:
   return err_to_errno(err);
 }
 
+/**
+ * setsockopt() MCAST_BLOCK_SOURCE / MCAST_UNBLOCK_SOURCE command
+ */
+int
+mcast_sock_block_unblock_source_group(struct ip_mc *ipmc, int optname, const struct group_source_req *gsr)
+{
+  struct netif *netif;
+  ip_addr_t multi_addr;
+  ip_addr_t blk_addr;
+  err_t err;
+
+  if (gsr->gsr_group.ss_family == AF_INET) {
+    inet_addr_to_ip4addr(ip_2_ip4(&multi_addr), &(((struct sockaddr_in *)&(gsr->gsr_group))->sin_addr));
+    inet_addr_to_ip4addr(ip_2_ip4(&blk_addr), &(((struct sockaddr_in *)&(gsr->gsr_source))->sin_addr));
+    IP_SET_TYPE_VAL(multi_addr, IPADDR_TYPE_V4);
+    IP_SET_TYPE_VAL(blk_addr, IPADDR_TYPE_V4);
+    if (!ip4_addr_ismulticast(ip_2_ip4(&multi_addr)) || ip4_addr_isany(ip_2_ip4(&blk_addr))) {
+      return EADDRNOTAVAIL;
+    }
+
+  } else if (gsr->gsr_group.ss_family == AF_INET6) {
+    inet6_addr_to_ip6addr(ip_2_ip6(&multi_addr), &(((struct sockaddr_in6 *)&(gsr->gsr_group))->sin6_addr));
+    inet6_addr_to_ip6addr(ip_2_ip6(&blk_addr), &(((struct sockaddr_in6 *)&(gsr->gsr_source))->sin6_addr));
+    IP_SET_TYPE_VAL(multi_addr, IPADDR_TYPE_V6);
+    IP_SET_TYPE_VAL(blk_addr, IPADDR_TYPE_V6);
+    if (!ip6_addr_ismulticast(ip_2_ip6(&multi_addr)) || ip6_addr_isany(ip_2_ip6(&blk_addr))) {
+      return EADDRNOTAVAIL;
+    }
+
+  } else {
+    return EADDRNOTAVAIL;
+  }
+
+  if (gsr->gsr_interface) {
+    netif = netif_get_by_index((u8_t)gsr->gsr_interface);
+  } else {
+    netif = netif_default; /* To default network interface */
+  }
+  if (netif == NULL) {
+    return ENXIO;
+  }
+
+  if (optname == MCAST_BLOCK_SOURCE) {
+    err = mcast_block_netif(ipmc, netif, &multi_addr, &blk_addr);
+  } else {
+    err = mcast_unblock_netif(ipmc, netif, &multi_addr, &blk_addr);
+  }
+
+  return err_to_errno(err);
+}
+
 /**
  * setsockopt() IPV6_JOIN_GROUP / IPV6_LEAVE_GROUP command
  */
-- 
2.25.1