[dhcpmon] Open different socket for dual tor to enable interface filtering

rules/dhcpmon.mk

@@ -4,6 +4,7 @@ SONIC_DHCPMON_VERSION = 1.0.0-0
 SONIC_DHCPMON_PKG_NAME = dhcpmon
 
 SONIC_DHCPMON = sonic-$(SONIC_DHCPMON_PKG_NAME)_$(SONIC_DHCPMON_VERSION)_$(CONFIGURED_ARCH).deb
+$(SONIC_DHCPMON)_DEPENDS = $(LIBSWSSCOMMON) $(LIBHIREDIS) $(LIBSWSSCOMMON_DEV) $(LIBHIREDIS_DEV)
 $(SONIC_DHCPMON)_SRC_PATH = $(SRC_PATH)/$(SONIC_DHCPMON_PKG_NAME)
 SONIC_DPKG_DEBS += $(SONIC_DHCPMON)
 

src/dhcpmon/Makefile

@@ -2,8 +2,9 @@ RM := rm -rf
 DHCPMON_TARGET := dhcpmon
 CP := cp
 MKDIR := mkdir
-CC := gcc
+CC := g++
 MV := mv
+PWD := $(shell pwd)
 
 # All of the sources participating in the build are defined here
 -include src/subdir.mk
@@ -23,7 +24,7 @@ all: sonic-dhcpmon
 # Tool invocations
 sonic-dhcpmon: $(OBJS) $(USER_OBJS)
 	@echo 'Building target: $@'
-	@echo 'Invoking: GCC C Linker'
+	@echo 'Invoking: G++ C Linker'
 	$(CC) -o "$(DHCPMON_TARGET)" $(OBJS) $(USER_OBJS) $(LIBS)
 	@echo 'Finished building target: $@'
 	@echo ' '

src/dhcpmon/objects.mk

@@ -1,4 +1,4 @@
 USER_OBJS :=
 
-LIBS := -levent -lexplain
+LIBS := -levent -lexplain -lswsscommon -pthread -lboost_thread -lboost_system -lhiredis
 

src/dhcpmon/src/dhcp_device.c → src/dhcpmon/src/dhcp_device.cpp

@@ -21,6 +21,8 @@
 #include <libexplain/ioctl.h>
 #include <linux/filter.h>
 #include <netpacket/packet.h>
+#include "subscriberstatetable.h"
+#include "select.h"
 
 #include "dhcp_device.h"
 
@@ -49,34 +51,42 @@
 #define OP_JSET     (BPF_JMP | BPF_JSET | BPF_K)    /** bpf jset */
 #define OP_LDXB     (BPF_LDX | BPF_B    | BPF_MSH)  /** bpf ldxb */
 
+std::shared_ptr<swss::DBConnector> mStateDbPtr = std::make_shared<swss::DBConnector> ("STATE_DB", 0);
+std::shared_ptr<swss::Table> mStateDbMuxTablePtr = std::make_shared<swss::Table> (
+            mStateDbPtr.get(), "HW_MUX_CABLE_TABLE"
+        );
+swss::DBConnector configDb("CONFIG_DB", 0);
+
 /** Berkeley Packet Filter program for "udp and (port 67 or port 68)".
  * This program is obtained using the following command tcpdump:
- * `tcpdump -dd "udp and (port 67 or port 68)"`
+ * `tcpdump -dd "inbound and udp and (port 67 or port 68)"`
  */
 static struct sock_filter dhcp_bpf_code[] = {
-    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x0000000c}, // (000) ldh      [12]
-    {.code = OP_JEQ,  .jt = 0,  .jf = 7,  .k = 0x000086dd}, // (001) jeq      #0x86dd          jt 2	jf 9
-    {.code = OP_LDB,  .jt = 0,  .jf = 0,  .k = 0x00000014}, // (002) ldb      [20]
-    {.code = OP_JEQ,  .jt = 0,  .jf = 18, .k = 0x00000011}, // (003) jeq      #0x11            jt 4	jf 22
-    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000036}, // (004) ldh      [54]
-    {.code = OP_JEQ,  .jt = 15, .jf = 0,  .k = 0x00000043}, // (005) jeq      #0x43            jt 21	jf 6
-    {.code = OP_JEQ,  .jt = 14, .jf = 0,  .k = 0x00000044}, // (006) jeq      #0x44            jt 21	jf 7
-    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000038}, // (007) ldh      [56]
-    {.code = OP_JEQ,  .jt = 12, .jf = 11, .k = 0x00000043}, // (008) jeq      #0x43            jt 21	jf 20
-    {.code = OP_JEQ,  .jt = 0,  .jf = 12, .k = 0x00000800}, // (009) jeq      #0x800           jt 10	jf 22
-    {.code = OP_LDB,  .jt = 0,  .jf = 0,  .k = 0x00000017}, // (010) ldb      [23]
-    {.code = OP_JEQ,  .jt = 0,  .jf = 10, .k = 0x00000011}, // (011) jeq      #0x11            jt 12	jf 22
-    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000014}, // (012) ldh      [20]
-    {.code = OP_JSET, .jt = 8,  .jf = 0,  .k = 0x00001fff}, // (013) jset     #0x1fff          jt 22	jf 14
-    {.code = OP_LDXB, .jt = 0,  .jf = 0,  .k = 0x0000000e}, // (014) ldxb     4*([14]&0xf)
-    {.code = OP_LDHI, .jt = 0,  .jf = 0,  .k = 0x0000000e}, // (015) ldh      [x + 14]
-    {.code = OP_JEQ,  .jt = 4,  .jf = 0,  .k = 0x00000043}, // (016) jeq      #0x43            jt 21	jf 17
-    {.code = OP_JEQ,  .jt = 3,  .jf = 0,  .k = 0x00000044}, // (017) jeq      #0x44            jt 21	jf 18
-    {.code = OP_LDHI, .jt = 0,  .jf = 0,  .k = 0x00000010}, // (018) ldh      [x + 16]
-    {.code = OP_JEQ,  .jt = 1,  .jf = 0,  .k = 0x00000043}, // (019) jeq      #0x43            jt 21	jf 20
-    {.code = OP_JEQ,  .jt = 0,  .jf = 1,  .k = 0x00000044}, // (020) jeq      #0x44            jt 21	jf 22
-    {.code = OP_RET,  .jt = 0,  .jf = 0,  .k = 0x00040000}, // (021) ret      #262144
-    {.code = OP_RET,  .jt = 0,  .jf = 0,  .k = 0x00000000}, // (022) ret      #0
+    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0xfffff004}, // (000) ldh      #fffff004
+    {.code = OP_JEQ, .jt = 22,  .jf = 0,  .k = 0x00000004}, // (001) jeq      #0x04            jt 22 jf 0
+    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x0000000c}, // (002) ldh      [12]
+    {.code = OP_JEQ,  .jt = 0,  .jf = 7,  .k = 0x000086dd}, // (003) jeq      #0x86dd          jt 2	jf 9
+    {.code = OP_LDB,  .jt = 0,  .jf = 0,  .k = 0x00000014}, // (004) ldb      [20]
+    {.code = OP_JEQ,  .jt = 0,  .jf = 18, .k = 0x00000011}, // (005) jeq      #0x11            jt 4	jf 22
+    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000036}, // (006) ldh      [54]
+    {.code = OP_JEQ,  .jt = 15, .jf = 0,  .k = 0x00000043}, // (007) jeq      #0x43            jt 21	jf 6
+    {.code = OP_JEQ,  .jt = 14, .jf = 0,  .k = 0x00000044}, // (008) jeq      #0x44            jt 21	jf 7
+    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000038}, // (009) ldh      [56]
+    {.code = OP_JEQ,  .jt = 12, .jf = 11, .k = 0x00000043}, // (010) jeq      #0x43            jt 21	jf 20
+    {.code = OP_JEQ,  .jt = 0,  .jf = 12, .k = 0x00000800}, // (011) jeq      #0x800           jt 10	jf 22
+    {.code = OP_LDB,  .jt = 0,  .jf = 0,  .k = 0x00000017}, // (012) ldb      [23]
+    {.code = OP_JEQ,  .jt = 0,  .jf = 10, .k = 0x00000011}, // (013) jeq      #0x11            jt 12	jf 22
+    {.code = OP_LDHA, .jt = 0,  .jf = 0,  .k = 0x00000014}, // (014) ldh      [20]
+    {.code = OP_JSET, .jt = 8,  .jf = 0,  .k = 0x00001fff}, // (015) jset     #0x1fff          jt 22	jf 14
+    {.code = OP_LDXB, .jt = 0,  .jf = 0,  .k = 0x0000000e}, // (016) ldxb     4*([14]&0xf)
+    {.code = OP_LDHI, .jt = 0,  .jf = 0,  .k = 0x0000000e}, // (017) ldh      [x + 14]
+    {.code = OP_JEQ,  .jt = 4,  .jf = 0,  .k = 0x00000043}, // (018) jeq      #0x43            jt 21	jf 17
+    {.code = OP_JEQ,  .jt = 3,  .jf = 0,  .k = 0x00000044}, // (019) jeq      #0x44            jt 21	jf 18
+    {.code = OP_LDHI, .jt = 0,  .jf = 0,  .k = 0x00000010}, // (020) ldh      [x + 16]
+    {.code = OP_JEQ,  .jt = 1,  .jf = 0,  .k = 0x00000043}, // (021) jeq      #0x43            jt 21	jf 20
+    {.code = OP_JEQ,  .jt = 0,  .jf = 1,  .k = 0x00000044}, // (022) jeq      #0x44            jt 21	jf 22
+    {.code = OP_RET,  .jt = 0,  .jf = 0,  .k = 0x00040000}, // (023) ret      #262144
+    {.code = OP_RET,  .jt = 0,  .jf = 0,  .k = 0x00000000}, // (024) ret      #0
 };
 
 /** Filter program socket struct */
@@ -176,7 +186,7 @@ static void read_callback(int fd, short event, void *arg)
         uint8_t *dhcphdr = context->buffer + DHCP_START_OFFSET;
         int dhcp_option_offset = DHCP_START_OFFSET + DHCP_OPTIONS_HEADER_SIZE;
 
-        if ((buffer_sz > UDP_START_OFFSET + sizeof(struct udphdr) + DHCP_OPTIONS_HEADER_SIZE) &&
+        if (((unsigned)buffer_sz > UDP_START_OFFSET + sizeof(struct udphdr) + DHCP_OPTIONS_HEADER_SIZE) &&
             (ntohs(udp->len) > DHCP_OPTIONS_HEADER_SIZE)) {
             int dhcp_sz = ntohs(udp->len) < buffer_sz - UDP_START_OFFSET - sizeof(struct udphdr) ?
                           ntohs(udp->len) : buffer_sz - UDP_START_OFFSET - sizeof(struct udphdr);
@@ -221,6 +231,87 @@ static void read_callback(int fd, short event, void *arg)
     }
 }
 
+/**
+ * @code read_callback_dual_tor(fd, event, arg);
+ *
+ * @brief callback for libevent which is called every time out in order to read queued packet capture when dual tor mode is enabled
+ *
+ * @param fd            socket to read from
+ * @param event         libevent triggered event
+ * @param arg           user provided argument for callback (interface context)
+ *
+ * @return none
+ */
+static void read_callback_dual_tor(int fd, short event, void *arg)
+{
+    dhcp_device_context_t *context = (dhcp_device_context_t*) arg;
+    ssize_t buffer_sz;
+    struct sockaddr_ll sll;
+    socklen_t slen = sizeof sll;
+
+    while ((event == EV_READ) &&
+           ((buffer_sz = recvfrom(fd, context->buffer, context->snaplen, MSG_DONTWAIT, (struct sockaddr *)&sll, &slen)) > 0)) 
+    {
+        std::string member_table = std::string("VLAN_MEMBER|") + context->intf + "|";
+        char interfaceName[IF_NAMESIZE];
+        char *interface = if_indextoname(sll.sll_ifindex, interfaceName);
+        std::string state;
+        std::string intf(interface);
+        mStateDbMuxTablePtr->hget(intf, "state", state);
+        if (state != "standby" && configDb.exists(member_table.append(interface))) {
+            struct ether_header *ethhdr = (struct ether_header*) context->buffer;
+            struct ip *iphdr = (struct ip*) (context->buffer + IP_START_OFFSET);
+            struct udphdr *udp = (struct udphdr*) (context->buffer + UDP_START_OFFSET);
+            uint8_t *dhcphdr = context->buffer + DHCP_START_OFFSET;
+            int dhcp_option_offset = DHCP_START_OFFSET + DHCP_OPTIONS_HEADER_SIZE;
+
+            if (((unsigned)buffer_sz > UDP_START_OFFSET + sizeof(struct udphdr) + DHCP_OPTIONS_HEADER_SIZE) &&
+                (ntohs(udp->len) > DHCP_OPTIONS_HEADER_SIZE)) 
+            {
+                int dhcp_sz = ntohs(udp->len) < buffer_sz - UDP_START_OFFSET - sizeof(struct udphdr) ?
+                            ntohs(udp->len) : buffer_sz - UDP_START_OFFSET - sizeof(struct udphdr);
+                int dhcp_option_sz = dhcp_sz - DHCP_OPTIONS_HEADER_SIZE;
+                const u_char *dhcp_option = context->buffer + dhcp_option_offset;
+                dhcp_packet_direction_t dir = (ethhdr->ether_shost[0] == context->mac[0] &&
+                                            ethhdr->ether_shost[1] == context->mac[1] &&
+                                            ethhdr->ether_shost[2] == context->mac[2] &&
+                                            ethhdr->ether_shost[3] == context->mac[3] &&
+                                            ethhdr->ether_shost[4] == context->mac[4] &&
+                                            ethhdr->ether_shost[5] == context->mac[5]) ?
+                                            DHCP_TX : DHCP_RX;
+                int offset = 0;
+                int stop_dhcp_processing = 0;
+                while ((offset < (dhcp_option_sz + 1)) && dhcp_option[offset] != 255) {
+                    switch (dhcp_option[offset])
+                    {
+                    case 53:
+                        if (offset < (dhcp_option_sz + 2)) {
+                            handle_dhcp_option_53(context, &dhcp_option[offset], dir, iphdr, dhcphdr);
+                        }
+                        stop_dhcp_processing = 1; // break while loop since we are only interested in Option 53
+                        break;
+                    default:
+                        break;
+                    }
+
+                    if (stop_dhcp_processing == 1) {
+                        break;
+                    }
+
+                    if (dhcp_option[offset] == 0) { // DHCP Option Padding
+                        offset++;
+                    } else {
+                        offset += dhcp_option[offset + 1] + 2;
+                    }
+                }
+            } else {
+                syslog(LOG_WARNING, "read_callback(%s): read length (%ld) is too small to capture DHCP options",
+                    context->intf, buffer_sz);
+            }
+        }
+    }
+}
+
 /**
  * @code dhcp_device_is_dhcp_inactive(counters);
  *
@@ -412,7 +503,7 @@ static int init_socket(dhcp_device_context_t *context, const char *intf)
 
         struct sockaddr_ll addr;
         memset(&addr, 0, sizeof(addr));
-        addr.sll_ifindex = if_nametoindex(intf);
+        addr.sll_ifindex = 0; // any interface
         addr.sll_family = AF_PACKET;
         addr.sll_protocol = htons(ETH_P_ALL);
         if (bind(context->sock, (struct sockaddr *) &addr, sizeof(addr))) {
@@ -553,6 +644,7 @@ int dhcp_device_start_capture(dhcp_device_context_t *context,
                               in_addr_t giaddr_ip)
 {
     int rv = -1;
+    struct event *ev;
 
     do {
         if (context == NULL) {
@@ -579,7 +671,11 @@ int dhcp_device_start_capture(dhcp_device_context_t *context,
             break;
         }
 
-        struct event *ev = event_new(base, context->sock, EV_READ | EV_PERSIST, read_callback, context);
+        if (dual_tor_sock)
+            ev = event_new(base, context->sock, EV_READ | EV_PERSIST, read_callback_dual_tor, context);
+        else
+            ev = event_new(base, context->sock, EV_READ | EV_PERSIST, read_callback, context);
+
         if (ev == NULL) {
             syslog(LOG_ALERT, "event_new: failed to allocate memory for libevent event '%s'\n", strerror(errno));
             break;

src/dhcpmon/src/dhcp_device.h

@@ -16,6 +16,7 @@
 #include <event2/bufferevent.h>
 #include <event2/buffer.h>
 
+extern bool dual_tor_sock;
 
 /**
  * DHCP message types

src/dhcpmon/src/dhcp_devman.c → src/dhcpmon/src/dhcp_devman.cpp

@@ -110,7 +110,7 @@ void dhcp_devman_shutdown()
 int dhcp_devman_add_intf(const char *name, char intf_type)
 {
     int rv = -1;
-    struct intf *dev = malloc(sizeof(struct intf));
+    struct intf *dev = (struct intf*) malloc(sizeof(struct intf));
 
     if (dev != NULL) {
         dev->name = name;

src/dhcpmon/src/main.c → src/dhcpmon/src/main.cpp

@@ -16,9 +16,12 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <unistd.h>
+#include "subscriberstatetable.h"
+#include "select.h"
 
 #include "dhcp_mon.h"
 #include "dhcp_devman.h"
+#include "dhcp_device.h"
 
 /** dhcpmon_default_snaplen: default snap length of packet being captured */
 static const size_t dhcpmon_default_snaplen = 65535;
@@ -29,6 +32,8 @@ static const uint32_t dhcpmon_default_health_check_window = 18;
  *  with DHCP relay */
 static const uint32_t dhcpmon_default_unhealthy_max_count = 10;
 
+bool dual_tor_sock = false;
+
 /**
  * @code usage(prog);
  *
@@ -134,6 +139,7 @@ int main(int argc, char **argv)
             i += 2;
             break;
         case 'u':
+            dual_tor_sock = true;
             if (dhcp_devman_setup_dual_tor_mode(argv[i + 1]) != 0) {
                 usage(basename(argv[0]));
             }

src/dhcpmon/src/subdir.mk

@@ -1,11 +1,11 @@
 # Add inputs and outputs from these tool invocations to the build variables 
-CC := gcc
+CC := g++
 
 C_SRCS += \
-../src/dhcp_device.c \
-../src/dhcp_devman.c \
-../src/dhcp_mon.c \
-../src/main.c 
+../src/dhcp_device.cpp \
+../src/dhcp_devman.cpp \
+../src/dhcp_mon.cpp \
+../src/main.cpp 
 
 OBJS += \
 ./src/dhcp_device.o \
@@ -21,9 +21,9 @@ C_DEPS += \
 
 
 # Each subdirectory must supply rules for building sources it contributes
-src/%.o: ../src/%.c
+src/%.o: src/%.cpp
 	@echo 'Building file: $<'
 	@echo 'Invoking: GCC C Compiler'
-	$(CC) -O3 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@)" -o "$@" "$<"
+	$(CC) -O3 -g3 -Wall -I$(PWD)/../sonic-swss-common/common -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@)" -o "$@" "$<"
 	@echo 'Finished building: $<'
 	@echo ' '