diff options
Diffstat (limited to 'pjlib-util/src/pjlib-util/pcap.c')
| -rw-r--r-- | pjlib-util/src/pjlib-util/pcap.c | 307 |
1 files changed, 307 insertions, 0 deletions
diff --git a/pjlib-util/src/pjlib-util/pcap.c b/pjlib-util/src/pjlib-util/pcap.c new file mode 100644 index 00000000..6be6af3d --- /dev/null +++ b/pjlib-util/src/pjlib-util/pcap.c @@ -0,0 +1,307 @@ +#include <pjlib-util/pcap.h> +#include <pj/assert.h> +#include <pj/errno.h> +#include <pj/file_io.h> +#include <pj/pool.h> +#include <pj/sock.h> +#include <pj/string.h> + + +typedef struct pj_pcap_hdr +{ + pj_uint32_t magic_number; /* magic number */ + pj_uint16_t version_major; /* major version number */ + pj_uint16_t version_minor; /* minor version number */ + pj_int32_t thiszone; /* GMT to local correction */ + pj_uint32_t sigfigs; /* accuracy of timestamps */ + pj_uint32_t snaplen; /* max length of captured packets, in octets */ + pj_uint32_t network; /* data link type */ +} pj_pcap_hdr; + +typedef struct pj_pcap_rec_hdr +{ + pj_uint32_t ts_sec; /* timestamp seconds */ + pj_uint32_t ts_usec; /* timestamp microseconds */ + pj_uint32_t incl_len; /* number of octets of packet saved in file */ + pj_uint32_t orig_len; /* actual length of packet */ +} pj_pcap_rec_hdr; + +typedef struct pj_pcap_eth_hdr +{ + pj_uint8_t dest[6]; + pj_uint8_t src[6]; + pj_uint16_t len; +} pj_pcap_eth_hdr; + +typedef struct pj_pcap_ip_hdr +{ + pj_uint8_t v_ihl; + pj_uint8_t tos; + pj_uint16_t len; + pj_uint16_t id; + pj_uint16_t flags_fragment; + pj_uint8_t ttl; + pj_uint8_t proto; + pj_uint16_t csum; + pj_uint32_t ip_src; + pj_uint32_t ip_dst; +} pj_pcap_ip_hdr; + +typedef struct pj_pcap_udp_hdr +{ + pj_uint16_t src_port; + pj_uint16_t dst_port; + pj_uint16_t len; + pj_uint16_t csum; +} pj_pcap_udp_hdr; + + +/* Implementation of pcap file */ +struct pj_pcap_file +{ + pj_oshandle_t fd; + pj_bool_t swap; + pj_pcap_hdr hdr; + pj_pcap_filter filter; +}; + +/* Init default filter */ +PJ_DEF(void) pj_pcap_filter_default(pj_pcap_filter *filter) +{ + pj_bzero(filter, sizeof(*filter)); +} + +/* Open pcap file */ +PJ_DEF(pj_status_t) pj_pcap_open(pj_pool_t *pool, + const char *path, + pj_pcap_file **p_file) +{ + pj_pcap_file *file; + pj_ssize_t sz; + pj_status_t status; + + PJ_ASSERT_RETURN(pool && path && p_file, PJ_EINVAL); + + file = PJ_POOL_ZALLOC_T(pool, pj_pcap_file); + + status = pj_file_open(pool, path, PJ_O_RDONLY, &file->fd); + if (status != PJ_SUCCESS) + return status; + + /* Read file pcap header */ + sz = sizeof(file->hdr); + status = pj_file_read(file->fd, &file->hdr, &sz); + if (status != PJ_SUCCESS) { + pj_file_close(file->fd); + return status; + } + + /* Check magic number */ + if (file->hdr.magic_number == 0xa1b2c3d4) { + file->swap = PJ_FALSE; + } else if (file->hdr.magic_number == 0xd4c3b2a1) { + file->swap = PJ_TRUE; + file->hdr.network = pj_ntohl(file->hdr.network); + } else { + /* Not PCAP file */ + pj_file_close(file->fd); + return PJ_EINVALIDOP; + } + + *p_file = file; + return PJ_SUCCESS; +} + +/* Close pcap file */ +PJ_DEF(pj_status_t) pj_pcap_close(pj_pcap_file *file) +{ + PJ_ASSERT_RETURN(file, PJ_EINVAL); + return pj_file_close(file->fd); +} + +/* Setup filter */ +PJ_DEF(pj_status_t) pj_pcap_set_filter(pj_pcap_file *file, + const pj_pcap_filter *fil) +{ + PJ_ASSERT_RETURN(file && fil, PJ_EINVAL); + pj_memcpy(&file->filter, fil, sizeof(pj_pcap_filter)); + return PJ_SUCCESS; +} + +/* Read file */ +static pj_status_t read_file(pj_pcap_file *file, + void *buf, + pj_ssize_t *sz) +{ + pj_status_t status; + status = pj_file_read(file->fd, buf, sz); + if (status != PJ_SUCCESS) + return status; + if (*sz == 0) + return PJ_EEOF; + return PJ_SUCCESS; +} + +static pj_status_t skip(pj_oshandle_t fd, pj_off_t bytes) +{ + pj_status_t status; + status = pj_file_setpos(fd, bytes, PJ_SEEK_CUR); + if (status != PJ_SUCCESS) + return status; + return PJ_SUCCESS; +} + + +#define SKIP_PKT() \ + if (rec_incl > sz_read) { \ + status = skip(file->fd, rec_incl-sz_read);\ + if (status != PJ_SUCCESS) \ + return status; \ + } + +/* Read UDP packet */ +PJ_DEF(pj_status_t) pj_pcap_read_udp(pj_pcap_file *file, + pj_uint8_t *udp_payload, + pj_size_t *udp_payload_size) +{ + PJ_ASSERT_RETURN(file && udp_payload && udp_payload_size, PJ_EINVAL); + PJ_ASSERT_RETURN(*udp_payload_size, PJ_EINVAL); + + /* Check data link type in PCAP file header */ + if ((file->filter.link && + file->hdr.network != (pj_uint32_t)file->filter.link) || + file->hdr.network != PJ_PCAP_LINK_TYPE_ETH) + { + /* Link header other than Ethernet is not supported for now */ + return PJ_ENOTSUP; + } + + /* Loop until we have the packet */ + for (;;) { + union { + pj_pcap_rec_hdr rec; + pj_pcap_eth_hdr eth; + pj_pcap_ip_hdr ip; + pj_pcap_udp_hdr udp; + } tmp; + unsigned rec_incl; + pj_ssize_t sz; + unsigned sz_read = 0; + pj_status_t status; + + /* Read PCAP packet header */ + sz = sizeof(tmp.rec); + status = read_file(file, &tmp.rec, &sz); + if (status != PJ_SUCCESS) + return status; + + rec_incl = tmp.rec.incl_len; + + /* Swap byte ordering */ + if (file->swap) { + tmp.rec.incl_len = pj_ntohl(tmp.rec.incl_len); + tmp.rec.orig_len = pj_ntohl(tmp.rec.orig_len); + tmp.rec.ts_sec = pj_ntohl(tmp.rec.ts_sec); + tmp.rec.ts_usec = pj_ntohl(tmp.rec.ts_usec); + } + + /* Read link layer header */ + switch (file->hdr.network) { + case PJ_PCAP_LINK_TYPE_ETH: + sz = sizeof(tmp.eth); + status = read_file(file, &tmp.eth, &sz); + break; + default: + return PJ_ENOTSUP; + } + + if (status != PJ_SUCCESS) + return status; + + sz_read += sz; + + /* Read IP header */ + sz = sizeof(tmp.ip); + status = read_file(file, &tmp.ip, &sz); + if (status != PJ_SUCCESS) + return status; + + sz_read += sz; + + /* Skip if IP source mismatch */ + if (file->filter.ip_src && tmp.ip.ip_src != file->filter.ip_src) { + SKIP_PKT(); + continue; + } + + /* Skip if IP destination mismatch */ + if (file->filter.ip_dst && tmp.ip.ip_dst != file->filter.ip_dst) { + SKIP_PKT(); + continue; + } + + /* Skip if proto mismatch */ + if (file->filter.proto && tmp.ip.proto != file->filter.proto) { + SKIP_PKT(); + continue; + } + + /* Read transport layer header */ + switch (tmp.ip.proto) { + case PJ_PCAP_PROTO_TYPE_UDP: + sz = sizeof(tmp.udp); + status = read_file(file, &tmp.udp, &sz); + if (status != PJ_SUCCESS) + return status; + + sz_read += sz; + + /* Skip if source port mismatch */ + if (file->filter.src_port && + tmp.udp.src_port != file->filter.src_port) + { + SKIP_PKT(); + continue; + } + + /* Skip if destination port mismatch */ + if (file->filter.dst_port && + tmp.udp.dst_port != file->filter.dst_port) + { + SKIP_PKT(); + continue; + } + + /* Calculate payload size */ + sz = pj_ntohs(tmp.udp.len) - sizeof(tmp.udp); + break; + default: + SKIP_PKT(); + continue; + } + + /* Check if payload fits the buffer */ + if (sz > (pj_ssize_t)*udp_payload_size) { + SKIP_PKT(); + return PJ_ETOOSMALL; + } + + /* Read the payload */ + status = read_file(file, udp_payload, &sz); + if (status != PJ_SUCCESS) + return status; + + sz_read += sz; + + *udp_payload_size = sz; + + /* Check that we've read all the packets */ + PJ_ASSERT_RETURN(sz_read == rec_incl, PJ_EBUG); + + return PJ_SUCCESS; + } + + /* Does not reach here */ +} + + |