diff --git a/omvs_main.c b/omvs_main.c index ee1d28a..bc65794 100644 --- a/omvs_main.c +++ b/omvs_main.c @@ -1,10 +1,12 @@ #include -#include +#include #include #include #include #include +#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);