--- /dev/null
+/* kontaxis 2021-04-03 */
+
+#include <stdio.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+#include <pcap.h>
+
+#if !__DEBUG__
+ #define NDEBUG
+#endif
+#include <assert.h>
+
+#include "mdns.h"
+#include "pcap_aux.h"
+
+#define likely(x) __builtin_expect((x),1)
+#define unlikely(x) __builtin_expect((x),0)
+
+struct config {
+ uint8_t flags;
+#define OPT_PROMISCUOUS (0x1 << 0)
+#define OPT_VERBOSE (0x1 << 1)
+ struct pcap_dev *dev;
+ struct pcap_dev *cur;
+ char *bpf_txt;
+};
+
+#define PROMISCUOUS(flags) ((flags) & OPT_PROMISCUOUS)
+#define VERBOSE(flags) ((flags) & OPT_VERBOSE)
+
+#define BPF_DEFAULT \
+ "ip and dst host 224.0.0.251 and udp and dst port 5353"
+
+static uint8_t break_pcap_loop = 0;
+
+static size_t parse_dns_name(const uint8_t *buffer, size_t buffer_size,
+ uint8_t **name)
+{
+ size_t consumed_bytes;
+
+ uint8_t *p;
+ size_t available_bytes;
+
+ uint8_t label_length;
+
+ for (p = (uint8_t *)buffer, available_bytes = buffer_size;
+ available_bytes;) {
+ label_length = *p;
+ available_bytes -= sizeof(label_length);
+
+ if (label_length == 0) {
+ break; /* Success */
+ }
+
+ if ((label_length & 0xC0) == 0xC0 && available_bytes > 0) {
+ label_length = 0;
+ available_bytes--;
+ break; /* Success */
+ }
+
+ if (available_bytes < label_length) {
+ break; /* Failure */
+ }
+ available_bytes -= label_length;
+ p = (uint8_t *)p + label_length + sizeof(label_length);
+ }
+
+ if (label_length != 0) {
+ available_bytes = buffer_size;
+ }
+
+ if (!(consumed_bytes = buffer_size - available_bytes)) {
+ return 0;
+ }
+
+ if (!(*name = malloc(consumed_bytes))) {
+ return consumed_bytes;
+ }
+ memset(*name, 0, consumed_bytes);
+ memcpy(*name, buffer, consumed_bytes);
+
+ for (p = *name, available_bytes = consumed_bytes; available_bytes;) {
+ label_length = *p;
+ available_bytes -= sizeof(label_length);
+
+ if (label_length == 0) {
+ break; /* Success */
+ }
+
+ if ((label_length & 0xC0) == 0xC0) {
+ *p = '.';
+ *((uint8_t *)p + 1) = 0;
+ break; /* Success */
+ }
+
+ *p = '.';
+
+ available_bytes -= label_length;
+ p = (uint8_t *)p + label_length + sizeof(label_length);
+ }
+
+ return consumed_bytes;
+}
+
+static size_t parse_dns_rr(const uint8_t *buffer, size_t buffer_size,
+ struct dns_rr *rr)
+{
+ size_t available_bytes;
+ size_t consumed_bytes;
+
+ available_bytes = buffer_size;
+ consumed_bytes = parse_dns_name(buffer, available_bytes, &rr->name);
+ if (!consumed_bytes || available_bytes < consumed_bytes) {
+ return 0;
+ }
+ available_bytes -= consumed_bytes;
+ buffer = (const uint8_t *)buffer + consumed_bytes;
+
+ if (available_bytes < sizeof(rr->type)) {
+ return 0;
+ }
+ rr->type = *(const uint16_t *)buffer;
+ available_bytes -= sizeof(rr->type);
+ buffer = (const uint8_t *)buffer + sizeof(rr->type);
+
+ if (available_bytes < sizeof(rr->class)) {
+ return 0;
+ }
+ rr->class = *(const uint16_t *)buffer;
+ available_bytes -= sizeof(rr->class);
+ buffer = (const uint8_t *)buffer + sizeof(rr->class);
+
+ if (available_bytes < sizeof(rr->ttl)) {
+ return 0;
+ }
+ rr->ttl = *(const uint32_t *)buffer;
+ available_bytes -= sizeof(rr->ttl);
+ buffer = (const uint8_t *)buffer + sizeof(rr->ttl);
+
+ if (available_bytes < sizeof(rr->rdlength)) {
+ return 0;
+ }
+ rr->rdlength = *(const uint16_t *)buffer;
+ available_bytes -= sizeof(rr->rdlength);
+ buffer = (const uint8_t *)buffer + sizeof(rr->rdlength);
+
+ if (available_bytes < ntohs(rr->rdlength)) {
+ return 0;
+ }
+ if (!(rr->rdata = malloc(ntohs(rr->rdlength)))) {
+ return 0;
+ }
+ memcpy(rr->rdata, buffer, ntohs(rr->rdlength));
+ available_bytes -= ntohs(rr->rdlength);
+ buffer = (const uint8_t *)buffer + ntohs(rr->rdlength);
+
+ return buffer_size - available_bytes;
+}
+
+static size_t parse_dns_question(const uint8_t *buffer, size_t buffer_size,
+ struct dns_q *q)
+{
+ size_t available_bytes;
+ size_t consumed_bytes;
+
+ available_bytes = buffer_size;
+ consumed_bytes = parse_dns_name(buffer, available_bytes, &q->qname);
+ if (!consumed_bytes || available_bytes < consumed_bytes) {
+ return 0;
+ }
+ available_bytes -= consumed_bytes;
+ buffer = (const uint8_t *)buffer + consumed_bytes;
+
+ if (available_bytes < sizeof(q->qtype)) {
+ if (q->qname) {
+ free(q->qname);
+ q->qname = NULL;
+ }
+ return 0;
+ }
+ q->qtype = *(const uint16_t *)buffer;
+ available_bytes -= sizeof(q->qtype);
+ buffer = (const uint8_t *)buffer + sizeof(q->qtype);
+
+ if (available_bytes < sizeof(q->qclass)) {
+ if (q->qname) {
+ free(q->qname);
+ q->qname = NULL;
+ }
+ return 0;
+ }
+ q->qclass = *(const uint16_t *)buffer;
+ available_bytes -= sizeof(q->qclass);
+ buffer = (const uint8_t *)buffer + sizeof(q->qclass);
+
+ return buffer_size - available_bytes;
+}
+
+#define ASSERT_ENOUGH_BYTES_FOR(MIN_BYTES) \
+{ \
+ if (unlikely(available_bytes < (MIN_BYTES))) { \
+ fprintf(stderr, "%s:%d available bytes %u < %u\n", \
+ __func__, __LINE__, available_bytes, (MIN_BYTES)); \
+ return; \
+ } \
+}
+
+#define ADVANCE_PACKET_PTR(CNT) \
+{ \
+ if (unlikely(available_bytes < (CNT))) { \
+ fprintf(stderr, "%s:%d available bytes %u < %u\n", \
+ __func__, __LINE__, available_bytes, (CNT)); \
+ } \
+ available_bytes -= (CNT); \
+ \
+ if (unlikely((size_t)packet_ptr > SIZE_MAX - (CNT))) { \
+ fprintf(stderr, "%s:%d 0x%08x + %u > 0x%08x\n", \
+ __func__, __LINE__, (size_t)packet_ptr, (CNT), SIZE_MAX); \
+ return; \
+ } \
+ packet_ptr = (const uint8_t *)packet_ptr + (CNT); \
+}
+
+static char pkt_ts_txt[16];
+static const char *print_pkt_ts(const struct timeval *ts)
+{
+ size_t r;
+ struct tm *ts_sec = localtime(&ts->tv_sec);
+ if (!(r = strftime(pkt_ts_txt, sizeof(pkt_ts_txt), "%H:%M:%S",
+ ts_sec))) {
+ return "_TS_";
+ }
+ if (snprintf(pkt_ts_txt + r, sizeof(pkt_ts_txt) - r, ".%06ld",
+ ts->tv_usec) == -1) {
+ return "_TS_";
+ }
+ return pkt_ts_txt;
+}
+
+static char pkt_hdr_ether[40];
+static const char *print_pkt_hdr_ether(const struct ether_header *ether)
+{
+ if (snprintf(pkt_hdr_ether, sizeof(pkt_hdr_ether),
+ "(%02x:%02x:%02x:%02x:%02x:%02x > "
+ "%02x:%02x:%02x:%02x:%02x:%02x)",
+ *(ether->ether_shost + 0),
+ *(ether->ether_shost + 1),
+ *(ether->ether_shost + 2),
+ *(ether->ether_shost + 3),
+ *(ether->ether_shost + 4),
+ *(ether->ether_shost + 5),
+ *(ether->ether_dhost + 0),
+ *(ether->ether_dhost + 1),
+ *(ether->ether_dhost + 2),
+ *(ether->ether_dhost + 3),
+ *(ether->ether_dhost + 4),
+ *(ether->ether_dhost + 5)) == -1) {
+ return "_ETH_";
+ }
+ return pkt_hdr_ether;
+}
+
+static char pkt_hdr_ip[34];
+static const char *print_pkt_hdr_ip(struct my_iphdr *ip)
+{
+ if (snprintf(pkt_hdr_ip, sizeof(pkt_hdr_ip),
+ "%u.%u.%u.%u > %u.%u.%u.%u",
+ *((uint8_t *)&ip->saddr + 0),
+ *((uint8_t *)&ip->saddr + 1),
+ *((uint8_t *)&ip->saddr + 2),
+ *((uint8_t *)&ip->saddr + 3),
+ *((uint8_t *)&ip->daddr + 0),
+ *((uint8_t *)&ip->daddr + 1),
+ *((uint8_t *)&ip->daddr + 2),
+ *((uint8_t *)&ip->daddr + 3)) == -1) {
+ return "_IP_";
+ }
+ return pkt_hdr_ip;
+}
+
+static char pkt_hdr_dns[101];
+static const char *print_pkt_hdr_dns(struct dnshdr *dns)
+{
+ if (snprintf(pkt_hdr_dns, sizeof(pkt_hdr_dns),
+ "ID 0x%04x Flags 0x%04x QD %u AN %u NS %u AR %u "
+ "(%s %.5s %s%s%s%s%s%s%s%s%.8s)",
+ ntohs(dns->id),
+ ntohs(dns->flags),
+ ntohs(dns->qdcount),
+ ntohs(dns->ancount),
+ ntohs(dns->nscount),
+ ntohs(dns->arcount),
+ DNS_QR(dns) ? "R" : "Q",
+ DNS_OPCODE_S(DNS_OP(dns)),
+ DNS_AA(dns) ? "AA " : "",
+ DNS_TC(dns) ? "TC " : "",
+ DNS_RD(dns) ? "RD " : "",
+ DNS_RA(dns) ? "RA " : "",
+ DNS_Z(dns) ? "!Z " : "",
+ DNS_AD(dns) ? "AD " : "",
+ DNS_CD(dns) ? "CD " : "",
+ DNS_RC(dns) ? "RC " : "",
+ DNS_RCODE_S(DNS_RC(dns))) == -1) {
+ return "_DNS_";
+ }
+ return pkt_hdr_dns;
+}
+
+static void my_handler(uint8_t *user, const struct pcap_pkthdr *header,
+ const uint8_t *packet)
+{
+#if __ETHERNET__
+ struct ether_header *ether;
+#endif
+ struct my_iphdr *ip;
+ struct udphdr *udp;
+ struct dnshdr *dns;
+ struct dns_q dns_q;
+ struct dns_rr dns_rr;
+
+ const uint8_t *packet_ptr;
+ size_t available_bytes;
+
+ uint16_t dns_q_count;
+ uint16_t dns_rr_count;
+ size_t dns_q_bytes;
+ size_t dns_rr_bytes;
+
+ struct config *config = (struct config *)user;
+
+ config->cur->ps_recv++;
+
+ if (header->caplen != header->len) {
+ fprintf(stderr, "%s: caplen %u != len %u\n", __func__,
+ header->caplen, header->len);
+ }
+ packet_ptr = packet;
+ available_bytes = header->caplen;
+
+#if __ETHERNET__
+ ASSERT_ENOUGH_BYTES_FOR(sizeof(struct ether_header));
+ ether = (struct ether_header *)packet_ptr;
+ if (unlikely(ntohs(ether->ether_type) != ETHERTYPE_IP)) {
+ fprintf(stderr, "%s: ether->ether_type %u != ETHERTYPE_IP\n",
+ __func__, ntohs(ether->ether_type));
+ return;
+ }
+ ADVANCE_PACKET_PTR(sizeof(struct ether_header));
+#endif
+
+ /* IP */
+ ASSERT_ENOUGH_BYTES_FOR(sizeof(struct my_iphdr));
+ ip = (struct my_iphdr *)packet_ptr;
+ if (unlikely(IP_V(ip) != IPVERSION)) {
+ fprintf(stderr, "%s: IP_V(ip) %u != IPVERSION\n",
+ __func__, IP_V(ip));
+ return;
+ }
+ ADVANCE_PACKET_PTR(IP_HL(ip) * sizeof(uint32_t));
+
+ /* UDP */
+ if (unlikely(ip->protocol != IPPROTO_UDP)) {
+ fprintf(stderr, "%s: ip->protocol %u != IPPROTO_UDP\n",
+ __func__, ip->protocol);
+ return;
+ }
+ ASSERT_ENOUGH_BYTES_FOR(sizeof(struct udphdr));
+ udp = (struct udphdr *)packet_ptr;
+ ASSERT_ENOUGH_BYTES_FOR(ntohs(udp->len));
+ ADVANCE_PACKET_PTR(sizeof(struct udphdr));
+
+
+ /* DNS */
+ ASSERT_ENOUGH_BYTES_FOR(sizeof(struct dnshdr));
+ dns = (struct dnshdr *)packet_ptr;
+ ADVANCE_PACKET_PTR(sizeof(struct dnshdr));
+
+ /* DNS Q */
+ for (dns_q_count = ntohs(dns->qdcount); dns_q_count; dns_q_count--) {
+ memset(&dns_q, 0, sizeof(dns_q));
+ if (!(dns_q_bytes = parse_dns_question(packet_ptr, available_bytes,
+ &dns_q))) {
+ break;
+ }
+
+ if (VERBOSE(config->flags)) {
+ fprintf(stdout, "%s %s %s %s\n\t%s\n"
+ "\t> %s %s(%u) %s%s\n",
+ print_pkt_ts(&header->ts),
+ config->cur->name,
+ print_pkt_hdr_ether(ether),
+ print_pkt_hdr_ip(ip),
+ print_pkt_hdr_dns(dns),
+ dns_q.qname ? dns_q.qname : (uint8_t *)"_NAME_",
+ DNS_QTYPE_S(ntohs(dns_q.qtype)),
+ ntohs(dns_q.qtype),
+ MDNS_UNICAST_RESPONSE(&dns_q) ? "U " : "",
+ DNS_RRCLASS_S(MDNS_QCLASS(&dns_q)));
+ }
+
+ if (dns_q.qname) {
+ free(dns_q.qname);
+ }
+
+ ADVANCE_PACKET_PTR(dns_q_bytes);
+ }
+
+ if (dns_q_count != 0) {
+ fprintf(stderr, "%s: Failed to parse all DNS questions\n", __func__);
+ return;
+ }
+
+ /* DNS RR */
+ dns_rr_count = ntohs(dns->ancount) + ntohs(dns->nscount) +
+ ntohs(dns->arcount);
+ char sym = '<';
+ for (; dns_rr_count; dns_rr_count--) {
+ memset(&dns_rr, 0, sizeof(dns_rr));
+ if (!(dns_rr_bytes = parse_dns_rr(packet_ptr, available_bytes,
+ &dns_rr))) {
+ break;
+ }
+
+ if (dns_rr_count <= ntohs(dns->nscount) + ntohs(dns->arcount)) {
+ sym = '$';
+ }
+ if (dns_rr_count <= ntohs(dns->arcount)) {
+ sym = '*';
+ }
+
+ if (VERBOSE(config->flags)) {
+ fprintf(stdout, "%s %s %s %s\n\t%s\n"
+ "\t%c %s %s(%u) %s%s ttl:%u ([%u]:)\n",
+ print_pkt_ts(&header->ts),
+ config->cur->name,
+ print_pkt_hdr_ether(ether),
+ print_pkt_hdr_ip(ip),
+ print_pkt_hdr_dns(dns),
+ sym,
+ dns_rr.name ? dns_rr.name : (uint8_t *)"_NAME_",
+ DNS_QTYPE_S(ntohs(dns_rr.type)),
+ ntohs(dns_rr.type),
+ /*
+ * The following doesn't make sense if
+ * ntohs(dns_rr.type) == 41 (OPT)
+ */
+ MDNS_CACHE_FLUSH(&dns_rr) ? "F " : "",
+ DNS_RRCLASS_S(MDNS_CLASS(&dns_rr)),
+ ntohs(dns_rr.ttl),
+ /* --- */
+ ntohs(dns_rr.rdlength));
+ }
+
+ if (dns_rr.name) {
+ free(dns_rr.name);
+ }
+
+ if (dns_rr.rdata) {
+ free(dns_rr.rdata);
+ }
+
+ ADVANCE_PACKET_PTR(dns_rr_bytes);
+ }
+
+ if (dns_rr_count != 0) {
+ fprintf(stderr, "%s: Failed to parse all DNS resoure records\n",
+ __func__);
+ return;
+ }
+
+ if (available_bytes != 0) {
+ fprintf(stderr, "%s: Failed to parse all available bytes\n",
+ __func__);
+ return;
+ }
+
+ struct pcap_dev *dst = config->dev;
+ struct pcap_dev *cur = config->cur;
+ int r;
+ for (; dst; dst = dst->next) {
+ if (dst == cur) {
+ continue;
+ }
+
+ switch (dst->flags) {
+ case PCAP_DEV_IO_TRCE | PCAP_DEV_MODE_R:
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_R:
+ /* Ignore */
+ break;
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_W:
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_R | PCAP_DEV_MODE_W:
+ if (!dst->handle) {
+ break;
+ }
+
+ config->cur->ps_sent++;
+
+ if (VERBOSE(config->flags)) {
+ fprintf(stdout, "%s %s > %s\n",
+ print_pkt_ts(&header->ts),
+ cur->name, dst->name);
+ }
+
+ r = pcap_inject(dst->handle, packet, header->caplen);
+ if (r == -1) {
+ fprintf(stderr, "%s: %s\n", __func__,
+ pcap_geterr(dst->handle));
+ }
+
+ break;
+ case PCAP_DEV_IO_DUMP | PCAP_DEV_MODE_W:
+ if (!dst->handle) {
+ break;
+ }
+
+ config->cur->ps_sent++;
+
+ pcap_dump((u_char *)dst->handle, header, packet);
+
+ break;
+ default:
+ fprintf(stderr, "%s Unexpected %s (0x%02x) write error\n",
+ __func__, dst->name, dst->flags);
+ break;
+ }
+ }
+}
+
+static void reset(struct config *config)
+{
+ del_pcap_devices(&config->dev);
+}
+
+static int configure(struct config *config, int argc, char *argv[])
+{
+ int i;
+
+ while ((i = getopt(argc, argv, "hvf:pi:o:r:w:")) != -1) {
+ switch(i) {
+ case 'h':
+ fprintf(stderr, "Use: %s [-h] [-v] [-f bpf] "
+ "-i if_in [-o if_out]\n", argv[0]);
+ return 1;
+ break;
+ case 'v':
+ config->flags |= OPT_VERBOSE;
+ break;
+ case 'f':
+ config->bpf_txt = optarg;
+ break;
+ case 'p':
+ config->flags |= OPT_PROMISCUOUS;
+ break;
+ case 'i':
+ add_pcap_device(&config->dev, optarg,
+ PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_R);
+ break;
+ case 'r':
+ add_pcap_device(&config->dev, optarg,
+ PCAP_DEV_IO_TRCE | PCAP_DEV_MODE_R);
+ break;
+ case 'o':
+ add_pcap_device(&config->dev, optarg,
+ PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_W);
+ break;
+ case 'w':
+ add_pcap_device(&config->dev, optarg,
+ PCAP_DEV_IO_DUMP | PCAP_DEV_MODE_W);
+ break;
+ default:
+ break;
+ }
+ }
+
+#if __DEBUG__
+ print_pcap_devices(config->dev, "config");
+#endif
+
+ if (!config->bpf_txt) {
+ config->bpf_txt = BPF_DEFAULT;
+ }
+
+ return 0;
+}
+
+static void signal_handler(int signum)
+{
+ switch(signum) {
+ case SIGINT:
+ case SIGTERM:
+ if (break_pcap_loop) {
+ exit(-1);
+ }
+ break_pcap_loop = 1;
+ break;
+ default:
+ break;
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ struct config config;
+ struct sigaction act, act_old;
+ int r;
+
+ memset(&config, 0, sizeof(config));
+ r = configure(&config, argc, argv);
+ switch (r) {
+ case -1:
+ fprintf(stderr, "%s: Configuration failed\n", __func__);
+ /* Fall through */
+ case 1:
+ return r;
+ break;
+ default:
+ break;
+ }
+
+ open_pcap_devices(config.dev, PROMISCUOUS(config.flags),
+ config.bpf_txt, PCAP_D_IN);
+ open_pcap_dumps(config.dev);
+
+#if __DEBUG__
+ print_pcap_devices(config.dev, "open ");
+#endif
+
+ act.sa_handler = signal_handler;
+ sigemptyset(&act.sa_mask);
+ act.sa_flags = 0;
+ if (sigaction(SIGINT, &act, &act_old) ||
+ sigaction(SIGTERM, &act, NULL)) {
+ fprintf(stderr, "%s: sigaction failed\n", __func__);
+ }
+
+ if (!VERBOSE(config.flags)) {
+ if (daemon(1 /* nochdir */, 0 /* noclose */) == -1) {
+ fprintf(stderr, "%s: daemon() has failed\n", __func__);
+ }
+ }
+
+ while (!break_pcap_loop) {
+ uint8_t active_handles = 0;
+
+ for (config.cur = config.dev;
+ !break_pcap_loop && config.cur;
+ config.cur = config.cur->next) {
+
+ if (!config.cur->handle ||
+ !(config.cur->flags & PCAP_DEV_MODE_R)) {
+ continue;
+ }
+ active_handles = 1;
+
+ r = pcap_dispatch(config.cur->handle, -1, &my_handler,
+ (unsigned char *)&config);
+ switch (r) {
+ case PCAP_ERROR:
+ fprintf(stderr, "%s: %s\n", __func__,
+ pcap_geterr(config.cur->handle));
+ break;
+ case 0:
+ if (config.cur->flags & PCAP_DEV_IO_TRCE) {
+ close_pcap(config.cur);
+ continue;
+ }
+ default:
+ break;
+ }
+ }
+
+ if (!active_handles) {
+ break;
+ }
+
+ sleep(PCAP_TIMEOUT / 1000);
+ }
+
+ if (sigaction(SIGINT, &act_old, NULL) ||
+ sigaction(SIGTERM, &act_old, NULL)) {
+ fprintf(stderr, "%s sigaction reset failed\n", __func__);
+ }
+
+#if __DEBUG__
+ print_pcap_devices(config.dev, "spent ");
+#endif
+
+#if __DEBUG__
+ stat_pcap_devices(config.dev);
+#endif
+
+ close_pcap_devices(config.dev);
+
+#if __DEBUG__
+ print_pcap_devices(config.dev, "closed");
+#endif
+
+ reset(&config);
+
+ return 0;
+}
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <pcap.h>
+
+#include "pcap_aux.h"
+
+void stat_pcap_devices(struct pcap_dev *dev)
+{
+ for (; dev; dev = dev->next) {
+ if (!dev->handle) {
+ continue;
+ }
+ fprintf(stderr, "%s packets received %u, reflected %u\n",
+ dev->name, dev->ps_recv, dev->ps_sent);
+ }
+}
+
+void print_pcap_devices(struct pcap_dev *dev, const char *label)
+{
+ while (dev) {
+ fprintf(stderr, "%s handle: 0x%08x flags: 0x%02x name: %s\n",
+ label, dev->handle ? (size_t)dev->handle : 0,
+ dev->flags, dev->name);
+ dev = dev->next;
+ }
+}
+
+int open_pcap_dumps(struct pcap_dev *devs)
+{
+ struct pcap_dev *src_dev, *dst_dev;
+
+ /*
+ * Find a source device to use as a reference in pcap_dump_open.
+ * (For its linktype, snaplen, precision)
+ * Assume all source devices are of the same type.
+ */
+ for (src_dev = devs; src_dev; src_dev = src_dev->next) {
+ if (!src_dev->handle) {
+ continue;
+ }
+ break;
+ }
+
+ for (dst_dev = devs; dst_dev; dst_dev = dst_dev->next) {
+ if (!(dst_dev->flags & PCAP_DEV_IO_DUMP)) {
+ continue;
+ }
+
+ if (!src_dev) {
+ fprintf(stderr, "%s: No source device reference\n", __func__);
+ return -1;
+ }
+
+ if (!(dst_dev->handle = pcap_dump_open(src_dev->handle,
+ dst_dev->name))) {
+ fprintf(stderr, "%s: %s\n", __func__, pcap_geterr(src_dev->handle));
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
+static int set_pcap_nonblock(pcap_t *handle)
+{
+ char errbuf[PCAP_ERRBUF_SIZE];
+
+ memset(errbuf, 0, PCAP_ERRBUF_SIZE);
+
+ if (pcap_setnonblock(handle, 1, errbuf) == PCAP_ERROR) {
+ fprintf(stderr, "%s: %s\n", __func__, errbuf);
+ return -1;
+ }
+
+ return 0;
+}
+
+static int set_pcap_filter(pcap_t *handle, char *bpf_s)
+{
+ int ret;
+ struct bpf_program bpf;
+
+ if (pcap_compile(handle, &bpf, bpf_s, BPF_OPTIMIZE,
+ PCAP_NETMASK_UNKNOWN) == -1) {
+ fprintf(stderr, "%s: %s\n", __func__, pcap_geterr(handle));
+ return -1;
+ }
+
+ if ((ret = pcap_setfilter(handle, &bpf)) == -1) {
+ fprintf(stderr, "%s: %s\n", __func__, pcap_geterr(handle));
+ }
+ pcap_freecode(&bpf);
+
+ return ret;
+}
+
+static void close_pcap_nop(struct pcap_dev *unused)
+{
+ return;
+}
+
+static void close_pcap_dump(struct pcap_dev *dev)
+{
+ if (!dev->handle) {
+ return;
+ }
+ pcap_dump_close(dev->handle);
+ dev->handle = NULL;
+}
+
+void close_pcap(struct pcap_dev *dev)
+{
+ if (!dev->handle) {
+ return;
+ }
+ pcap_close(dev->handle);
+ dev->handle = NULL;
+}
+
+void close_pcap_devices(struct pcap_dev *dev)
+{
+ struct pcap_dev *cur;
+
+ static void (*close_pcap_device[])(struct pcap_dev *) = {
+ &close_pcap_nop, /* 0 */
+ &close_pcap, /* PCAP_DEV_IO_TRCE */
+ &close_pcap, /* PCAP_DEV_IO_WIRE */
+ &close_pcap_nop,
+ &close_pcap_dump, /* PCAP_DEV_IO_DUMP */
+ &close_pcap_nop,
+ &close_pcap_nop,
+ &close_pcap_nop, /* 7 */
+ };
+
+ for (cur = dev; cur; cur = cur->next) {
+ close_pcap_device[cur->flags & PCAP_DEV_IO](cur);
+ }
+
+ return;
+}
+
+static int open_pcap_nop(struct pcap_dev *unused1, uint8_t unused2)
+{
+ return 1;
+}
+
+static int open_pcap_offline(struct pcap_dev *dev, uint8_t unused)
+{
+ char errbuf[PCAP_ERRBUF_SIZE];
+
+ memset(errbuf, 0, PCAP_ERRBUF_SIZE);
+
+ if (!(dev->handle = pcap_open_offline(dev->name, errbuf))) {
+ fprintf(stderr, "%s: %s\n", __func__, errbuf);
+ return -1;
+ }
+
+ return 0;
+}
+
+static int open_pcap_live(struct pcap_dev *dev, uint8_t promiscuous)
+{
+ char errbuf[PCAP_ERRBUF_SIZE];
+
+ memset(errbuf, 0, PCAP_ERRBUF_SIZE);
+
+ if (!(dev->handle = pcap_open_live(dev->name, PCAP_SNAPLEN, promiscuous,
+ PCAP_TIMEOUT, errbuf))) {
+ fprintf(stderr, "%s: %s\n", __func__, errbuf);
+ return -1;
+ }
+
+ return 0;
+}
+
+int open_pcap_devices(struct pcap_dev *dev, uint8_t promiscuous, char *bpf_s,
+ pcap_direction_t d)
+{
+ struct pcap_dev *cur;
+ int r;
+
+ static int (*open_pcap_device[])(struct pcap_dev *, uint8_t) = {
+ &open_pcap_nop, /* 0 */
+ &open_pcap_offline, /* PCAP_DEV_IO_TRCE */
+ &open_pcap_live, /* PCAP_DEV_IO_WIRE */
+ &open_pcap_nop,
+ &open_pcap_nop, /* PCAP_DEV_IO_DUMP */
+ &open_pcap_nop,
+ &open_pcap_nop,
+ &open_pcap_nop, /* 7 */
+ };
+
+ for (cur = dev; cur; cur = cur->next) {
+ r = open_pcap_device[cur->flags & PCAP_DEV_IO](cur, promiscuous);
+ switch (r) {
+ case -1:
+ fprintf(stderr, "%s: Failed to open %s.\n",
+ __func__, cur->name);
+ /* Fall through */
+ case 1:
+ continue;
+ break;
+ default:
+ break;
+ }
+
+ if (set_pcap_filter(cur->handle, bpf_s) == -1) {
+ fprintf(stderr, "%s: Failed to set filter on %s\n",
+ __func__, cur->name);
+ pcap_close(cur->handle);
+ cur->handle = NULL;
+ continue;
+ }
+
+ if (cur->flags & PCAP_DEV_IO_WIRE &&
+ set_pcap_nonblock(cur->handle) == -1) {
+ fprintf(stderr, "%s: Failed to set %s as non-blocking\n",
+ __func__, cur->name);
+ pcap_close(cur->handle);
+ cur->handle = NULL;
+ continue;
+ }
+
+ if (cur->flags & PCAP_DEV_IO_WIRE &&
+ pcap_setdirection(cur->handle, d) == PCAP_ERROR) {
+ fprintf(stderr, "%s: Failed to set direction for %s\n",
+ __func__, cur->name);
+ pcap_close(cur->handle);
+ cur->handle = NULL;
+ continue;
+ }
+ }
+
+ return 0;
+}
+
+void del_pcap_devices(struct pcap_dev **dev)
+{
+ struct pcap_dev *cur;
+
+ while (*dev) {
+ cur = *dev;
+ *dev = cur->next;
+ free(cur);
+ }
+}
+
+static struct pcap_dev *find_pcap_device(struct pcap_dev *dev, char *name)
+{
+ struct pcap_dev *cur;
+
+ for (cur = dev; cur; cur = cur->next) {
+ if (strcmp(cur->name, name) !=0 ) {
+ continue;
+ }
+ break;
+ }
+
+ return cur;
+}
+
+int add_pcap_device(struct pcap_dev **dev, char *name, uint8_t flags)
+{
+ struct pcap_dev *new, *cur;
+
+ switch (flags) {
+ case PCAP_DEV_IO_TRCE | PCAP_DEV_MODE_R:
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_R:
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_W:
+ case PCAP_DEV_IO_WIRE | PCAP_DEV_MODE_RW:
+ case PCAP_DEV_IO_DUMP | PCAP_DEV_MODE_W:
+ break;
+ default:
+ fprintf(stderr, "%s: Unsupported flags %0x02x for %s\n",
+ __func__, flags, name);
+ return -1;
+ }
+
+ new = find_pcap_device(*dev, name);
+ if (new) {
+ if ((new->flags & PCAP_DEV_IO) != (flags & PCAP_DEV_IO)) {
+ fprintf(stderr, "%s: conflicting flags (0x%02x, 0x%02x) for %s\n",
+ __func__, new->flags, flags, new->name);
+ return -1;
+ }
+ new->flags |= flags;
+ return 0;
+ }
+
+ new = malloc(sizeof(*new));
+ if (!new) {
+ fprintf(stderr, "%s: malloc failed\n", __func__);
+ return -1;
+ }
+ memset(new, 0, sizeof(*new));
+ new->name = name;
+ new->flags = flags;
+
+ if (!*dev) {
+ *dev = new;
+ return 0;
+ }
+
+ for (cur = *dev; cur->next; cur = cur->next) {;}
+ cur->next = new;
+
+ return 0;
+}
git.99rst.org / mdns-reflect.git / commitdiff