diff --git a/Makefile b/Makefile index e1a95213d76d132fd3bb42e5c63659ba053a72c1..3daf1f9d26a78ed565c7f4cf99bf188b657bf3de 100644 --- a/Makefile +++ b/Makefile @@ -69,7 +69,6 @@ LIB_PATHS := -L lib PROG_LDFLAGS := $(LDFLAGS) -lieee1905 -lubus -lubox -lblobmsg_json PROG_LDFLAGS += -lcrypto -ljson-c -lwifi-6 -leasy -luci PROG_LDFLAGS += -lpthread -ldl -lrt -#PROG_LDFLAGS += -lpcap ifeq ($(CONFIG_IEEE1905_ALME_OVER_UBUS),y) CFLAGS += -DALME_OVER_UBUS diff --git a/src/platform/platform_os.c b/src/platform/platform_os.c index 1395387ff97c4e2ada201e753d8172312648d917..8f303a7899407e98afc08ba70cdc338d3968cbab 100644 --- a/src/platform/platform_os.c +++ b/src/platform/platform_os.c @@ -65,7 +65,6 @@ #include <stdlib.h> // free(), malloc(), ... #include <string.h> // memcpy(), memcmp(), ... #include <pthread.h> // threads and mutex functions -#include <pcap/pcap.h> // pcap_*() functions #include <errno.h> // errno #include <poll.h> // poll() #include <sys/inotify.h> // inotify_*() @@ -101,91 +100,10 @@ static uint8_t gs_qdepth; // *********** Packet capture stuff ******************************************** -// We use 'libpcap' to capture 1905 packets on all interfaces. -// It works like this: -// -// - When the PLATFORM API user calls "PLATFORM_REGISTER_QUEUE_EVENT()" with -// 'PLATFORM_QUEUE_EVENT_NEW_1905_PACKET', 'libpcap' is used to set the -// corresponding interface into monitor mode. -// -// - In addition, a new thread is created ('_pcapLoopThread()') which runs -// forever and, everytime a new packet is received on the corresponding -// interface, that thread calls '_pcapProcessPacket()' -// -// - '_pcapProcessPacket()' simply post the whole contents of the received -// packet to a queue so that the user can later obtain it with a call to -// "PLATFORM_QUEUE_READ()" - static pthread_mutex_t pcap_filters_mutex = PTHREAD_MUTEX_INITIALIZER; static pthread_cond_t pcap_filters_cond = PTHREAD_COND_INITIALIZER; static int pcap_filters_flag; -struct _pcapCaptureThreadData { - char interface_name[IFNAMSIZ]; - INT8U interface_mac_address[6]; - INT8U al_mac_address[6]; -}; - -static void _pcapProcessPacket(u_char *arg, const struct pcap_pkthdr *pkthdr, const u_char *packet) -{ - // This function is executed (on a per-interface dedicated thread) every - // time a new 1905 packet arrives - - struct _pcapCaptureThreadData *aux; - - INT8U message[MAX_PCAP_QUEUE_SIZE]; - INT16U message_len, mlen; - uint8_t *p; - - if (arg == NULL) { - // Invalid argument - ERROR("Invalid arguments"); - return; - } - - if (pkthdr->caplen != pkthdr->len) { - // This should never happen - ERROR("pcap caplen|%d| is not equal to len|%d|", - pkthdr->caplen, pkthdr->len); - return; - } - - aux = (struct _pcapCaptureThreadData *)arg; - - if (pkthdr->len > MAX_NETWORK_SEGMENT_SIZE) { - // This should never happen - ERROR("Captured packet too big"); - return; - } - // In order to build the message that will be inserted into the queue, we - // need to follow the "message format" defines in the documentation of - // function 'PLATFORM_REGISTER_QUEUE_EVENT()' - memset(&message, 0, MAX_PCAP_QUEUE_SIZE); - - message_len = (INT16U) (pkthdr->len + 6 + 3); - mlen = (INT16U) (6 + pkthdr->len); - - message[0] = PLATFORM_QUEUE_EVENT_NEW_1905_PACKET; - - p = &message[1]; - _I2B(&mlen, &p); - _InB(&aux->interface_mac_address[0], &p, 6); - _InB(packet, &p, pkthdr->len); - - // Now simply send the message. - DEBUG("*Pcap thread* Sending %d bytes to queue (0x%02x, 0x%02x, 0x%02x, ...)", - message_len, message[0], message[1], message[2]); - - if (sendMessageToAlQueue(message, message_len) == 0) { - ERROR("Error sending message to AL-Queue"); - return; - } - - return; -} - -#ifndef CONFIG_USE_LIBPCAP - static void dbg_dump(const char *h, unsigned char *buf, int len) { int i; @@ -202,7 +120,7 @@ static void dbg_dump(const char *h, unsigned char *buf, int len) fprintf(stderr, "\n"); } -int ieee1905_rx_loop(char *ifname) +int lldp_rx_loop(char *ifname) { int sk; int reuse = 1; @@ -227,7 +145,7 @@ int ieee1905_rx_loop(char *ifname) } ifindex = ifr.ifr_ifindex; - printf("ifindex = %d\n", ifindex); + printf("%s(): ifindex = %d\n", __func__, ifindex); ifr.ifr_addr.sa_family = AF_INET; if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) == -1) { @@ -238,7 +156,7 @@ int ieee1905_rx_loop(char *ifname) close(sockfd); - sk = socket(AF_PACKET, SOCK_RAW, htons(ETHERTYPE_1905)); + sk = socket(AF_PACKET, SOCK_RAW, htons(ETHERTYPE_LLDP)); if (sk < 0) { fprintf(stderr, "socket err\n"); return -1; @@ -250,7 +168,7 @@ int ieee1905_rx_loop(char *ifname) } sa.sll_family = PF_PACKET; - sa.sll_protocol = htons(ETHERTYPE_1905); + sa.sll_protocol = htons(ETHERTYPE_LLDP); sa.sll_halen = ETH_ALEN; sa.sll_ifindex = ifindex; if ((bind(sk, (struct sockaddr *)&sa, sizeof(struct sockaddr_ll))) != 0) { @@ -259,19 +177,6 @@ int ieee1905_rx_loop(char *ifname) return -1; } - /* subscribe to 1905 multicast */ - memset(&mr,0,sizeof(mr)); - mr.mr_ifindex = sa.sll_ifindex; - mr.mr_type = PACKET_MR_MULTICAST; - mr.mr_alen = ETH_ALEN; - memcpy(mr.mr_address, "\x01\x80\xC2\x00\x00\x13", ETH_ALEN); - if (setsockopt(sk, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) < 0) { - fprintf(stderr, "%s: setsockopt error (%s)\n", __func__, - strerror(errno)); - close(sk); - return -1; - } - /* subscribe to lldp multicast */ memset(&mr,0,sizeof(mr)); mr.mr_ifindex = sa.sll_ifindex; @@ -285,10 +190,10 @@ int ieee1905_rx_loop(char *ifname) return -1; } - // Signal the main thread so that it can continue its work pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; - pthread_cond_signal(&pcap_filters_cond); + pcap_filters_flag--; + if (pcap_filters_flag == 0) + pthread_cond_signal(&pcap_filters_cond); pthread_mutex_unlock(&pcap_filters_mutex); /* receive and process in a loop forever */ @@ -296,10 +201,9 @@ int ieee1905_rx_loop(char *ifname) uint8_t message[MAX_PCAP_QUEUE_SIZE]; uint16_t message_len, mlen; uint8_t *p; - struct sockaddr_ll address = {}; - socklen_t addrlen = sizeof(struct sockaddr_ll); memset(&message, 0, MAX_PCAP_QUEUE_SIZE); + fprintf(stderr, "%s: before receive\n", __func__); rlen = recvfrom(sk, &message[9], sizeof(message) - 9, 0, NULL, NULL); if (rlen == -1) { fprintf(stderr, "recv err\n"); @@ -329,153 +233,161 @@ out: close(sk); return 0; } -#endif /* !CONFIG_USE_LIBPCAP */ -static void pcap_thread_cleanup(void *arg) +static void *_lldpLoopThread(void *p) { - pcap_t *pdes = (pcap_t *) (arg); - // Cleanup starts from here - pcap_close(pdes); + char *ifname = (char *)p; + + + if (p == NULL) { + pthread_mutex_lock(&pcap_filters_mutex); + pcap_filters_flag--; + if (pcap_filters_flag == 0) + pthread_cond_signal(&pcap_filters_cond); + pthread_mutex_unlock(&pcap_filters_mutex); + + return NULL; + } + + lldp_rx_loop(ifname); + + ERROR("[PLATFORM] *lldp thread* Exiting thread (interface %s)", ifname); + return NULL; } -static void *_pcapLoopThread(void *p) +int ieee1905_rx_loop(char *ifname) { - // This function will loop forever in the "pcap_loop()" function, which - // generates a callback to "_pcapProcessPacket()" every time a new 1905 - // packet arrives + int sk; + int reuse = 1; + struct sockaddr_ll sa; + struct packet_mreq mr; + unsigned char buffer[MAX_NETWORK_SEGMENT_SIZE] = { 0 }; + ssize_t rlen = 0; + struct ifreq ifr; + int sockfd; + int ifindex; + uint8_t iface_macaddr[ETH_ALEN] = { 0 }; - char errbuf[PCAP_ERRBUF_SIZE]; - pcap_t *pcap_descriptor; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) { + return -1; + } - struct _pcapCaptureThreadData *aux, *aux_temp; - struct _pcapCaptureThreadData pcap_thread_data; + strncpy(ifr.ifr_name, ifname, 15); + ifr.ifr_name[15] = '\0'; + if (ioctl(sockfd, SIOCGIFINDEX, &ifr) == -1) { + close(sockfd); + return -1; + } - char pcap_filter_expression[255] = ""; - struct bpf_program fcode; + ifindex = ifr.ifr_ifindex; + printf("%s(): ifindex = %d\n", __func__, ifindex); - if (p == NULL) { - // 'p' must point to a valid 'struct _pcapCaptureThreadData' - // - ERROR("[PLATFORM] *Pcap thread* Invalid arguments in _pcapLoopThread()"); + ifr.ifr_addr.sa_family = AF_INET; + if (ioctl(sockfd, SIOCGIFHWADDR, &ifr) == -1) { + close(sockfd); + fprintf(stderr, "get hwaddr err\n"); + return -1; + } + memcpy(iface_macaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN); + close(sockfd); - pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; - pthread_cond_signal(&pcap_filters_cond); - pthread_mutex_unlock(&pcap_filters_mutex); - return NULL; + sk = socket(AF_PACKET, SOCK_RAW, htons(ETHERTYPE_1905)); + if (sk < 0) { + fprintf(stderr, "socket err\n"); + return -1; } - aux = &pcap_thread_data; - aux_temp = (struct _pcapCaptureThreadData *)p; - - strncpy(aux->interface_name, aux_temp->interface_name, IFNAMSIZ); - memcpy(aux->interface_mac_address, aux_temp->interface_mac_address, 6); - memcpy(aux->al_mac_address, aux_temp->al_mac_address, 6); - // Free the allocated aux_temp - FREE(aux_temp); - -#ifndef CONFIG_USE_LIBPCAP - ieee1905_rx_loop(aux->interface_name); -#else - // Open the interface in pcap. - // The third argument of 'pcap_open_live()' is set to '1' so that the - // interface is configured in 'monitor mode'. This is needed because we are - // not only interested in receiving packets addressed to the interface - // MAC address (or broadcast), but also those packets addressed to the - // "non-existent" (virtual?) AL MAC address of the AL entity (contained in - // 'aux->al_mac_address') - // - pcap_descriptor = pcap_open_live(aux->interface_name, MAX_NETWORK_SEGMENT_SIZE, 1, 512, errbuf); - if (pcap_descriptor == NULL) { - // Could not configure interface to capture 1905 packets - // - ERROR("[PLATFORM] *Pcap thread* Error opening interface %s", aux->interface_name); + if (setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) != 0) { + fprintf(stderr, "reuseaddr err\n"); + goto out; + } - pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; - pthread_cond_signal(&pcap_filters_cond); - pthread_mutex_unlock(&pcap_filters_mutex); + sa.sll_family = PF_PACKET; + sa.sll_protocol = htons(ETHERTYPE_1905); + sa.sll_halen = ETH_ALEN; + sa.sll_ifindex = ifindex; + if ((bind(sk, (struct sockaddr *)&sa, sizeof(struct sockaddr_ll))) != 0) { + fprintf(stderr, "bind() error\n"); + close(sk); + return -1; + } - return NULL; + /* subscribe to 1905 multicast */ + memset(&mr,0,sizeof(mr)); + mr.mr_ifindex = sa.sll_ifindex; + mr.mr_type = PACKET_MR_MULTICAST; + mr.mr_alen = ETH_ALEN; + memcpy(mr.mr_address, "\x01\x80\xC2\x00\x00\x13", ETH_ALEN); + if (setsockopt(sk, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) < 0) { + fprintf(stderr, "%s: setsockopt error (%s)\n", __func__, + strerror(errno)); + close(sk); + return -1; } - // If we started capturing now, we would receive *all* packets. This means - // *all* packets (even those that have nothing to do with 1905) would be - // copied from kernel space into user space (which is a very costly - // operation). - // - // To mitigate this effect (which takes place when enabling 'monitor mode' - // on an interface), 'pcap' let's us define "filtering rules" that take - // place in kernel space, thus limiting the amount of copies that need to - // be done to user space. - // - // Here we are going to configure a filter that only lets certain types of - // packets to get through. In particular those that meet any of these - // requirements: - // - // 1. Have ethertype == ETHERTYPE_1905 *and* are addressed to either the - // interface MAC address, the AL MAC address or the broadcast AL MAC - // address - // - // 2. Have ethertype == ETHERTYPE_LLDP *and* are addressed to the special - // LLDP nearest bridge multicast MAC address - snprintf(pcap_filter_expression, - sizeof(pcap_filter_expression), - "not ether src %02x:%02x:%02x:%02x:%02x:%02x and " - "not ether src %02x:%02x:%02x:%02x:%02x:%02x and " - "((ether proto 0x%04x and (ether dst %02x:%02x:%02x:%02x:%02x:%02x or ether dst %02x:%02x:%02x:%02x:%02x:%02x or ether dst %02x:%02x:%02x:%02x:%02x:%02x)) or " - "(ether proto 0x%04x and ether dst %02x:%02x:%02x:%02x:%02x:%02x))", - aux->interface_mac_address[0], aux->interface_mac_address[1], aux->interface_mac_address[2], aux->interface_mac_address[3], - aux->interface_mac_address[4], aux->interface_mac_address[5], aux->al_mac_address[0], aux->al_mac_address[1], aux->al_mac_address[2], - aux->al_mac_address[3], aux->al_mac_address[4], aux->al_mac_address[5], ETHERTYPE_1905, aux->interface_mac_address[0], - aux->interface_mac_address[1], aux->interface_mac_address[2], aux->interface_mac_address[3], aux->interface_mac_address[4], - aux->interface_mac_address[5], MCAST_1905_B0, MCAST_1905_B1, MCAST_1905_B2, MCAST_1905_B3, MCAST_1905_B4, MCAST_1905_B5, - aux->al_mac_address[0], aux->al_mac_address[1], aux->al_mac_address[2], aux->al_mac_address[3], aux->al_mac_address[4], aux->al_mac_address[5], - ETHERTYPE_LLDP, MCAST_LLDP_B0, MCAST_LLDP_B1, MCAST_LLDP_B2, MCAST_LLDP_B3, MCAST_LLDP_B4, MCAST_LLDP_B5); - - if (pcap_compile(pcap_descriptor, &fcode, pcap_filter_expression, 1, 0xffffff) < 0) { - ERROR("[PLATFORM] *Pcap thread* Cannot compile pcap filter (interface %s)", aux->interface_name); - pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; + pthread_mutex_lock(&pcap_filters_mutex); + pcap_filters_flag--; + if (pcap_filters_flag == 0) pthread_cond_signal(&pcap_filters_cond); - pthread_mutex_unlock(&pcap_filters_mutex); - pcap_close(pcap_descriptor); + pthread_mutex_unlock(&pcap_filters_mutex); - return NULL; - } + /* receive and process in a loop forever */ + do { + uint8_t message[MAX_PCAP_QUEUE_SIZE]; + uint16_t message_len, mlen; + uint8_t *p; - DEBUG("[PLATFORM] *Pcap thread* Installing pcap filter on interface %s: %s", aux->interface_name, pcap_filter_expression); - if (pcap_setfilter(pcap_descriptor, &fcode) < 0) { - ERROR("[PLATFORM] *Pcap thread* Cannot attach pcap filter to interface %s", aux->interface_name); + memset(&message, 0, MAX_PCAP_QUEUE_SIZE); + fprintf(stderr, "%s: before receive\n", __func__); + rlen = recvfrom(sk, &message[9], sizeof(message) - 9, 0, NULL, NULL); + if (rlen == -1) { + fprintf(stderr, "recv err\n"); + goto out; + } + //dbg_dump("", &message[9], rlen); + + message_len = rlen + 9; + mlen = (uint16_t)(6 + rlen); + message[0] = PLATFORM_QUEUE_EVENT_NEW_1905_PACKET; + p = &message[1]; + _I2B(&mlen, &p); + _InB(&iface_macaddr, &p, 6); + + DEBUG("Sending %d bytes to queue (0x%02x, 0x%02x, 0x%02x, ...)", + message_len, message[0], message[1], message[2]); + + + if (sendMessageToAlQueue(message, message_len) == 0) { + ERROR("Error sending message to AL-Queue"); + break; + } + } while (1); + +out: + close(sk); + return 0; +} + +static void *_i1905LoopThread(void *p) +{ + char *ifname = (char *)p; + + + if (p == NULL) { pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; - pthread_cond_signal(&pcap_filters_cond); + pcap_filters_flag--; + if (pcap_filters_flag == 0) + pthread_cond_signal(&pcap_filters_cond); pthread_mutex_unlock(&pcap_filters_mutex); - pcap_close(pcap_descriptor); return NULL; } - // Signal the main thread so that it can continue its work - // - pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 1; - pthread_cond_signal(&pcap_filters_cond); - pthread_mutex_unlock(&pcap_filters_mutex); - // Start the pcap loop. This goes on forever... - // Everytime a new packet (that meets the filtering rules defined above) - // arrives, the '_pcapProcessPacket()' callback is executed - // - pthread_cleanup_push(pcap_thread_cleanup, pcap_descriptor); - pcap_loop(pcap_descriptor, -1, _pcapProcessPacket, (u_char *) aux); - pthread_cleanup_pop(1); -#endif /* CONFIG_USE_LIBPCAP */ + ieee1905_rx_loop(ifname); - // This point should never be reached - // - ERROR("[PLATFORM] *Pcap thread* Exiting thread (interface %s)", aux->interface_name); + ERROR("[PLATFORM] *1905 thread* Exiting thread (interface %s)", ifname); return NULL; } @@ -726,41 +638,40 @@ INT8U PLATFORM_REGISTER_QUEUE_EVENT(INT8U event_type, void *data) switch (event_type) { case PLATFORM_QUEUE_EVENT_NEW_1905_PACKET: { - pthread_t thread; struct event1905Packet *p1; - struct _pcapCaptureThreadData *p2; + pthread_t i1905_thread; + pthread_t lldp_thread; + int num_thrs = 2; if (data == NULL) return 0; p1 = (struct event1905Packet *)data; - p2 = (struct _pcapCaptureThreadData *) malloc(sizeof(struct _pcapCaptureThreadData)); - if (p2 == NULL) - return 0; - - strncpy(p2->interface_name, p1->interface_name, IFNAMSIZ); - memcpy(p2->interface_mac_address, p1->interface_mac_address, 6); - memcpy(p2->al_mac_address, p1->al_mac_address, 6); pthread_mutex_lock(&pcap_filters_mutex); - pcap_filters_flag = 0; + pcap_filters_flag = num_thrs; pthread_mutex_unlock(&pcap_filters_mutex); - // ownership of p2 pass to thread - if (pthread_create(&thread, NULL, _pcapLoopThread, (void *)p2) != 0) { - FREE(p2); - ERROR("Failed to create sniffer thread(%s)!!!", p2->interface_name); + if (pthread_create(&i1905_thread, NULL, _i1905LoopThread, (void *)p1->interface_name) != 0) { + fprintf(stderr, "Failed to create sniffer i1905 thread(%s)!!!", p1->interface_name); + return 0; + } + + add_tid_to_list(i1905_thread); + + if (pthread_create(&lldp_thread, NULL, _lldpLoopThread, (void *)p1->interface_name) != 0) { + fprintf(stderr, "Failed to create lldp thread(%s)!!!", p1->interface_name); return 0; } - add_tid_to_list(thread); - // While it is not strictly needed, we will now wait until the PCAP - // thread registers the needed capture filters. + add_tid_to_list(lldp_thread); + pthread_mutex_lock(&pcap_filters_mutex); - while (pcap_filters_flag == 0) { + while (pcap_filters_flag > 0) { pthread_cond_wait(&pcap_filters_cond, &pcap_filters_mutex); } pthread_mutex_unlock(&pcap_filters_mutex); + fprintf(stderr, "Main thread resumed...\n"); break; }