mirror
/
F5OEO-tstools
kopia lustrzana https://github.com/F5OEO/tstools
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
F5OEO-tstools/pcap.c

679 wiersze
15 KiB
C
Czysty Bezpośredni odnośnik Wina Historia

/* 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:
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik