Linux Audio

Check our new training course

Loading...
Note: File does not exist in v5.4.
   1/*
   2 * Minimal BPF debugger
   3 *
   4 * Minimal BPF debugger that mimics the kernel's engine (w/o extensions)
   5 * and allows for single stepping through selected packets from a pcap
   6 * with a provided user filter in order to facilitate verification of a
   7 * BPF program. Besides others, this is useful to verify BPF programs
   8 * before attaching to a live system, and can be used in socket filters,
   9 * cls_bpf, xt_bpf, team driver and e.g. PTP code; in particular when a
  10 * single more complex BPF program is being used. Reasons for a more
  11 * complex BPF program are likely primarily to optimize execution time
  12 * for making a verdict when multiple simple BPF programs are combined
  13 * into one in order to prevent parsing same headers multiple times.
  14 *
  15 * More on how to debug BPF opcodes see Documentation/networking/filter.txt
  16 * which is the main document on BPF. Mini howto for getting started:
  17 *
  18 *  1) `./bpf_dbg` to enter the shell (shell cmds denoted with '>'):
  19 *  2) > load bpf 6,40 0 0 12,21 0 3 20... (output from `bpf_asm` or
  20 *     `tcpdump -iem1 -ddd port 22 | tr '\n' ','` to load as filter)
  21 *  3) > load pcap foo.pcap
  22 *  4) > run <n>/disassemble/dump/quit (self-explanatory)
  23 *  5) > breakpoint 2 (sets bp at loaded BPF insns 2, do `run` then;
  24 *       multiple bps can be set, of course, a call to `breakpoint`
  25 *       w/o args shows currently loaded bps, `breakpoint reset` for
  26 *       resetting all breakpoints)
  27 *  6) > select 3 (`run` etc will start from the 3rd packet in the pcap)
  28 *  7) > step [-<n>, +<n>] (performs single stepping through the BPF)
  29 *
  30 * Copyright 2013 Daniel Borkmann <borkmann@redhat.com>
  31 * Licensed under the GNU General Public License, version 2.0 (GPLv2)
  32 */
  33
  34#include <stdio.h>
  35#include <unistd.h>
  36#include <stdlib.h>
  37#include <ctype.h>
  38#include <stdbool.h>
  39#include <stdarg.h>
  40#include <setjmp.h>
  41#include <linux/filter.h>
  42#include <linux/if_packet.h>
  43#include <readline/readline.h>
  44#include <readline/history.h>
  45#include <sys/types.h>
  46#include <sys/socket.h>
  47#include <sys/stat.h>
  48#include <sys/mman.h>
  49#include <fcntl.h>
  50#include <errno.h>
  51#include <signal.h>
  52#include <arpa/inet.h>
  53#include <net/ethernet.h>
  54
  55#define TCPDUMP_MAGIC	0xa1b2c3d4
  56
  57#define BPF_LDX_B	(BPF_LDX | BPF_B)
  58#define BPF_LDX_W	(BPF_LDX | BPF_W)
  59#define BPF_JMP_JA	(BPF_JMP | BPF_JA)
  60#define BPF_JMP_JEQ	(BPF_JMP | BPF_JEQ)
  61#define BPF_JMP_JGT	(BPF_JMP | BPF_JGT)
  62#define BPF_JMP_JGE	(BPF_JMP | BPF_JGE)
  63#define BPF_JMP_JSET	(BPF_JMP | BPF_JSET)
  64#define BPF_ALU_ADD	(BPF_ALU | BPF_ADD)
  65#define BPF_ALU_SUB	(BPF_ALU | BPF_SUB)
  66#define BPF_ALU_MUL	(BPF_ALU | BPF_MUL)
  67#define BPF_ALU_DIV	(BPF_ALU | BPF_DIV)
  68#define BPF_ALU_MOD	(BPF_ALU | BPF_MOD)
  69#define BPF_ALU_NEG	(BPF_ALU | BPF_NEG)
  70#define BPF_ALU_AND	(BPF_ALU | BPF_AND)
  71#define BPF_ALU_OR	(BPF_ALU | BPF_OR)
  72#define BPF_ALU_XOR	(BPF_ALU | BPF_XOR)
  73#define BPF_ALU_LSH	(BPF_ALU | BPF_LSH)
  74#define BPF_ALU_RSH	(BPF_ALU | BPF_RSH)
  75#define BPF_MISC_TAX	(BPF_MISC | BPF_TAX)
  76#define BPF_MISC_TXA	(BPF_MISC | BPF_TXA)
  77#define BPF_LD_B	(BPF_LD | BPF_B)
  78#define BPF_LD_H	(BPF_LD | BPF_H)
  79#define BPF_LD_W	(BPF_LD | BPF_W)
  80
  81#ifndef array_size
  82# define array_size(x)	(sizeof(x) / sizeof((x)[0]))
  83#endif
  84
  85#ifndef __check_format_printf
  86# define __check_format_printf(pos_fmtstr, pos_fmtargs) \
  87	__attribute__ ((format (printf, (pos_fmtstr), (pos_fmtargs))))
  88#endif
  89
  90enum {
  91	CMD_OK,
  92	CMD_ERR,
  93	CMD_EX,
  94};
  95
  96struct shell_cmd {
  97	const char *name;
  98	int (*func)(char *args);
  99};
 100
 101struct pcap_filehdr {
 102	uint32_t magic;
 103	uint16_t version_major;
 104	uint16_t version_minor;
 105	int32_t  thiszone;
 106	uint32_t sigfigs;
 107	uint32_t snaplen;
 108	uint32_t linktype;
 109};
 110
 111struct pcap_timeval {
 112	int32_t tv_sec;
 113	int32_t tv_usec;
 114};
 115
 116struct pcap_pkthdr {
 117	struct pcap_timeval ts;
 118	uint32_t caplen;
 119	uint32_t len;
 120};
 121
 122struct bpf_regs {
 123	uint32_t A;
 124	uint32_t X;
 125	uint32_t M[BPF_MEMWORDS];
 126	uint32_t R;
 127	bool     Rs;
 128	uint16_t Pc;
 129};
 130
 131static struct sock_filter bpf_image[BPF_MAXINSNS + 1];
 132static unsigned int bpf_prog_len = 0;
 133
 134static int bpf_breakpoints[64];
 135static struct bpf_regs bpf_regs[BPF_MAXINSNS + 1];
 136static struct bpf_regs bpf_curr;
 137static unsigned int bpf_regs_len = 0;
 138
 139static int pcap_fd = -1;
 140static unsigned int pcap_packet = 0;
 141static size_t pcap_map_size = 0;
 142static char *pcap_ptr_va_start, *pcap_ptr_va_curr;
 143
 144static const char * const op_table[] = {
 145	[BPF_ST]	= "st",
 146	[BPF_STX]	= "stx",
 147	[BPF_LD_B]	= "ldb",
 148	[BPF_LD_H]	= "ldh",
 149	[BPF_LD_W]	= "ld",
 150	[BPF_LDX]	= "ldx",
 151	[BPF_LDX_B]	= "ldxb",
 152	[BPF_JMP_JA]	= "ja",
 153	[BPF_JMP_JEQ]	= "jeq",
 154	[BPF_JMP_JGT]	= "jgt",
 155	[BPF_JMP_JGE]	= "jge",
 156	[BPF_JMP_JSET]	= "jset",
 157	[BPF_ALU_ADD]	= "add",
 158	[BPF_ALU_SUB]	= "sub",
 159	[BPF_ALU_MUL]	= "mul",
 160	[BPF_ALU_DIV]	= "div",
 161	[BPF_ALU_MOD]	= "mod",
 162	[BPF_ALU_NEG]	= "neg",
 163	[BPF_ALU_AND]	= "and",
 164	[BPF_ALU_OR]	= "or",
 165	[BPF_ALU_XOR]	= "xor",
 166	[BPF_ALU_LSH]	= "lsh",
 167	[BPF_ALU_RSH]	= "rsh",
 168	[BPF_MISC_TAX]	= "tax",
 169	[BPF_MISC_TXA]	= "txa",
 170	[BPF_RET]	= "ret",
 171};
 172
 173static __check_format_printf(1, 2) int rl_printf(const char *fmt, ...)
 174{
 175	int ret;
 176	va_list vl;
 177
 178	va_start(vl, fmt);
 179	ret = vfprintf(rl_outstream, fmt, vl);
 180	va_end(vl);
 181
 182	return ret;
 183}
 184
 185static int matches(const char *cmd, const char *pattern)
 186{
 187	int len = strlen(cmd);
 188
 189	if (len > strlen(pattern))
 190		return -1;
 191
 192	return memcmp(pattern, cmd, len);
 193}
 194
 195static void hex_dump(const uint8_t *buf, size_t len)
 196{
 197	int i;
 198
 199	rl_printf("%3u: ", 0);
 200	for (i = 0; i < len; i++) {
 201		if (i && !(i % 16))
 202			rl_printf("\n%3u: ", i);
 203		rl_printf("%02x ", buf[i]);
 204	}
 205	rl_printf("\n");
 206}
 207
 208static bool bpf_prog_loaded(void)
 209{
 210	if (bpf_prog_len == 0)
 211		rl_printf("no bpf program loaded!\n");
 212
 213	return bpf_prog_len > 0;
 214}
 215
 216static void bpf_disasm(const struct sock_filter f, unsigned int i)
 217{
 218	const char *op, *fmt;
 219	int val = f.k;
 220	char buf[256];
 221
 222	switch (f.code) {
 223	case BPF_RET | BPF_K:
 224		op = op_table[BPF_RET];
 225		fmt = "#%#x";
 226		break;
 227	case BPF_RET | BPF_A:
 228		op = op_table[BPF_RET];
 229		fmt = "a";
 230		break;
 231	case BPF_RET | BPF_X:
 232		op = op_table[BPF_RET];
 233		fmt = "x";
 234		break;
 235	case BPF_MISC_TAX:
 236		op = op_table[BPF_MISC_TAX];
 237		fmt = "";
 238		break;
 239	case BPF_MISC_TXA:
 240		op = op_table[BPF_MISC_TXA];
 241		fmt = "";
 242		break;
 243	case BPF_ST:
 244		op = op_table[BPF_ST];
 245		fmt = "M[%d]";
 246		break;
 247	case BPF_STX:
 248		op = op_table[BPF_STX];
 249		fmt = "M[%d]";
 250		break;
 251	case BPF_LD_W | BPF_ABS:
 252		op = op_table[BPF_LD_W];
 253		fmt = "[%d]";
 254		break;
 255	case BPF_LD_H | BPF_ABS:
 256		op = op_table[BPF_LD_H];
 257		fmt = "[%d]";
 258		break;
 259	case BPF_LD_B | BPF_ABS:
 260		op = op_table[BPF_LD_B];
 261		fmt = "[%d]";
 262		break;
 263	case BPF_LD_W | BPF_LEN:
 264		op = op_table[BPF_LD_W];
 265		fmt = "#len";
 266		break;
 267	case BPF_LD_W | BPF_IND:
 268		op = op_table[BPF_LD_W];
 269		fmt = "[x+%d]";
 270		break;
 271	case BPF_LD_H | BPF_IND:
 272		op = op_table[BPF_LD_H];
 273		fmt = "[x+%d]";
 274		break;
 275	case BPF_LD_B | BPF_IND:
 276		op = op_table[BPF_LD_B];
 277		fmt = "[x+%d]";
 278		break;
 279	case BPF_LD | BPF_IMM:
 280		op = op_table[BPF_LD_W];
 281		fmt = "#%#x";
 282		break;
 283	case BPF_LDX | BPF_IMM:
 284		op = op_table[BPF_LDX];
 285		fmt = "#%#x";
 286		break;
 287	case BPF_LDX_B | BPF_MSH:
 288		op = op_table[BPF_LDX_B];
 289		fmt = "4*([%d]&0xf)";
 290		break;
 291	case BPF_LD | BPF_MEM:
 292		op = op_table[BPF_LD_W];
 293		fmt = "M[%d]";
 294		break;
 295	case BPF_LDX | BPF_MEM:
 296		op = op_table[BPF_LDX];
 297		fmt = "M[%d]";
 298		break;
 299	case BPF_JMP_JA:
 300		op = op_table[BPF_JMP_JA];
 301		fmt = "%d";
 302		val = i + 1 + f.k;
 303		break;
 304	case BPF_JMP_JGT | BPF_X:
 305		op = op_table[BPF_JMP_JGT];
 306		fmt = "x";
 307		break;
 308	case BPF_JMP_JGT | BPF_K:
 309		op = op_table[BPF_JMP_JGT];
 310		fmt = "#%#x";
 311		break;
 312	case BPF_JMP_JGE | BPF_X:
 313		op = op_table[BPF_JMP_JGE];
 314		fmt = "x";
 315		break;
 316	case BPF_JMP_JGE | BPF_K:
 317		op = op_table[BPF_JMP_JGE];
 318		fmt = "#%#x";
 319		break;
 320	case BPF_JMP_JEQ | BPF_X:
 321		op = op_table[BPF_JMP_JEQ];
 322		fmt = "x";
 323		break;
 324	case BPF_JMP_JEQ | BPF_K:
 325		op = op_table[BPF_JMP_JEQ];
 326		fmt = "#%#x";
 327		break;
 328	case BPF_JMP_JSET | BPF_X:
 329		op = op_table[BPF_JMP_JSET];
 330		fmt = "x";
 331		break;
 332	case BPF_JMP_JSET | BPF_K:
 333		op = op_table[BPF_JMP_JSET];
 334		fmt = "#%#x";
 335		break;
 336	case BPF_ALU_NEG:
 337		op = op_table[BPF_ALU_NEG];
 338		fmt = "";
 339		break;
 340	case BPF_ALU_LSH | BPF_X:
 341		op = op_table[BPF_ALU_LSH];
 342		fmt = "x";
 343		break;
 344	case BPF_ALU_LSH | BPF_K:
 345		op = op_table[BPF_ALU_LSH];
 346		fmt = "#%d";
 347		break;
 348	case BPF_ALU_RSH | BPF_X:
 349		op = op_table[BPF_ALU_RSH];
 350		fmt = "x";
 351		break;
 352	case BPF_ALU_RSH | BPF_K:
 353		op = op_table[BPF_ALU_RSH];
 354		fmt = "#%d";
 355		break;
 356	case BPF_ALU_ADD | BPF_X:
 357		op = op_table[BPF_ALU_ADD];
 358		fmt = "x";
 359		break;
 360	case BPF_ALU_ADD | BPF_K:
 361		op = op_table[BPF_ALU_ADD];
 362		fmt = "#%d";
 363		break;
 364	case BPF_ALU_SUB | BPF_X:
 365		op = op_table[BPF_ALU_SUB];
 366		fmt = "x";
 367		break;
 368	case BPF_ALU_SUB | BPF_K:
 369		op = op_table[BPF_ALU_SUB];
 370		fmt = "#%d";
 371		break;
 372	case BPF_ALU_MUL | BPF_X:
 373		op = op_table[BPF_ALU_MUL];
 374		fmt = "x";
 375		break;
 376	case BPF_ALU_MUL | BPF_K:
 377		op = op_table[BPF_ALU_MUL];
 378		fmt = "#%d";
 379		break;
 380	case BPF_ALU_DIV | BPF_X:
 381		op = op_table[BPF_ALU_DIV];
 382		fmt = "x";
 383		break;
 384	case BPF_ALU_DIV | BPF_K:
 385		op = op_table[BPF_ALU_DIV];
 386		fmt = "#%d";
 387		break;
 388	case BPF_ALU_MOD | BPF_X:
 389		op = op_table[BPF_ALU_MOD];
 390		fmt = "x";
 391		break;
 392	case BPF_ALU_MOD | BPF_K:
 393		op = op_table[BPF_ALU_MOD];
 394		fmt = "#%d";
 395		break;
 396	case BPF_ALU_AND | BPF_X:
 397		op = op_table[BPF_ALU_AND];
 398		fmt = "x";
 399		break;
 400	case BPF_ALU_AND | BPF_K:
 401		op = op_table[BPF_ALU_AND];
 402		fmt = "#%#x";
 403		break;
 404	case BPF_ALU_OR | BPF_X:
 405		op = op_table[BPF_ALU_OR];
 406		fmt = "x";
 407		break;
 408	case BPF_ALU_OR | BPF_K:
 409		op = op_table[BPF_ALU_OR];
 410		fmt = "#%#x";
 411		break;
 412	case BPF_ALU_XOR | BPF_X:
 413		op = op_table[BPF_ALU_XOR];
 414		fmt = "x";
 415		break;
 416	case BPF_ALU_XOR | BPF_K:
 417		op = op_table[BPF_ALU_XOR];
 418		fmt = "#%#x";
 419		break;
 420	default:
 421		op = "nosup";
 422		fmt = "%#x";
 423		val = f.code;
 424		break;
 425	}
 426
 427	memset(buf, 0, sizeof(buf));
 428	snprintf(buf, sizeof(buf), fmt, val);
 429	buf[sizeof(buf) - 1] = 0;
 430
 431	if ((BPF_CLASS(f.code) == BPF_JMP && BPF_OP(f.code) != BPF_JA))
 432		rl_printf("l%d:\t%s %s, l%d, l%d\n", i, op, buf,
 433			  i + 1 + f.jt, i + 1 + f.jf);
 434	else
 435		rl_printf("l%d:\t%s %s\n", i, op, buf);
 436}
 437
 438static void bpf_dump_curr(struct bpf_regs *r, struct sock_filter *f)
 439{
 440	int i, m = 0;
 441
 442	rl_printf("pc:       [%u]\n", r->Pc);
 443	rl_printf("code:     [%u] jt[%u] jf[%u] k[%u]\n",
 444		  f->code, f->jt, f->jf, f->k);
 445	rl_printf("curr:     ");
 446	bpf_disasm(*f, r->Pc);
 447
 448	if (f->jt || f->jf) {
 449		rl_printf("jt:       ");
 450		bpf_disasm(*(f + f->jt + 1), r->Pc + f->jt + 1);
 451		rl_printf("jf:       ");
 452		bpf_disasm(*(f + f->jf + 1), r->Pc + f->jf + 1);
 453	}
 454
 455	rl_printf("A:        [%#08x][%u]\n", r->A, r->A);
 456	rl_printf("X:        [%#08x][%u]\n", r->X, r->X);
 457	if (r->Rs)
 458		rl_printf("ret:      [%#08x][%u]!\n", r->R, r->R);
 459
 460	for (i = 0; i < BPF_MEMWORDS; i++) {
 461		if (r->M[i]) {
 462			m++;
 463			rl_printf("M[%d]: [%#08x][%u]\n", i, r->M[i], r->M[i]);
 464		}
 465	}
 466	if (m == 0)
 467		rl_printf("M[0,%d]:  [%#08x][%u]\n", BPF_MEMWORDS - 1, 0, 0);
 468}
 469
 470static void bpf_dump_pkt(uint8_t *pkt, uint32_t pkt_caplen, uint32_t pkt_len)
 471{
 472	if (pkt_caplen != pkt_len)
 473		rl_printf("cap: %u, len: %u\n", pkt_caplen, pkt_len);
 474	else
 475		rl_printf("len: %u\n", pkt_len);
 476
 477	hex_dump(pkt, pkt_caplen);
 478}
 479
 480static void bpf_disasm_all(const struct sock_filter *f, unsigned int len)
 481{
 482	unsigned int i;
 483
 484	for (i = 0; i < len; i++)
 485		bpf_disasm(f[i], i);
 486}
 487
 488static void bpf_dump_all(const struct sock_filter *f, unsigned int len)
 489{
 490	unsigned int i;
 491
 492	rl_printf("/* { op, jt, jf, k }, */\n");
 493	for (i = 0; i < len; i++)
 494		rl_printf("{ %#04x, %2u, %2u, %#010x },\n",
 495			  f[i].code, f[i].jt, f[i].jf, f[i].k);
 496}
 497
 498static bool bpf_runnable(struct sock_filter *f, unsigned int len)
 499{
 500	int sock, ret, i;
 501	struct sock_fprog bpf = {
 502		.filter = f,
 503		.len = len,
 504	};
 505
 506	sock = socket(AF_INET, SOCK_DGRAM, 0);
 507	if (sock < 0) {
 508		rl_printf("cannot open socket!\n");
 509		return false;
 510	}
 511	ret = setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &bpf, sizeof(bpf));
 512	close(sock);
 513	if (ret < 0) {
 514		rl_printf("program not allowed to run by kernel!\n");
 515		return false;
 516	}
 517	for (i = 0; i < len; i++) {
 518		if (BPF_CLASS(f[i].code) == BPF_LD &&
 519		    f[i].k > SKF_AD_OFF) {
 520			rl_printf("extensions currently not supported!\n");
 521			return false;
 522		}
 523	}
 524
 525	return true;
 526}
 527
 528static void bpf_reset_breakpoints(void)
 529{
 530	int i;
 531
 532	for (i = 0; i < array_size(bpf_breakpoints); i++)
 533		bpf_breakpoints[i] = -1;
 534}
 535
 536static void bpf_set_breakpoints(unsigned int where)
 537{
 538	int i;
 539	bool set = false;
 540
 541	for (i = 0; i < array_size(bpf_breakpoints); i++) {
 542		if (bpf_breakpoints[i] == (int) where) {
 543			rl_printf("breakpoint already set!\n");
 544			set = true;
 545			break;
 546		}
 547
 548		if (bpf_breakpoints[i] == -1 && set == false) {
 549			bpf_breakpoints[i] = where;
 550			set = true;
 551		}
 552	}
 553
 554	if (!set)
 555		rl_printf("too many breakpoints set, reset first!\n");
 556}
 557
 558static void bpf_dump_breakpoints(void)
 559{
 560	int i;
 561
 562	rl_printf("breakpoints: ");
 563
 564	for (i = 0; i < array_size(bpf_breakpoints); i++) {
 565		if (bpf_breakpoints[i] < 0)
 566			continue;
 567		rl_printf("%d ", bpf_breakpoints[i]);
 568	}
 569
 570	rl_printf("\n");
 571}
 572
 573static void bpf_reset(void)
 574{
 575	bpf_regs_len = 0;
 576
 577	memset(bpf_regs, 0, sizeof(bpf_regs));
 578	memset(&bpf_curr, 0, sizeof(bpf_curr));
 579}
 580
 581static void bpf_safe_regs(void)
 582{
 583	memcpy(&bpf_regs[bpf_regs_len++], &bpf_curr, sizeof(bpf_curr));
 584}
 585
 586static bool bpf_restore_regs(int off)
 587{
 588	unsigned int index = bpf_regs_len - 1 + off;
 589
 590	if (index == 0) {
 591		bpf_reset();
 592		return true;
 593	} else if (index < bpf_regs_len) {
 594		memcpy(&bpf_curr, &bpf_regs[index], sizeof(bpf_curr));
 595		bpf_regs_len = index;
 596		return true;
 597	} else {
 598		rl_printf("reached bottom of register history stack!\n");
 599		return false;
 600	}
 601}
 602
 603static uint32_t extract_u32(uint8_t *pkt, uint32_t off)
 604{
 605	uint32_t r;
 606
 607	memcpy(&r, &pkt[off], sizeof(r));
 608
 609	return ntohl(r);
 610}
 611
 612static uint16_t extract_u16(uint8_t *pkt, uint32_t off)
 613{
 614	uint16_t r;
 615
 616	memcpy(&r, &pkt[off], sizeof(r));
 617
 618	return ntohs(r);
 619}
 620
 621static uint8_t extract_u8(uint8_t *pkt, uint32_t off)
 622{
 623	return pkt[off];
 624}
 625
 626static void set_return(struct bpf_regs *r)
 627{
 628	r->R = 0;
 629	r->Rs = true;
 630}
 631
 632static void bpf_single_step(struct bpf_regs *r, struct sock_filter *f,
 633			    uint8_t *pkt, uint32_t pkt_caplen,
 634			    uint32_t pkt_len)
 635{
 636	uint32_t K = f->k;
 637	int d;
 638
 639	switch (f->code) {
 640	case BPF_RET | BPF_K:
 641		r->R = K;
 642		r->Rs = true;
 643		break;
 644	case BPF_RET | BPF_A:
 645		r->R = r->A;
 646		r->Rs = true;
 647		break;
 648	case BPF_RET | BPF_X:
 649		r->R = r->X;
 650		r->Rs = true;
 651		break;
 652	case BPF_MISC_TAX:
 653		r->X = r->A;
 654		break;
 655	case BPF_MISC_TXA:
 656		r->A = r->X;
 657		break;
 658	case BPF_ST:
 659		r->M[K] = r->A;
 660		break;
 661	case BPF_STX:
 662		r->M[K] = r->X;
 663		break;
 664	case BPF_LD_W | BPF_ABS:
 665		d = pkt_caplen - K;
 666		if (d >= sizeof(uint32_t))
 667			r->A = extract_u32(pkt, K);
 668		else
 669			set_return(r);
 670		break;
 671	case BPF_LD_H | BPF_ABS:
 672		d = pkt_caplen - K;
 673		if (d >= sizeof(uint16_t))
 674			r->A = extract_u16(pkt, K);
 675		else
 676			set_return(r);
 677		break;
 678	case BPF_LD_B | BPF_ABS:
 679		d = pkt_caplen - K;
 680		if (d >= sizeof(uint8_t))
 681			r->A = extract_u8(pkt, K);
 682		else
 683			set_return(r);
 684		break;
 685	case BPF_LD_W | BPF_IND:
 686		d = pkt_caplen - (r->X + K);
 687		if (d >= sizeof(uint32_t))
 688			r->A = extract_u32(pkt, r->X + K);
 689		break;
 690	case BPF_LD_H | BPF_IND:
 691		d = pkt_caplen - (r->X + K);
 692		if (d >= sizeof(uint16_t))
 693			r->A = extract_u16(pkt, r->X + K);
 694		else
 695			set_return(r);
 696		break;
 697	case BPF_LD_B | BPF_IND:
 698		d = pkt_caplen - (r->X + K);
 699		if (d >= sizeof(uint8_t))
 700			r->A = extract_u8(pkt, r->X + K);
 701		else
 702			set_return(r);
 703		break;
 704	case BPF_LDX_B | BPF_MSH:
 705		d = pkt_caplen - K;
 706		if (d >= sizeof(uint8_t)) {
 707			r->X = extract_u8(pkt, K);
 708			r->X = (r->X & 0xf) << 2;
 709		} else
 710			set_return(r);
 711		break;
 712	case BPF_LD_W | BPF_LEN:
 713		r->A = pkt_len;
 714		break;
 715	case BPF_LDX_W | BPF_LEN:
 716		r->A = pkt_len;
 717		break;
 718	case BPF_LD | BPF_IMM:
 719		r->A = K;
 720		break;
 721	case BPF_LDX | BPF_IMM:
 722		r->X = K;
 723		break;
 724	case BPF_LD | BPF_MEM:
 725		r->A = r->M[K];
 726		break;
 727	case BPF_LDX | BPF_MEM:
 728		r->X = r->M[K];
 729		break;
 730	case BPF_JMP_JA:
 731		r->Pc += K;
 732		break;
 733	case BPF_JMP_JGT | BPF_X:
 734		r->Pc += r->A > r->X ? f->jt : f->jf;
 735		break;
 736	case BPF_JMP_JGT | BPF_K:
 737		r->Pc += r->A > K ? f->jt : f->jf;
 738		break;
 739	case BPF_JMP_JGE | BPF_X:
 740		r->Pc += r->A >= r->X ? f->jt : f->jf;
 741		break;
 742	case BPF_JMP_JGE | BPF_K:
 743		r->Pc += r->A >= K ? f->jt : f->jf;
 744		break;
 745	case BPF_JMP_JEQ | BPF_X:
 746		r->Pc += r->A == r->X ? f->jt : f->jf;
 747		break;
 748	case BPF_JMP_JEQ | BPF_K:
 749		r->Pc += r->A == K ? f->jt : f->jf;
 750		break;
 751	case BPF_JMP_JSET | BPF_X:
 752		r->Pc += r->A & r->X ? f->jt : f->jf;
 753		break;
 754	case BPF_JMP_JSET | BPF_K:
 755		r->Pc += r->A & K ? f->jt : f->jf;
 756		break;
 757	case BPF_ALU_NEG:
 758		r->A = -r->A;
 759		break;
 760	case BPF_ALU_LSH | BPF_X:
 761		r->A <<= r->X;
 762		break;
 763	case BPF_ALU_LSH | BPF_K:
 764		r->A <<= K;
 765		break;
 766	case BPF_ALU_RSH | BPF_X:
 767		r->A >>= r->X;
 768		break;
 769	case BPF_ALU_RSH | BPF_K:
 770		r->A >>= K;
 771		break;
 772	case BPF_ALU_ADD | BPF_X:
 773		r->A += r->X;
 774		break;
 775	case BPF_ALU_ADD | BPF_K:
 776		r->A += K;
 777		break;
 778	case BPF_ALU_SUB | BPF_X:
 779		r->A -= r->X;
 780		break;
 781	case BPF_ALU_SUB | BPF_K:
 782		r->A -= K;
 783		break;
 784	case BPF_ALU_MUL | BPF_X:
 785		r->A *= r->X;
 786		break;
 787	case BPF_ALU_MUL | BPF_K:
 788		r->A *= K;
 789		break;
 790	case BPF_ALU_DIV | BPF_X:
 791	case BPF_ALU_MOD | BPF_X:
 792		if (r->X == 0) {
 793			set_return(r);
 794			break;
 795		}
 796		goto do_div;
 797	case BPF_ALU_DIV | BPF_K:
 798	case BPF_ALU_MOD | BPF_K:
 799		if (K == 0) {
 800			set_return(r);
 801			break;
 802		}
 803do_div:
 804		switch (f->code) {
 805		case BPF_ALU_DIV | BPF_X:
 806			r->A /= r->X;
 807			break;
 808		case BPF_ALU_DIV | BPF_K:
 809			r->A /= K;
 810			break;
 811		case BPF_ALU_MOD | BPF_X:
 812			r->A %= r->X;
 813			break;
 814		case BPF_ALU_MOD | BPF_K:
 815			r->A %= K;
 816			break;
 817		}
 818		break;
 819	case BPF_ALU_AND | BPF_X:
 820		r->A &= r->X;
 821		break;
 822	case BPF_ALU_AND | BPF_K:
 823		r->A &= K;
 824		break;
 825	case BPF_ALU_OR | BPF_X:
 826		r->A |= r->X;
 827		break;
 828	case BPF_ALU_OR | BPF_K:
 829		r->A |= K;
 830		break;
 831	case BPF_ALU_XOR | BPF_X:
 832		r->A ^= r->X;
 833		break;
 834	case BPF_ALU_XOR | BPF_K:
 835		r->A ^= K;
 836		break;
 837	}
 838}
 839
 840static bool bpf_pc_has_breakpoint(uint16_t pc)
 841{
 842	int i;
 843
 844	for (i = 0; i < array_size(bpf_breakpoints); i++) {
 845		if (bpf_breakpoints[i] < 0)
 846			continue;
 847		if (bpf_breakpoints[i] == pc)
 848			return true;
 849	}
 850
 851	return false;
 852}
 853
 854static bool bpf_handle_breakpoint(struct bpf_regs *r, struct sock_filter *f,
 855				  uint8_t *pkt, uint32_t pkt_caplen,
 856				  uint32_t pkt_len)
 857{
 858	rl_printf("-- register dump --\n");
 859	bpf_dump_curr(r, &f[r->Pc]);
 860	rl_printf("-- packet dump --\n");
 861	bpf_dump_pkt(pkt, pkt_caplen, pkt_len);
 862	rl_printf("(breakpoint)\n");
 863	return true;
 864}
 865
 866static int bpf_run_all(struct sock_filter *f, uint16_t bpf_len, uint8_t *pkt,
 867		       uint32_t pkt_caplen, uint32_t pkt_len)
 868{
 869	bool stop = false;
 870
 871	while (bpf_curr.Rs == false && stop == false) {
 872		bpf_safe_regs();
 873
 874		if (bpf_pc_has_breakpoint(bpf_curr.Pc))
 875			stop = bpf_handle_breakpoint(&bpf_curr, f, pkt,
 876						     pkt_caplen, pkt_len);
 877
 878		bpf_single_step(&bpf_curr, &f[bpf_curr.Pc], pkt, pkt_caplen,
 879				pkt_len);
 880		bpf_curr.Pc++;
 881	}
 882
 883	return stop ? -1 : bpf_curr.R;
 884}
 885
 886static int bpf_run_stepping(struct sock_filter *f, uint16_t bpf_len,
 887			    uint8_t *pkt, uint32_t pkt_caplen,
 888			    uint32_t pkt_len, int next)
 889{
 890	bool stop = false;
 891	int i = 1;
 892
 893	while (bpf_curr.Rs == false && stop == false) {
 894		bpf_safe_regs();
 895
 896		if (i++ == next)
 897			stop = bpf_handle_breakpoint(&bpf_curr, f, pkt,
 898						     pkt_caplen, pkt_len);
 899
 900		bpf_single_step(&bpf_curr, &f[bpf_curr.Pc], pkt, pkt_caplen,
 901				pkt_len);
 902		bpf_curr.Pc++;
 903	}
 904
 905	return stop ? -1 : bpf_curr.R;
 906}
 907
 908static bool pcap_loaded(void)
 909{
 910	if (pcap_fd < 0)
 911		rl_printf("no pcap file loaded!\n");
 912
 913	return pcap_fd >= 0;
 914}
 915
 916static struct pcap_pkthdr *pcap_curr_pkt(void)
 917{
 918	return (void *) pcap_ptr_va_curr;
 919}
 920
 921static bool pcap_next_pkt(void)
 922{
 923	struct pcap_pkthdr *hdr = pcap_curr_pkt();
 924
 925	if (pcap_ptr_va_curr + sizeof(*hdr) -
 926	    pcap_ptr_va_start >= pcap_map_size)
 927		return false;
 928	if (hdr->caplen == 0 || hdr->len == 0 || hdr->caplen > hdr->len)
 929		return false;
 930	if (pcap_ptr_va_curr + sizeof(*hdr) + hdr->caplen -
 931	    pcap_ptr_va_start >= pcap_map_size)
 932		return false;
 933
 934	pcap_ptr_va_curr += (sizeof(*hdr) + hdr->caplen);
 935	return true;
 936}
 937
 938static void pcap_reset_pkt(void)
 939{
 940	pcap_ptr_va_curr = pcap_ptr_va_start + sizeof(struct pcap_filehdr);
 941}
 942
 943static int try_load_pcap(const char *file)
 944{
 945	struct pcap_filehdr *hdr;
 946	struct stat sb;
 947	int ret;
 948
 949	pcap_fd = open(file, O_RDONLY);
 950	if (pcap_fd < 0) {
 951		rl_printf("cannot open pcap [%s]!\n", strerror(errno));
 952		return CMD_ERR;
 953	}
 954
 955	ret = fstat(pcap_fd, &sb);
 956	if (ret < 0) {
 957		rl_printf("cannot fstat pcap file!\n");
 958		return CMD_ERR;
 959	}
 960
 961	if (!S_ISREG(sb.st_mode)) {
 962		rl_printf("not a regular pcap file, duh!\n");
 963		return CMD_ERR;
 964	}
 965
 966	pcap_map_size = sb.st_size;
 967	if (pcap_map_size <= sizeof(struct pcap_filehdr)) {
 968		rl_printf("pcap file too small!\n");
 969		return CMD_ERR;
 970	}
 971
 972	pcap_ptr_va_start = mmap(NULL, pcap_map_size, PROT_READ,
 973				 MAP_SHARED | MAP_LOCKED, pcap_fd, 0);
 974	if (pcap_ptr_va_start == MAP_FAILED) {
 975		rl_printf("mmap of file failed!");
 976		return CMD_ERR;
 977	}
 978
 979	hdr = (void *) pcap_ptr_va_start;
 980	if (hdr->magic != TCPDUMP_MAGIC) {
 981		rl_printf("wrong pcap magic!\n");
 982		return CMD_ERR;
 983	}
 984
 985	pcap_reset_pkt();
 986
 987	return CMD_OK;
 988
 989}
 990
 991static void try_close_pcap(void)
 992{
 993	if (pcap_fd >= 0) {
 994		munmap(pcap_ptr_va_start, pcap_map_size);
 995		close(pcap_fd);
 996
 997		pcap_ptr_va_start = pcap_ptr_va_curr = NULL;
 998		pcap_map_size = 0;
 999		pcap_packet = 0;
1000		pcap_fd = -1;
1001	}
1002}
1003
1004static int cmd_load_bpf(char *bpf_string)
1005{
1006	char sp, *token, separator = ',';
1007	unsigned short bpf_len, i = 0;
1008	struct sock_filter tmp;
1009
1010	bpf_prog_len = 0;
1011	memset(bpf_image, 0, sizeof(bpf_image));
1012
1013	if (sscanf(bpf_string, "%hu%c", &bpf_len, &sp) != 2 ||
1014	    sp != separator || bpf_len > BPF_MAXINSNS || bpf_len == 0) {
1015		rl_printf("syntax error in head length encoding!\n");
1016		return CMD_ERR;
1017	}
1018
1019	token = bpf_string;
1020	while ((token = strchr(token, separator)) && (++token)[0]) {
1021		if (i >= bpf_len) {
1022			rl_printf("program exceeds encoded length!\n");
1023			return CMD_ERR;
1024		}
1025
1026		if (sscanf(token, "%hu %hhu %hhu %u,",
1027			   &tmp.code, &tmp.jt, &tmp.jf, &tmp.k) != 4) {
1028			rl_printf("syntax error at instruction %d!\n", i);
1029			return CMD_ERR;
1030		}
1031
1032		bpf_image[i].code = tmp.code;
1033		bpf_image[i].jt = tmp.jt;
1034		bpf_image[i].jf = tmp.jf;
1035		bpf_image[i].k = tmp.k;
1036
1037		i++;
1038	}
1039
1040	if (i != bpf_len) {
1041		rl_printf("syntax error exceeding encoded length!\n");
1042		return CMD_ERR;
1043	} else
1044		bpf_prog_len = bpf_len;
1045	if (!bpf_runnable(bpf_image, bpf_prog_len))
1046		bpf_prog_len = 0;
1047
1048	return CMD_OK;
1049}
1050
1051static int cmd_load_pcap(char *file)
1052{
1053	char *file_trim, *tmp;
1054
1055	file_trim = strtok_r(file, " ", &tmp);
1056	if (file_trim == NULL)
1057		return CMD_ERR;
1058
1059	try_close_pcap();
1060
1061	return try_load_pcap(file_trim);
1062}
1063
1064static int cmd_load(char *arg)
1065{
1066	char *subcmd, *cont, *tmp = strdup(arg);
1067	int ret = CMD_OK;
1068
1069	subcmd = strtok_r(tmp, " ", &cont);
1070	if (subcmd == NULL)
1071		goto out;
1072	if (matches(subcmd, "bpf") == 0) {
1073		bpf_reset();
1074		bpf_reset_breakpoints();
1075
1076		ret = cmd_load_bpf(cont);
1077	} else if (matches(subcmd, "pcap") == 0) {
1078		ret = cmd_load_pcap(cont);
1079	} else {
1080out:
1081		rl_printf("bpf <code>:  load bpf code\n");
1082		rl_printf("pcap <file>: load pcap file\n");
1083		ret = CMD_ERR;
1084	}
1085
1086	free(tmp);
1087	return ret;
1088}
1089
1090static int cmd_step(char *num)
1091{
1092	struct pcap_pkthdr *hdr;
1093	int steps, ret;
1094
1095	if (!bpf_prog_loaded() || !pcap_loaded())
1096		return CMD_ERR;
1097
1098	steps = strtol(num, NULL, 10);
1099	if (steps == 0 || strlen(num) == 0)
1100		steps = 1;
1101	if (steps < 0) {
1102		if (!bpf_restore_regs(steps))
1103			return CMD_ERR;
1104		steps = 1;
1105	}
1106
1107	hdr = pcap_curr_pkt();
1108	ret = bpf_run_stepping(bpf_image, bpf_prog_len,
1109			       (uint8_t *) hdr + sizeof(*hdr),
1110			       hdr->caplen, hdr->len, steps);
1111	if (ret >= 0 || bpf_curr.Rs) {
1112		bpf_reset();
1113		if (!pcap_next_pkt()) {
1114			rl_printf("(going back to first packet)\n");
1115			pcap_reset_pkt();
1116		} else {
1117			rl_printf("(next packet)\n");
1118		}
1119	}
1120
1121	return CMD_OK;
1122}
1123
1124static int cmd_select(char *num)
1125{
1126	unsigned int which, i;
1127	bool have_next = true;
1128
1129	if (!pcap_loaded() || strlen(num) == 0)
1130		return CMD_ERR;
1131
1132	which = strtoul(num, NULL, 10);
1133	if (which == 0) {
1134		rl_printf("packet count starts with 1, clamping!\n");
1135		which = 1;
1136	}
1137
1138	pcap_reset_pkt();
1139	bpf_reset();
1140
1141	for (i = 0; i < which && (have_next = pcap_next_pkt()); i++)
1142		/* noop */;
1143	if (!have_next || pcap_curr_pkt() == NULL) {
1144		rl_printf("no packet #%u available!\n", which);
1145		pcap_reset_pkt();
1146		return CMD_ERR;
1147	}
1148
1149	return CMD_OK;
1150}
1151
1152static int cmd_breakpoint(char *subcmd)
1153{
1154	if (!bpf_prog_loaded())
1155		return CMD_ERR;
1156	if (strlen(subcmd) == 0)
1157		bpf_dump_breakpoints();
1158	else if (matches(subcmd, "reset") == 0)
1159		bpf_reset_breakpoints();
1160	else {
1161		unsigned int where = strtoul(subcmd, NULL, 10);
1162
1163		if (where < bpf_prog_len) {
1164			bpf_set_breakpoints(where);
1165			rl_printf("breakpoint at: ");
1166			bpf_disasm(bpf_image[where], where);
1167		}
1168	}
1169
1170	return CMD_OK;
1171}
1172
1173static int cmd_run(char *num)
1174{
1175	static uint32_t pass = 0, fail = 0;
1176	bool has_limit = true;
1177	int pkts = 0, i = 0;
1178
1179	if (!bpf_prog_loaded() || !pcap_loaded())
1180		return CMD_ERR;
1181
1182	pkts = strtol(num, NULL, 10);
1183	if (pkts == 0 || strlen(num) == 0)
1184		has_limit = false;
1185
1186	do {
1187		struct pcap_pkthdr *hdr = pcap_curr_pkt();
1188		int ret = bpf_run_all(bpf_image, bpf_prog_len,
1189				      (uint8_t *) hdr + sizeof(*hdr),
1190				      hdr->caplen, hdr->len);
1191		if (ret > 0)
1192			pass++;
1193		else if (ret == 0)
1194			fail++;
1195		else
1196			return CMD_OK;
1197		bpf_reset();
1198	} while (pcap_next_pkt() && (!has_limit || (has_limit && ++i < pkts)));
1199
1200	rl_printf("bpf passes:%u fails:%u\n", pass, fail);
1201
1202	pcap_reset_pkt();
1203	bpf_reset();
1204
1205	pass = fail = 0;
1206	return CMD_OK;
1207}
1208
1209static int cmd_disassemble(char *line_string)
1210{
1211	bool single_line = false;
1212	unsigned long line;
1213
1214	if (!bpf_prog_loaded())
1215		return CMD_ERR;
1216	if (strlen(line_string) > 0 &&
1217	    (line = strtoul(line_string, NULL, 10)) < bpf_prog_len)
1218		single_line = true;
1219	if (single_line)
1220		bpf_disasm(bpf_image[line], line);
1221	else
1222		bpf_disasm_all(bpf_image, bpf_prog_len);
1223
1224	return CMD_OK;
1225}
1226
1227static int cmd_dump(char *dontcare)
1228{
1229	if (!bpf_prog_loaded())
1230		return CMD_ERR;
1231
1232	bpf_dump_all(bpf_image, bpf_prog_len);
1233
1234	return CMD_OK;
1235}
1236
1237static int cmd_quit(char *dontcare)
1238{
1239	return CMD_EX;
1240}
1241
1242static const struct shell_cmd cmds[] = {
1243	{ .name = "load", .func = cmd_load },
1244	{ .name = "select", .func = cmd_select },
1245	{ .name = "step", .func = cmd_step },
1246	{ .name = "run", .func = cmd_run },
1247	{ .name = "breakpoint", .func = cmd_breakpoint },
1248	{ .name = "disassemble", .func = cmd_disassemble },
1249	{ .name = "dump", .func = cmd_dump },
1250	{ .name = "quit", .func = cmd_quit },
1251};
1252
1253static int execf(char *arg)
1254{
1255	char *cmd, *cont, *tmp = strdup(arg);
1256	int i, ret = 0, len;
1257
1258	cmd = strtok_r(tmp, " ", &cont);
1259	if (cmd == NULL)
1260		goto out;
1261	len = strlen(cmd);
1262	for (i = 0; i < array_size(cmds); i++) {
1263		if (len != strlen(cmds[i].name))
1264			continue;
1265		if (strncmp(cmds[i].name, cmd, len) == 0) {
1266			ret = cmds[i].func(cont);
1267			break;
1268		}
1269	}
1270out:
1271	free(tmp);
1272	return ret;
1273}
1274
1275static char *shell_comp_gen(const char *buf, int state)
1276{
1277	static int list_index, len;
1278
1279	if (!state) {
1280		list_index = 0;
1281		len = strlen(buf);
1282	}
1283
1284	for (; list_index < array_size(cmds); ) {
1285		const char *name = cmds[list_index].name;
1286
1287		list_index++;
1288		if (strncmp(name, buf, len) == 0)
1289			return strdup(name);
1290	}
1291
1292	return NULL;
1293}
1294
1295static char **shell_completion(const char *buf, int start, int end)
1296{
1297	char **matches = NULL;
1298
1299	if (start == 0)
1300		matches = rl_completion_matches(buf, shell_comp_gen);
1301
1302	return matches;
1303}
1304
1305static void intr_shell(int sig)
1306{
1307	if (rl_end)
1308		rl_kill_line(-1, 0);
1309
1310	rl_crlf();
1311	rl_refresh_line(0, 0);
1312	rl_free_line_state();
1313}
1314
1315static void init_shell(FILE *fin, FILE *fout)
1316{
1317	char file[128];
1318
1319	snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1320	read_history(file);
1321
1322	rl_instream = fin;
1323	rl_outstream = fout;
1324
1325	rl_readline_name = "bpf_dbg";
1326	rl_terminal_name = getenv("TERM");
1327
1328	rl_catch_signals = 0;
1329	rl_catch_sigwinch = 1;
1330
1331	rl_attempted_completion_function = shell_completion;
1332
1333	rl_bind_key('\t', rl_complete);
1334
1335	rl_bind_key_in_map('\t', rl_complete, emacs_meta_keymap);
1336	rl_bind_key_in_map('\033', rl_complete, emacs_meta_keymap);
1337
1338	snprintf(file, sizeof(file), "%s/.bpf_dbg_init", getenv("HOME"));
1339	rl_read_init_file(file);
1340
1341	rl_prep_terminal(0);
1342	rl_set_signals();
1343
1344	signal(SIGINT, intr_shell);
1345}
1346
1347static void exit_shell(FILE *fin, FILE *fout)
1348{
1349	char file[128];
1350
1351	snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1352	write_history(file);
1353
1354	clear_history();
1355	rl_deprep_terminal();
1356
1357	try_close_pcap();
1358
1359	if (fin != stdin)
1360		fclose(fin);
1361	if (fout != stdout)
1362		fclose(fout);
1363}
1364
1365static int run_shell_loop(FILE *fin, FILE *fout)
1366{
1367	char *buf;
1368
1369	init_shell(fin, fout);
1370
1371	while ((buf = readline("> ")) != NULL) {
1372		int ret = execf(buf);
1373		if (ret == CMD_EX)
1374			break;
1375		if (ret == CMD_OK && strlen(buf) > 0)
1376			add_history(buf);
1377
1378		free(buf);
1379	}
1380
1381	exit_shell(fin, fout);
1382	return 0;
1383}
1384
1385int main(int argc, char **argv)
1386{
1387	FILE *fin = NULL, *fout = NULL;
1388
1389	if (argc >= 2)
1390		fin = fopen(argv[1], "r");
1391	if (argc >= 3)
1392		fout = fopen(argv[2], "w");
1393
1394	return run_shell_loop(fin ? : stdin, fout ? : stdout);
1395}