From d3f37628b0b9fae97ff4adc45792c81c23497d5d Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Tue, 12 Aug 2014 19:35:59 -0700 Subject: [PATCH] monitor: Add support for writing combined PCAP trace files --- monitor/main.c | 10 +++- monitor/nlmon.c | 85 +++++++++++++++++++++++++++++----- monitor/nlmon.h | 2 +- monitor/pcap.c | 121 +++++++++++++++++++++++++++++++++++++++++++++--- monitor/pcap.h | 7 ++- 5 files changed, 203 insertions(+), 22 deletions(-) diff --git a/monitor/main.c b/monitor/main.c index ef9fd6a5..f8ff9c4c 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -38,6 +38,7 @@ #include "monitor/display.h" static struct nlmon *nlmon = NULL; +static const char *writer_path = NULL; #define NLA_OK(nla,len) ((len) >= (int) sizeof(struct nlattr) && \ (nla)->nla_len >= sizeof(struct nlattr) && \ @@ -83,7 +84,7 @@ static void genl_parse(uint16_t type, const void *data, uint32_t len, return; if (!strcmp(name, NL80211_GENL_NAME)) { - nlmon = nlmon_open(ifname, id); + nlmon = nlmon_open(ifname, id, writer_path); if (!nlmon) l_main_quit(); } @@ -236,12 +237,14 @@ static void usage(void) printf("\tiwmon [options]\n"); printf("Options:\n" "\t-r, --read Read netlink PCAP trace files\n" + "\t-w, --write Write netlink PCAP trace files\n" "\t-i, --interface Use specified netlink monitor\n" "\t-h, --help Show help options\n"); } static const struct option main_options[] = { { "read", required_argument, NULL, 'r' }, + { "write", required_argument, NULL, 'w' }, { "interface", required_argument, NULL, 'i' }, { "version", no_argument, NULL, 'v' }, { "help", no_argument, NULL, 'h' }, @@ -260,7 +263,7 @@ int main(int argc, char *argv[]) for (;;) { int opt; - opt = getopt_long(argc, argv, "r:i:vh", main_options, NULL); + opt = getopt_long(argc, argv, "r:w:i:vh", main_options, NULL); if (opt < 0) break; @@ -268,6 +271,9 @@ int main(int argc, char *argv[]) case 'r': reader_path = optarg; break; + case 'w': + writer_path = optarg; + break; case 'i': ifname = optarg; break; diff --git a/monitor/nlmon.c b/monitor/nlmon.c index b2caa590..308f6b08 100644 --- a/monitor/nlmon.c +++ b/monitor/nlmon.c @@ -42,6 +42,7 @@ #include "linux/nl80211.h" #include "src/ie.h" +#include "monitor/pcap.h" #include "monitor/display.h" #include "monitor/nlmon.h" @@ -66,6 +67,7 @@ struct nlmon { struct l_io *io; struct l_io *pae_io; struct l_queue *req_list; + struct pcap *pcap; }; struct nlmon_req { @@ -1489,10 +1491,43 @@ static bool nlmon_req_match(const void *a, const void *b) return (req->seq == match->seq && req->pid == match->pid); } -static void store_message(struct nlmon *nlmon, const struct timeval *tv, - const struct tpacket_auxdata *tp, +static void store_packet(struct nlmon *nlmon, const struct timeval *tv, + uint16_t pkt_type, + uint16_t arphrd_type, + uint16_t proto_type, + const void *data, uint32_t size) +{ + uint8_t sll_hdr[16], *buf = sll_hdr; + + if (!nlmon->pcap) + return; + + memset(sll_hdr, 0, sizeof(sll_hdr)); + + pkt_type = L_CPU_TO_BE16(pkt_type); + L_PUT_UNALIGNED(pkt_type, (uint16_t *) buf); + + arphrd_type = L_CPU_TO_BE16(arphrd_type); + L_PUT_UNALIGNED(arphrd_type, (uint16_t *) (buf + 2)); + + proto_type = L_CPU_TO_BE16(proto_type); + L_PUT_UNALIGNED(proto_type, (uint16_t *) (buf + 14)); + + pcap_write(nlmon->pcap, tv, &sll_hdr, sizeof(sll_hdr), data, size); +} + +static void store_netlink(struct nlmon *nlmon, const struct timeval *tv, + uint16_t proto_type, const struct nlmsghdr *nlmsg) { + store_packet(nlmon, tv, PACKET_HOST, ARPHRD_NETLINK, proto_type, + nlmsg, nlmsg->nlmsg_len); +} + +static void store_message(struct nlmon *nlmon, const struct timeval *tv, + const struct nlmsghdr *nlmsg) +{ + store_netlink(nlmon, tv, NETLINK_GENERIC, nlmsg); } static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv, @@ -1528,7 +1563,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv, return; } - store_message(nlmon, tv, tp, nlmsg); + store_message(nlmon, tv, nlmsg); print_message(tv, type, nlmsg->nlmsg_flags, status, req->cmd, req->version, NULL, sizeof(status)); @@ -1537,8 +1572,11 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv, return; } - if (nlmsg->nlmsg_type != nlmon->id) + if (nlmsg->nlmsg_type != nlmon->id) { + if (nlmsg->nlmsg_type == GENL_ID_CTRL) + store_message(nlmon, tv, nlmsg); return; + } if (nlmsg->nlmsg_flags & NLM_F_REQUEST) { const struct genlmsghdr *genlmsg = NLMSG_DATA(nlmsg); @@ -1554,7 +1592,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv, l_queue_push_tail(nlmon->req_list, req); - store_message(nlmon, tv, tp, nlmsg); + store_message(nlmon, tv, nlmsg); print_message(tv, MSG_REQUEST, flags, 0, req->cmd, req->version, NLMSG_DATA(nlmsg) + GENL_HDRLEN, @@ -1577,7 +1615,7 @@ static void nlmon_message(struct nlmon *nlmon, const struct timeval *tv, type = MSG_RESULT; } - store_message(nlmon, tv, tp, nlmsg); + store_message(nlmon, tv, nlmsg); print_message(tv, type, nlmsg->nlmsg_flags, 0, genlmsg->cmd, genlmsg->version, NLMSG_DATA(nlmsg) + GENL_HDRLEN, @@ -1752,6 +1790,7 @@ static bool nlmon_receive(struct l_io *io, void *user_data) struct tpacket_auxdata copy_tp; const struct timeval *tv = NULL; const struct tpacket_auxdata *tp = NULL; + uint16_t proto_type; unsigned char buf[8192]; unsigned char control[32]; ssize_t bytes_read; @@ -1783,12 +1822,11 @@ static bool nlmon_receive(struct l_io *io, void *user_data) return true; } - if (sll.sll_protocol != htons(NETLINK_GENERIC)) - return true; - if (sll.sll_hatype != ARPHRD_NETLINK) return true; + proto_type = ntohs(sll.sll_protocol); + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == SOL_SOCKET && @@ -1806,7 +1844,14 @@ static bool nlmon_receive(struct l_io *io, void *user_data) for (nlmsg = iov.iov_base; NLMSG_OK(nlmsg, bytes_read); nlmsg = NLMSG_NEXT(nlmsg, bytes_read)) { - nlmon_message(nlmon, tv, tp, nlmsg); + switch (proto_type) { + case NETLINK_ROUTE: + store_netlink(nlmon, tv, proto_type, nlmsg); + break; + case NETLINK_GENERIC: + nlmon_message(nlmon, tv, tp, nlmsg); + break; + } } return true; @@ -1989,6 +2034,9 @@ static bool pae_receive(struct l_io *io, void *user_data) } } + store_packet(nlmon, tv, sll.sll_pkttype, ARPHRD_ETHER, ETH_P_PAE, + buf, bytes_read); + nlmon_print_pae(nlmon, tv, sll.sll_pkttype, sll.sll_ifindex, buf, bytes_read); @@ -2020,10 +2068,11 @@ static struct l_io *open_pae(void) return io; } -struct nlmon *nlmon_open(const char *ifname, uint16_t id) +struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname) { struct nlmon *nlmon; struct l_io *io, *pae_io; + struct pcap *pcap; io = open_packet(ifname); if (!io) @@ -2035,12 +2084,23 @@ struct nlmon *nlmon_open(const char *ifname, uint16_t id) return NULL; } + if (pathname) { + pcap = pcap_create(pathname); + if (!pcap) { + l_io_destroy(pae_io); + l_io_destroy(io); + return NULL; + } + } else + pcap = NULL; + nlmon = l_new(struct nlmon, 1); nlmon->id = id; nlmon->io = io; nlmon->pae_io = pae_io; nlmon->req_list = l_queue_new(); + nlmon->pcap = pcap; l_io_set_read_handler(nlmon->io, nlmon_receive, nlmon, NULL); l_io_set_read_handler(nlmon->pae_io, pae_receive, nlmon, NULL); @@ -2057,5 +2117,8 @@ void nlmon_close(struct nlmon *nlmon) l_io_destroy(nlmon->pae_io); l_queue_destroy(nlmon->req_list, nlmon_req_free); + if (nlmon->pcap) + pcap_close(nlmon->pcap); + l_free(nlmon); } diff --git a/monitor/nlmon.h b/monitor/nlmon.h index 7ef0d5ee..99abaff7 100644 --- a/monitor/nlmon.h +++ b/monitor/nlmon.h @@ -25,7 +25,7 @@ struct nlmon; -struct nlmon *nlmon_open(const char *ifname, uint16_t id); +struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname); void nlmon_close(struct nlmon *nlmon); struct nlmon *nlmon_create(void); diff --git a/monitor/pcap.c b/monitor/pcap.c index 73df376e..caf35ef7 100644 --- a/monitor/pcap.c +++ b/monitor/pcap.c @@ -28,6 +28,7 @@ #include #include #include +#include #include #include "monitor/pcap.h" @@ -53,11 +54,12 @@ struct pcap_pkt { struct pcap { int fd; + bool closed; uint32_t type; uint32_t snaplen; }; -struct pcap *pcap_open(const char *path) +struct pcap *pcap_open(const char *pathname) { struct pcap *pcap; struct pcap_hdr hdr; @@ -65,7 +67,7 @@ struct pcap *pcap_open(const char *path) pcap = l_new(struct pcap, 1); - pcap->fd = open(path, O_RDONLY | O_CLOEXEC); + pcap->fd = open(pathname, O_RDONLY | O_CLOEXEC); if (pcap->fd < 0) { perror("Failed to open PCAP file"); l_free(pcap); @@ -93,6 +95,7 @@ struct pcap *pcap_open(const char *path) goto failed; } + pcap->closed = false; pcap->snaplen = hdr.snaplen; pcap->type = hdr.network; @@ -105,6 +108,56 @@ failed: return NULL; } + +struct pcap *pcap_create(const char *pathname) +{ + struct pcap *pcap; + struct pcap_hdr hdr; + ssize_t len; + + pcap = l_new(struct pcap, 1); + + pcap->fd = open(pathname, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC, + S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (pcap->fd < 0) { + perror("Failed to create PCAP file"); + l_free(pcap); + return NULL; + } + + pcap->closed = false; + pcap->snaplen = 0x0000ffff; + pcap->type = 0x00000071; + + memset(&hdr, 0, sizeof(hdr)); + hdr.magic_number = 0xa1b2c3d4; + hdr.version_major = 0x0002; + hdr.version_minor = 0x0004; + hdr.thiszone = 0; + hdr.sigfigs = 0; + hdr.snaplen = pcap->snaplen; + hdr.network = pcap->type; + + len = write(pcap->fd, &hdr, PCAP_HDR_SIZE); + if (len < 0) { + perror("Failed to write PCAP header"); + goto failed; + } + + if (len != PCAP_HDR_SIZE) { + fprintf(stderr, "Written PCAP header size mimatch\n"); + goto failed; + } + + return pcap; + +failed: + close(pcap->fd); + l_free(pcap); + + return NULL; +} + void pcap_close(struct pcap *pcap) { if (!pcap) @@ -142,21 +195,32 @@ bool pcap_read(struct pcap *pcap, struct timeval *tv, if (!pcap) return false; - bytes_read = read(pcap->fd, &pkt, PCAP_PKT_SIZE); - if (bytes_read != PCAP_PKT_SIZE) + if (pcap->closed) return false; + bytes_read = read(pcap->fd, &pkt, PCAP_PKT_SIZE); + if (bytes_read != PCAP_PKT_SIZE) { + pcap->closed = true; + return false; + } + if (pkt.incl_len > size) toread = size; else toread = pkt.incl_len; bytes_read = read(pcap->fd, data, toread); - if (bytes_read < 0) + if (bytes_read < 0) { + pcap->closed = true; return false; + } - if (bytes_read < pkt.incl_len) - lseek(pcap->fd, pkt.incl_len - bytes_read, SEEK_CUR); + if (bytes_read < pkt.incl_len) { + if (lseek(pcap->fd, pkt.incl_len - bytes_read, SEEK_CUR) < 0) { + pcap->closed = true; + return false; + } + } if (tv) { tv->tv_sec = pkt.ts_sec; @@ -171,3 +235,46 @@ bool pcap_read(struct pcap *pcap, struct timeval *tv, return true; } + +bool pcap_write(struct pcap *pcap, const struct timeval *tv, + const void *phdr, uint32_t plen, + const void *data, uint32_t size) +{ + struct iovec iov[3]; + struct pcap_pkt pkt; + ssize_t written; + + if (!pcap) + return false; + + if (pcap->closed) + return false; + + memset(&pkt, 0, sizeof(pkt)); + if (tv) { + pkt.ts_sec = tv->tv_sec; + pkt.ts_usec = tv->tv_usec; + } + pkt.incl_len = plen + size; + pkt.orig_len = plen + size; + + iov[0].iov_base = &pkt; + iov[0].iov_len = PCAP_PKT_SIZE; + iov[1].iov_base = (void *) phdr; + iov[1].iov_len = plen; + iov[2].iov_base = (void *) data; + iov[2].iov_len = size; + + written = writev(pcap->fd, iov, 3); + if (written < 0) { + pcap->closed = true; + return false; + } + + if (written < (ssize_t) (PCAP_PKT_SIZE + plen + size)) { + pcap->closed = true; + return false; + } + + return true; +} diff --git a/monitor/pcap.h b/monitor/pcap.h index 429d77a5..80ec574b 100644 --- a/monitor/pcap.h +++ b/monitor/pcap.h @@ -30,7 +30,8 @@ struct pcap; -struct pcap *pcap_open(const char *path); +struct pcap *pcap_open(const char *pathname); +struct pcap *pcap_create(const char *pathname); void pcap_close(struct pcap *pcap); uint32_t pcap_get_type(struct pcap *pcap); @@ -38,3 +39,7 @@ uint32_t pcap_get_snaplen(struct pcap *pcap); bool pcap_read(struct pcap *pcap, struct timeval *tv, void *data, uint32_t size, uint32_t *len, uint32_t *real_len); + +bool pcap_write(struct pcap *pcap, const struct timeval *tv, + const void *phdr, uint32_t plen, + const void *data, uint32_t size);