kopia lustrzana https://github.com/F5OEO/tstools
679 wiersze
15 KiB
C
679 wiersze
15 KiB
C
/* pcap.c */
|
|
/*
|
|
* Read pcap files.
|
|
*
|
|
* ***** BEGIN LICENSE BLOCK *****
|
|
* Version: MPL 1.1
|
|
*
|
|
* The contents of this file are subject to the Mozilla Public License Version
|
|
* 1.1 (the "License"); you may not use this file except in compliance with
|
|
* the License. You may obtain a copy of the License at
|
|
* http://www.mozilla.org/MPL/
|
|
*
|
|
* Software distributed under the License is distributed on an "AS IS" basis,
|
|
* WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
|
|
* for the specific language governing rights and limitations under the
|
|
* License.
|
|
*
|
|
* The Original Code is the MPEG TS, PS and ES tools.
|
|
*
|
|
* The Initial Developer of the Original Code is Amino Communications Ltd.
|
|
* Portions created by the Initial Developer are Copyright (C) 2008
|
|
* the Initial Developer. All Rights Reserved.
|
|
*
|
|
* Contributor(s):
|
|
* Richard Watts, Kynesim <rrw@kynesim.co.uk>
|
|
*
|
|
* ***** END LICENSE BLOCK *****
|
|
*/
|
|
|
|
|
|
/* Both of these return 1 on success, 0 on EOF, <0 on error */
|
|
|
|
#include "pcap.h"
|
|
#include "misc_fns.h"
|
|
|
|
static inline uint32_t uint_32_ctx(const struct _pcap_io_ctx *const ctx, const void *v)
|
|
{
|
|
return ctx->is_be ? uint_32_be(v) : uint_32_le(v);
|
|
}
|
|
|
|
static inline uint16_t uint_16_ctx(const struct _pcap_io_ctx *const ctx, const void *v)
|
|
{
|
|
return ctx->is_be ? uint_16_be(v) : uint_16_le(v);
|
|
}
|
|
|
|
// Hi-32, Lo-32 but native within!
|
|
static inline uint64_t uint_64_be_ctx(const struct _pcap_io_ctx *const ctx, const void *v)
|
|
{
|
|
return ((uint64_t)uint_32_ctx(ctx, v) << 32) | (uint64_t)uint_32_ctx(ctx, (const char*)v + 4);
|
|
}
|
|
|
|
static inline uint64_t uint_64_ctx(const struct _pcap_io_ctx *const ctx, const void *v)
|
|
{
|
|
return ctx->is_be ?
|
|
((uint64_t)uint_32_be(v) << 32) | (uint64_t)uint_32_be((const uint8_t *)v + 4) :
|
|
((uint64_t)uint_32_le((const uint8_t *)v + 4) << 32) | (uint64_t)uint_32_le(v);
|
|
}
|
|
|
|
|
|
|
|
static int read_block_header(const struct _pcap_io_ctx *const ctx, uint32_t *const pLength)
|
|
{
|
|
uint32_t buf[2];
|
|
int rv;
|
|
|
|
*pLength = 0;
|
|
rv = fread(buf, 8, 1, ctx->file);
|
|
if (rv != 1)
|
|
{
|
|
if (feof(ctx->file))
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_FILE_READ;
|
|
}
|
|
}
|
|
|
|
*pLength = uint_32_ctx(ctx, buf + 1);
|
|
return uint_32_ctx(ctx, buf + 0);
|
|
}
|
|
|
|
|
|
static int read_chunk(FILE *const f, const size_t len, uint8_t **const pBuf)
|
|
{
|
|
int rv;
|
|
void *buf = malloc(len);
|
|
|
|
*pBuf = NULL;
|
|
if (buf == NULL)
|
|
{
|
|
return PCAP_ERR_OUT_OF_MEMORY;
|
|
}
|
|
|
|
rv = fread(buf, len, 1, f);
|
|
if (rv != 1)
|
|
{
|
|
free(buf);
|
|
if (feof(f))
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_FILE_READ;
|
|
}
|
|
}
|
|
|
|
*pBuf = buf;
|
|
return 1;
|
|
}
|
|
|
|
static int read_options(FILE *const f, const size_t len, uint8_t **const pBuf)
|
|
{
|
|
// If all we have is the final total length data - skip it
|
|
if (len <= 4)
|
|
{
|
|
fseek(f, len, SEEK_CUR);
|
|
*pBuf = NULL;
|
|
return 1;
|
|
}
|
|
|
|
return read_chunk(f, len, pBuf);
|
|
}
|
|
|
|
typedef enum pcapng_type_e
|
|
{
|
|
PCAPNG_TYPE_INVALID_BLOCK = 0,
|
|
PCAPNG_TYPE_INTERFACE_BLOCK = 1,
|
|
PCAPNG_TYPE_PACKET_BLOCK = 2,
|
|
PCAPNG_TYPE_SIMPLE_PACKET_BLOCK = 3,
|
|
PCAPNG_TYPE_NAME_RESOLUTION_BLOCK = 4,
|
|
PCAPNG_TYPE_INTERFACE_STATISTICS_BLOCK = 5,
|
|
PCAPNG_TYPE_ENHANCED_PACKET_BLOCK = 6,
|
|
PCAPNG_TYPE_SECTION_HEADER_BLOCK = 0x0a0d0d0a
|
|
} pcapng_type_t;
|
|
|
|
typedef struct pcapng_hdr_packet_s
|
|
{
|
|
uint16_t drops_count;
|
|
uint32_t interface_id;
|
|
uint32_t captured_len;
|
|
uint32_t packet_len;
|
|
uint64_t timestamp;
|
|
} pcapng_hdr_packet_t;
|
|
|
|
typedef struct pcapng_hdr_section_s
|
|
{
|
|
uint16_t major_version;
|
|
uint16_t minor_version;
|
|
uint64_t section_length;
|
|
} pcapng_hdr_section_t;
|
|
|
|
|
|
|
|
typedef struct pcapng_header_s
|
|
{
|
|
pcapng_type_t type;
|
|
uint8_t *data;
|
|
uint8_t *options;
|
|
union
|
|
{
|
|
pcapng_hdr_packet_t packet;
|
|
pcapng_hdr_interface_t iface;
|
|
pcapng_hdr_section_t section;
|
|
} hdr;
|
|
} pcapng_header_t;
|
|
|
|
|
|
// Kill header contents
|
|
static void free_block(pcapng_header_t *const hdr)
|
|
{
|
|
hdr->type = PCAPNG_TYPE_INVALID_BLOCK;
|
|
if (hdr->data != NULL)
|
|
{
|
|
free(hdr->data);
|
|
hdr->data = NULL;
|
|
}
|
|
if (hdr->options != NULL)
|
|
{
|
|
free(hdr->options);
|
|
hdr->options = NULL;
|
|
}
|
|
}
|
|
|
|
static int do_section_header(struct _pcap_io_ctx *const ctx, uint32_t length, const uint8_t *const buf,
|
|
pcapng_header_t *const hdr)
|
|
{
|
|
uint32_t magic;
|
|
int rv;
|
|
|
|
// PCAP-NG
|
|
ctx->is_ng = 1;
|
|
|
|
magic = uint_32_ctx(ctx, buf + 0);
|
|
|
|
printf("Magic = %08x, Len = %#x\n", magic, length);
|
|
|
|
if (magic == 0x1a2b3c4d)
|
|
{
|
|
// Right way up
|
|
}
|
|
else if (magic == 0x4d3c2b1a)
|
|
{
|
|
// Wrong way up
|
|
ctx->is_be = !ctx->is_be;
|
|
// Endian reverse length
|
|
length = (length >> 16) | (length << 16);
|
|
length = ((length >> 8) & 0xff00ff) | ((length << 8) & 0xff00ff00);
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_INVALID_MAGIC;
|
|
}
|
|
|
|
|
|
#if SIZEOF_PCAP_HDR_ON_DISC != 24
|
|
#error I am confused
|
|
#endif
|
|
|
|
// Length here includes headers
|
|
if (length < 28)
|
|
{
|
|
return PCAP_ERR_BAD_LENGTH;
|
|
}
|
|
|
|
length -= 24;
|
|
|
|
hdr->hdr.section.major_version = uint_16_ctx(ctx, buf + 4);
|
|
hdr->hdr.section.minor_version = uint_16_ctx(ctx, buf + 6);
|
|
hdr->hdr.section.section_length = uint_64_ctx(ctx, buf + 8);
|
|
|
|
if ((rv = read_options(ctx->file, length, &hdr->options)) <= 0)
|
|
return rv;
|
|
|
|
return 1;
|
|
}
|
|
|
|
static int read_block(struct _pcap_io_ctx *const ctx, pcapng_header_t *const hdr)
|
|
{
|
|
int rv = 1;
|
|
int hdr_type;
|
|
uint32_t length;
|
|
|
|
hdr->type = PCAPNG_TYPE_INVALID_BLOCK;
|
|
hdr->data = NULL;
|
|
hdr->options = NULL;
|
|
|
|
if ((hdr_type = read_block_header(ctx, &length)) <= 0)
|
|
{
|
|
return hdr_type;
|
|
}
|
|
|
|
// If section header length may be endian confused so sort in a bit
|
|
if (hdr_type != PCAPNG_TYPE_SECTION_HEADER_BLOCK)
|
|
{
|
|
if (length > 0x100000 || length < 8)
|
|
{
|
|
return PCAP_ERR_BAD_LENGTH;
|
|
}
|
|
length -= 8;
|
|
}
|
|
|
|
switch (hdr_type)
|
|
{
|
|
case PCAPNG_TYPE_INTERFACE_BLOCK:
|
|
{
|
|
uint8_t buf[8];
|
|
|
|
if (length < 12)
|
|
return PCAP_ERR_BAD_LENGTH;
|
|
|
|
if (fread(buf, 8, 1, ctx->file) != 1)
|
|
return PCAP_ERR_FILE_READ;
|
|
|
|
hdr->hdr.iface.link_type = uint_16_ctx(ctx, buf + 0);
|
|
hdr->hdr.iface.snap_len = uint_32_ctx(ctx, buf + 4);
|
|
|
|
if ((rv = read_options(ctx->file, length - 8, &hdr->options)) <= 0)
|
|
return rv;
|
|
|
|
// Now stash - cos we need it later
|
|
// Alloc a new if (or at least check we have one)
|
|
if (ctx->if_count + 1 > ctx->if_size)
|
|
{
|
|
if (ctx->interfaces == NULL)
|
|
{
|
|
if ((ctx->interfaces = malloc(sizeof(*ctx->interfaces) * 4)) == NULL)
|
|
return PCAP_ERR_OUT_OF_MEMORY;
|
|
ctx->if_size = 4;
|
|
}
|
|
else
|
|
{
|
|
pcapng_hdr_interface_t *resized = realloc(ctx->interfaces,
|
|
sizeof(*ctx->interfaces) * ctx->if_size * 2);
|
|
if (resized == NULL)
|
|
return PCAP_ERR_OUT_OF_MEMORY;
|
|
ctx->if_size *= 2;
|
|
ctx->interfaces = resized;
|
|
}
|
|
}
|
|
|
|
ctx->interfaces[ctx->if_count++] = hdr->hdr.iface;
|
|
break;
|
|
}
|
|
|
|
case PCAPNG_TYPE_PACKET_BLOCK:
|
|
case PCAPNG_TYPE_ENHANCED_PACKET_BLOCK:
|
|
{
|
|
uint8_t buf[20];
|
|
size_t data_len;
|
|
|
|
if (length < 24)
|
|
return PCAP_ERR_BAD_LENGTH;
|
|
|
|
if (fread(buf, 20, 1, ctx->file) != 1)
|
|
return PCAP_ERR_FILE_READ;
|
|
|
|
if (hdr_type == PCAPNG_TYPE_PACKET_BLOCK)
|
|
{
|
|
hdr->hdr.packet.interface_id = uint_16_ctx(ctx, buf + 0);
|
|
hdr->hdr.packet.drops_count = uint_16_ctx(ctx, buf + 2);
|
|
}
|
|
else
|
|
{
|
|
hdr->hdr.packet.interface_id = uint_32_ctx(ctx, buf + 0);
|
|
hdr->hdr.packet.drops_count = 0;
|
|
}
|
|
hdr->hdr.packet.timestamp = uint_64_be_ctx(ctx, buf + 4);
|
|
hdr->hdr.packet.captured_len = uint_32_ctx(ctx, buf + 12);
|
|
hdr->hdr.packet.packet_len = uint_32_ctx(ctx, buf + 16);
|
|
|
|
if (hdr->hdr.packet.interface_id >= ctx->if_count)
|
|
return PCAP_ERR_BAD_INTERFACE_ID;
|
|
|
|
length -= 20;
|
|
data_len = (hdr->hdr.packet.captured_len + 3) & ~3;
|
|
|
|
if (length - 4 < data_len)
|
|
return PCAP_ERR_BAD_LENGTH;
|
|
|
|
if ((rv = read_chunk(ctx->file, data_len, &hdr->data)) <= 0)
|
|
break;
|
|
|
|
length -= data_len;
|
|
|
|
if ((rv = read_options(ctx->file, length, &hdr->options)) <= 0)
|
|
break;
|
|
|
|
break;
|
|
}
|
|
|
|
case PCAPNG_TYPE_SECTION_HEADER_BLOCK:
|
|
{
|
|
uint8_t buf[16];
|
|
|
|
// Clear out old data even if we error
|
|
|
|
// All interfaces are toast
|
|
if (ctx->interfaces != NULL)
|
|
{
|
|
free(ctx->interfaces);
|
|
ctx->interfaces = NULL;
|
|
ctx->if_count = 0;
|
|
ctx->if_size = 0;
|
|
}
|
|
|
|
if (fread(buf, 16, 1, ctx->file) != 1)
|
|
return PCAP_ERR_FILE_READ;
|
|
|
|
if ((rv = do_section_header(ctx, length, buf, hdr)) < 0)
|
|
return rv;
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
fseek(ctx->file, length, SEEK_CUR);
|
|
break;
|
|
}
|
|
|
|
if (rv <= 0)
|
|
{
|
|
free_block(hdr);
|
|
}
|
|
else
|
|
{
|
|
hdr->type = hdr_type;
|
|
}
|
|
|
|
return rv;
|
|
}
|
|
|
|
|
|
static int pcap_read_header(PCAP_reader_p ctx, pcap_hdr_t *hdr)
|
|
{
|
|
uint8_t hdr_val[SIZEOF_PCAP_HDR_ON_DISC];
|
|
int rv;
|
|
uint32_t magic;
|
|
|
|
// This reads an old-style header which is shorter than the shortest new-style one
|
|
rv = fread(&hdr_val[0], SIZEOF_PCAP_HDR_ON_DISC, 1, ctx->file);
|
|
if (rv != 1)
|
|
{
|
|
if (feof(ctx->file))
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_FILE_READ;
|
|
}
|
|
}
|
|
|
|
magic = uint_32_be(hdr_val + 0);
|
|
|
|
if (magic == PCAPNG_TYPE_SECTION_HEADER_BLOCK)
|
|
{
|
|
pcapng_header_t nghdr = { 0 };
|
|
|
|
printf("NG header found\n");
|
|
|
|
// PCAP-NG
|
|
ctx->is_ng = 1;
|
|
|
|
if ((rv = do_section_header(ctx, uint_32_ctx(ctx, hdr_val + 4), hdr_val + 8, &nghdr)) <= 0)
|
|
return rv;
|
|
|
|
hdr->magic_number = 0x1a2b3c4d;
|
|
|
|
hdr->version_major = nghdr.hdr.section.major_version;
|
|
hdr->version_minor = nghdr.hdr.section.minor_version;
|
|
|
|
printf("Version: %d.%d\n", hdr->version_major, hdr->version_minor);
|
|
|
|
// Find the 1st i/f block (there must be one before the data)
|
|
|
|
for (;;)
|
|
{
|
|
free_block(&nghdr);
|
|
|
|
if ((rv = read_block(ctx, &nghdr)) <= 0)
|
|
{
|
|
return rv;
|
|
}
|
|
|
|
if (nghdr.type == PCAPNG_TYPE_INTERFACE_BLOCK)
|
|
{
|
|
hdr->snaplen = nghdr.hdr.iface.snap_len;
|
|
hdr->network = nghdr.hdr.iface.link_type;
|
|
free_block(&nghdr);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ctx->is_ng = 0;
|
|
|
|
/* The magic number is 0xa1b2c3d4. If the writing
|
|
* machine was BE, the first byte will be a1 else d4
|
|
*/
|
|
if (magic == 0xa1b2c3d4)
|
|
{
|
|
// Big endian.
|
|
ctx->is_be = 1;
|
|
}
|
|
else if (magic == 0xd4c3b2a1)
|
|
{
|
|
// Little endian.
|
|
ctx->is_be = 0;
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_INVALID_MAGIC;
|
|
}
|
|
|
|
hdr->magic_number = 0xa1b2c3d4;
|
|
|
|
hdr->version_major = (ctx->is_be ? uint_16_be(&hdr_val[4]) :
|
|
uint_16_le(&hdr_val[4]));
|
|
hdr->version_minor = (ctx->is_be ? uint_16_be(&hdr_val[6]) :
|
|
uint_16_le(&hdr_val[6]));
|
|
hdr->thiszone = (int32_t)(ctx->is_be ? uint_32_be(&hdr_val[8]) :
|
|
uint_32_le(&hdr_val[8]));
|
|
hdr->sigfigs = (ctx->is_be ? uint_32_be(&hdr_val[12]) :
|
|
uint_32_le(&hdr_val[12]));
|
|
hdr->snaplen = (ctx->is_be ? uint_32_be(&hdr_val[16]) :
|
|
uint_32_le(&hdr_val[16]));
|
|
hdr->network = (ctx->is_be ? uint_32_be(&hdr_val[20]) :
|
|
uint_32_le(&hdr_val[20]));
|
|
}
|
|
|
|
|
|
return 1;
|
|
}
|
|
|
|
static int pcap_read_pktheader(PCAP_reader_p ctx, pcaprec_hdr_t *hdr)
|
|
{
|
|
uint8_t hdr_val[SIZEOF_PCAPREC_HDR_ON_DISC];
|
|
int rv;
|
|
|
|
rv = fread(&hdr_val[0], SIZEOF_PCAPREC_HDR_ON_DISC, 1, ctx->file);
|
|
if (rv != 1)
|
|
{
|
|
if (feof(ctx->file))
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
return PCAP_ERR_FILE_READ;
|
|
}
|
|
}
|
|
|
|
hdr->ts_sec = (ctx->is_be ? uint_32_be(&hdr_val[0]) :
|
|
uint_32_le(&hdr_val[0]));
|
|
hdr->ts_usec = (ctx->is_be ? uint_32_be(&hdr_val[4]) :
|
|
uint_32_le(&hdr_val[4]));
|
|
hdr->incl_len = (ctx->is_be ? uint_32_be(&hdr_val[8]) :
|
|
uint_32_le(&hdr_val[8]));
|
|
hdr->orig_len = (ctx->is_be ? uint_32_be(&hdr_val[12]) :
|
|
uint_32_le(&hdr_val[12]));
|
|
|
|
return 1;
|
|
}
|
|
|
|
extern int pcap_open(PCAP_reader_p *ctx_p, pcap_hdr_t *out_hdr,
|
|
const char *filename)
|
|
{
|
|
FILE *fptr = (filename ? fopen(filename, "rb") : stdin);
|
|
PCAP_reader_p ctx;
|
|
int rv;
|
|
|
|
(*ctx_p) = NULL;
|
|
|
|
if (!fptr)
|
|
{
|
|
// Couldn't open the file.
|
|
return -1;
|
|
}
|
|
ctx = (PCAP_reader_p)calloc(SIZEOF_PCAP_READER, 1);
|
|
if (!ctx)
|
|
{
|
|
fclose(fptr);
|
|
// Out of memory.
|
|
return -2;
|
|
}
|
|
|
|
ctx->file = fptr;
|
|
|
|
rv = pcap_read_header(ctx, out_hdr);
|
|
|
|
if (rv != 1)
|
|
{
|
|
// Header read failed.
|
|
fclose(ctx->file);
|
|
free(ctx);
|
|
return -4;
|
|
}
|
|
|
|
(*ctx_p) = ctx;
|
|
|
|
return 0;
|
|
}
|
|
|
|
extern int pcap_read_next(PCAP_reader_p ctx, pcaprec_hdr_t *out_hdr,
|
|
uint8_t **out_data,
|
|
uint32_t *out_len)
|
|
{
|
|
int rv;
|
|
|
|
(*out_data) = NULL; (*out_len) = 0;
|
|
|
|
if (ctx->is_ng)
|
|
{
|
|
for (;;)
|
|
{
|
|
pcapng_header_t nghdr;
|
|
|
|
if ((rv = read_block(ctx, &nghdr)) <= 0)
|
|
{
|
|
return rv;
|
|
}
|
|
|
|
if (nghdr.type == PCAPNG_TYPE_PACKET_BLOCK ||
|
|
nghdr.type == PCAPNG_TYPE_ENHANCED_PACKET_BLOCK)
|
|
{
|
|
*out_data = nghdr.data;
|
|
*out_len = nghdr.hdr.packet.captured_len;
|
|
|
|
out_hdr->incl_len = nghdr.hdr.packet.captured_len;
|
|
out_hdr->orig_len = nghdr.hdr.packet.packet_len;
|
|
out_hdr->ts_sec = (uint32_t)(nghdr.hdr.packet.timestamp / 1000000);
|
|
out_hdr->ts_usec = (uint32_t)(nghdr.hdr.packet.timestamp % 1000000);
|
|
|
|
// NULL out so we don't free it here!
|
|
nghdr.data = NULL;
|
|
free_block(&nghdr);
|
|
return 1;
|
|
}
|
|
|
|
free_block(&nghdr);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
rv = pcap_read_pktheader(ctx, out_hdr);
|
|
if (rv != 1)
|
|
{return rv; }
|
|
|
|
// Otherwise we now know how long our packet is ..
|
|
(*out_data) = (uint8_t*)malloc(out_hdr->incl_len);
|
|
|
|
if (!(*out_data))
|
|
{
|
|
// Out of memory.
|
|
return -3;
|
|
}
|
|
|
|
(*out_len) = out_hdr->incl_len;
|
|
|
|
rv = fread((*out_data), (*out_len), 1, ctx->file);
|
|
if (rv != 1)
|
|
{
|
|
free(*out_data); (*out_data) = NULL;
|
|
*out_len = 0;
|
|
|
|
if (feof(ctx->file))
|
|
{
|
|
// Ah. EOF.
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
// Error. Curses.
|
|
return rv;
|
|
}
|
|
}
|
|
else if (rv == 1)
|
|
{
|
|
// Gotcha.
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int pcap_close(PCAP_reader_p *const pctx)
|
|
{
|
|
PCAP_reader_p ctx = *pctx;
|
|
|
|
if (ctx == NULL)
|
|
return 0;
|
|
|
|
if (ctx->interfaces != NULL)
|
|
{
|
|
free(ctx->interfaces);
|
|
}
|
|
if (ctx->file != NULL)
|
|
{
|
|
fclose(ctx->file);
|
|
}
|
|
free(ctx);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// Local Variables:
|
|
// tab-width: 8
|
|
// indent-tabs-mode: nil
|
|
// c-basic-offset: 2
|
|
// End:
|
|
// vim: set tabstop=8 shiftwidth=2 expandtab:
|
|
|