~ubuntu-branches/ubuntu/wily/sflphone/wily

/* $Id: pcap.c 4537 2013-06-19 06:47:43Z riza $ */
/* 
 * Copyright (C) 2008-2011 Teluu Inc. (http://www.teluu.com)
 * Copyright (C) 2003-2008 Benny Prijono <benny@prijono.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
 */
#include <pjlib-util/pcap.h>
#include <pj/assert.h>
#include <pj/errno.h>
#include <pj/file_io.h>
#include <pj/log.h>
#include <pj/pool.h>
#include <pj/sock.h>
#include <pj/string.h>

#if 0
#   define TRACE_(x)	PJ_LOG(5,x)
#else
#   define TRACE_(x)
#endif


#pragma pack(1)

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;

#if 0
/* gcc insisted on aligning this struct to 32bit on ARM */
typedef struct pj_pcap_eth_hdr 
{
    pj_uint8_t  dest[6];
    pj_uint8_t  src[6];
    pj_uint8_t  len[2];
} pj_pcap_eth_hdr;
#else
typedef pj_uint8_t pj_pcap_eth_hdr[14];
#endif

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;

/* Implementation of pcap file */
struct pj_pcap_file
{
    char	    obj_name[PJ_MAX_OBJ_NAME];
    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);

    /* More sanity checks */
    TRACE_(("pcap", "sizeof(pj_pcap_eth_hdr)=%d",
	    sizeof(pj_pcap_eth_hdr)));
    PJ_ASSERT_RETURN(sizeof(pj_pcap_eth_hdr)==14, PJ_EBUG);
    TRACE_(("pcap", "sizeof(pj_pcap_ip_hdr)=%d",
	    sizeof(pj_pcap_ip_hdr)));
    PJ_ASSERT_RETURN(sizeof(pj_pcap_ip_hdr)==20, PJ_EBUG);
    TRACE_(("pcap", "sizeof(pj_pcap_udp_hdr)=%d",
	    sizeof(pj_pcap_udp_hdr)));
    PJ_ASSERT_RETURN(sizeof(pj_pcap_udp_hdr)==8, PJ_EBUG);
    
    file = PJ_POOL_ZALLOC_T(pool, pj_pcap_file);

    pj_ansi_strcpy(file->obj_name, "pcap");

    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;
    }

    TRACE_((file->obj_name, "PCAP file %s opened", path));
    
    *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);
    TRACE_((file->obj_name, "PCAP file closed"));
    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_pcap_udp_hdr *udp_hdr,
				     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;
	pj_size_t sz_read = 0;
	pj_status_t status;

	TRACE_((file->obj_name, "Reading packet.."));

	/* Read PCAP packet header */
	sz = sizeof(tmp.rec);
	status = read_file(file, &tmp.rec, &sz); 
	if (status != PJ_SUCCESS) {
	    TRACE_((file->obj_name, "read_file() error: %d", status));
	    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:
	    TRACE_((file->obj_name, "Error: link layer not Ethernet"));
	    return PJ_ENOTSUP;
	}

	if (status != PJ_SUCCESS) {
	    TRACE_((file->obj_name, "Error reading Eth header: %d", status));
	    return status;
	}

	sz_read += sz;
	    
	/* Read IP header */
	sz = sizeof(tmp.ip);
	status = read_file(file, &tmp.ip, &sz);
	if (status != PJ_SUCCESS) {
	    TRACE_((file->obj_name, "Error reading IP header: %d", status));
	    return status;
	}

	sz_read += sz;

	/* Skip if IP source mismatch */
	if (file->filter.ip_src && tmp.ip.ip_src != file->filter.ip_src) {
	    TRACE_((file->obj_name, "IP source %s mismatch, skipping", 
		    pj_inet_ntoa(*(pj_in_addr*)&tmp.ip.ip_src)));
	    SKIP_PKT();
	    continue;
	}

	/* Skip if IP destination mismatch */
	if (file->filter.ip_dst && tmp.ip.ip_dst != file->filter.ip_dst) {
	    TRACE_((file->obj_name, "IP detination %s mismatch, skipping", 
		    pj_inet_ntoa(*(pj_in_addr*)&tmp.ip.ip_dst)));
	    SKIP_PKT();
	    continue;
	}

	/* Skip if proto mismatch */
	if (file->filter.proto && tmp.ip.proto != file->filter.proto) {
	    TRACE_((file->obj_name, "IP proto %d mismatch, skipping", 
		    tmp.ip.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) {
		TRACE_((file->obj_name, "Error reading UDP header: %d",status));
		return status;
	    }

	    sz_read += sz;

	    /* Skip if source port mismatch */
	    if (file->filter.src_port && 
	        tmp.udp.src_port != file->filter.src_port) 
	    {
		TRACE_((file->obj_name, "UDP src port %d mismatch, skipping", 
			pj_ntohs(tmp.udp.src_port)));
		SKIP_PKT();
		continue;
	    }

	    /* Skip if destination port mismatch */
	    if (file->filter.dst_port && 
		tmp.udp.dst_port != file->filter.dst_port) 
	    {
		TRACE_((file->obj_name, "UDP dst port %d mismatch, skipping", 
			pj_ntohs(tmp.udp.dst_port)));
		SKIP_PKT();
		continue;
	    }

	    /* Copy UDP header if caller wants it */
	    if (udp_hdr) {
		pj_memcpy(udp_hdr, &tmp.udp, sizeof(*udp_hdr));
	    }

	    /* Calculate payload size */
	    sz = pj_ntohs(tmp.udp.len) - sizeof(tmp.udp);
	    break;
	default:
	    TRACE_((file->obj_name, "Not UDP, skipping"));
	    SKIP_PKT();
	    continue;
	}

	/* Check if payload fits the buffer */
	if (sz > (pj_ssize_t)*udp_payload_size) {
	    TRACE_((file->obj_name, 
		    "Error: packet too large (%d bytes required)", sz));
	    SKIP_PKT();
	    return PJ_ETOOSMALL;
	}

	/* Read the payload */
	status = read_file(file, udp_payload, &sz);
	if (status != PJ_SUCCESS) {
	    TRACE_((file->obj_name, "Error reading payload: %d", status));
	    return status;
	}

	sz_read += sz;

	*udp_payload_size = sz;

	// Some layers may have trailer, e.g: link eth2.
	/* Check that we've read all the packets */
	//PJ_ASSERT_RETURN(sz_read == rec_incl, PJ_EBUG);

	/* Skip trailer */
	while (sz_read < rec_incl) {
	    sz = rec_incl - sz_read;
	    status = read_file(file, &tmp.eth, &sz);
	    if (status != PJ_SUCCESS) {
		TRACE_((file->obj_name, "Error reading trailer: %d", status));
		return status;
	    }
	    sz_read += sz;
	}

	return PJ_SUCCESS;
    }

    /* Does not reach here */
}