Find multicast UDP port list.

omvs_main.c

@@ -1,10 +1,12 @@
 #include <string.h>
-#include <pcap.h>
+#include <pcap/pcap.h>
 #include <glib.h>
 #include <glib/gstdio.h>
 #include <gio/gio.h>
 #include <gst/gst.h>
 
+#define _OMVS_IPPROTO_UDP 17
+
 static gint _omvs_net_dev_idx;
 static gboolean _omvs_list_devs;
 static gchar *_omvs_outdir = "omvs_out";
@@ -36,14 +38,43 @@ static GOptionEntry _omvs_entries[] =
   { NULL }
 };
 
-typedef struct _OMVSPort {
-  gint port;
-  gint type;
-} OMVSPort;
+typedef struct _OMVSEthHdr {
+  guint8 ether_dhost[6];
+  guint8 ether_shost[6];
+  guint16 ether_type;
+} __attribute__((packed)) OMVSEthHdr;
+
+typedef struct _OMVSIPHdr {
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+  guint ihl:4;
+  guint version:4;
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+  guint version:4;
+  guint ihl:4;
+#else
+#error "endian error"
+#endif
+  guint8 tos;
+  guint16 tot_len;
+  guint16 id;
+  guint16 frag_off;
+  guint8 ttl;
+  guint8 protocol;
+  guint16 check;
+  guint32 saddr;
+  guint32 daddr;
+} OMVSIPHdr;
+
+typedef struct _OMVSUDPHdr {
+  guint16 uh_sport;
+  guint16 uh_dport;
+  guint16 uh_ulen;
+  guint16 uh_sum;
+} OMVSUDPHdr;
 
 typedef struct _OMVSIPAddr {
   guint32 addr;
-  OMVSPort *ports;
+  guint16 *ports;
   gint num_ports;
 } OMVSIPAddr;
 
@@ -116,6 +147,10 @@ static gpointer _omvs_start_scan_job(gpointer data) {
   GSocketAddress *socket_address;
   GInetAddress *inet_address;
   gint i;
+  pcap_t *pcap;
+  gchar errbuf[PCAP_ERRBUF_SIZE];
+  const guint8 *packet;
+  struct pcap_pkthdr *header;
 
   scanner = (OMVSScanner *)data;
 
@@ -136,15 +171,35 @@ static gpointer _omvs_start_scan_job(gpointer data) {
     goto finish_return2;
   }
 
+  pcap = pcap_open_live(_omvs_net_dev_names[_omvs_net_dev_idx],
+    sizeof(OMVSEthHdr) + sizeof(OMVSIPHdr) + sizeof(OMVSUDPHdr),
+    TRUE, 500, errbuf);
+  if (pcap == NULL) {
+    g_printerr("[%p] can't open pcap: %s\n", (void *)g_thread_self(), errbuf);
+    goto finish_return2;
+  }
+  if (pcap_datalink(pcap) != DLT_EN10MB) {
+    g_printerr("[%p] %s is not ethernet\n", (void *)g_thread_self(),
+      _omvs_net_dev_names[_omvs_net_dev_idx]);
+    goto finish_return3;
+  }
+
   for (i = 0; i < scanner->num_ipaddrs; i++) {
+    OMVSIPAddr *o_ipaddr;
     guint8 addr[4];
+    guint32 naddr;
     gchar *addr_str;
     GInetAddress *group_address;
+    gint64 time1, time2;
+    gint j;
 
-    addr[0] = (guint8)((scanner->ipaddrs[i].addr >> 24) & 0xff);
-    addr[1] = (guint8)((scanner->ipaddrs[i].addr >> 16) & 0xff);
-    addr[2] = (guint8)((scanner->ipaddrs[i].addr >> 8) & 0xff);
-    addr[3] = (guint8)((scanner->ipaddrs[i].addr >> 0) & 0xff);
+    o_ipaddr = &scanner->ipaddrs[i];
+
+    naddr = g_htonl(o_ipaddr->addr);
+    addr[0] = (guint8)((o_ipaddr->addr >> 24) & 0xff);
+    addr[1] = (guint8)((o_ipaddr->addr >> 16) & 0xff);
+    addr[2] = (guint8)((o_ipaddr->addr >> 8) & 0xff);
+    addr[3] = (guint8)((o_ipaddr->addr >> 0) & 0xff);
 
     addr_str =
       g_strdup_printf("%d.%d.%d.%d", addr[0], addr[1], addr[2], addr[3]);
@@ -159,7 +214,86 @@ static gpointer _omvs_start_scan_job(gpointer data) {
         addr_str, error->message);
     }
 
-    g_usleep(_omvs_timeout * 1000);
+    time1 = g_get_monotonic_time();
+    for (;;) {
+      gint ret;
+      guint caplen;
+      OMVSIPHdr *ip;
+      OMVSUDPHdr *udp;
+      guint ip_hdr_len;
+      gint udp_port;
+      gboolean is_udp_port_found;
+
+      ret = pcap_next_ex(pcap, &header, &packet);
+
+      if (ret < 0) {
+        break;
+      }
+
+      time2 = g_get_monotonic_time();
+      if (time2 - time1 > (gint64)((gint64)_omvs_timeout * 1000)) {
+        break;
+      }
+
+      if (ret == 0) {
+        continue;
+      }
+
+      caplen = header->caplen;
+
+      if (caplen < sizeof(OMVSEthHdr)) {
+        continue;
+      }
+
+      packet += sizeof(OMVSEthHdr);
+      caplen -= sizeof(OMVSEthHdr);
+      if (caplen < sizeof(OMVSIPHdr)) {
+        continue;
+      }
+
+      ip = (OMVSIPHdr *)packet;
+      ip_hdr_len = ip->ihl * 4;
+      if (caplen < ip_hdr_len) {
+        continue;
+      }
+
+      if (ip->protocol != _OMVS_IPPROTO_UDP) {
+        continue;
+      }
+
+      if (naddr != ip->daddr) {
+        continue;
+      }
+
+      packet += ip_hdr_len;
+      caplen -= ip_hdr_len;
+
+      if (caplen < sizeof(OMVSUDPHdr)) {
+        continue;
+      }
+
+      udp = (OMVSUDPHdr *)packet;
+      udp_port = g_ntohs(udp->uh_dport);
+
+      is_udp_port_found = FALSE;
+      for (j = 0; j < o_ipaddr->num_ports; j++) {
+        if (o_ipaddr->ports[j] == udp_port) {
+          is_udp_port_found = TRUE;
+          break;
+        }
+      }
+
+      if (!is_udp_port_found) {
+        o_ipaddr->ports = g_realloc(o_ipaddr->ports,
+          (o_ipaddr->num_ports + 1) * sizeof(guint16));
+        o_ipaddr->ports[o_ipaddr->num_ports] = udp_port;
+        o_ipaddr->num_ports++;
+        if (_omvs_verbose) {
+          g_print("[%p] found udp://%s:%d\n", (void *)g_thread_self(),
+            addr_str, udp_port);
+        }
+      }
+    }
 
     if (_omvs_verbose) {
       g_print("[%p] finish scanning %s\n", (void *)g_thread_self(), addr_str);
@@ -175,6 +309,8 @@ static gpointer _omvs_start_scan_job(gpointer data) {
     g_usleep(_omvs_sleep * 1000);
   }
 
+finish_return3:
+  pcap_close(pcap);
 finish_return2:
   g_object_unref(socket_address);
   g_object_unref(socket);
@@ -256,7 +392,7 @@ int main(int argc, char *argv[]) {
     goto finish_return;
   }
 
-  if (argc < 2) {
+  if (argc < 2 || _omvs_net_dev_idx >= _omvs_num_net_devs) {
     gchar *help_msg;
     help_msg = g_option_context_get_help(context, TRUE, NULL);
     g_print("%s", help_msg);