linux/tools/bpf/bpf_dbg.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Minimal BPF debugger
   4 *
   5 * Minimal BPF debugger that mimics the kernel's engine (w/o extensions)
   6 * and allows for single stepping through selected packets from a pcap
   7 * with a provided user filter in order to facilitate verification of a
   8 * BPF program. Besides others, this is useful to verify BPF programs
   9 * before attaching to a live system, and can be used in socket filters,
  10 * cls_bpf, xt_bpf, team driver and e.g. PTP code; in particular when a
  11 * single more complex BPF program is being used. Reasons for a more
  12 * complex BPF program are likely primarily to optimize execution time
  13 * for making a verdict when multiple simple BPF programs are combined
  14 * into one in order to prevent parsing same headers multiple times.
  15 *
  16 * More on how to debug BPF opcodes see Documentation/networking/filter.rst
  17 * which is the main document on BPF. Mini howto for getting started:
  18 *
  19 *  1) `./bpf_dbg` to enter the shell (shell cmds denoted with '>'):
  20 *  2) > load bpf 6,40 0 0 12,21 0 3 20... (output from `bpf_asm` or
  21 *     `tcpdump -iem1 -ddd port 22 | tr '\n' ','` to load as filter)
  22 *  3) > load pcap foo.pcap
  23 *  4) > run <n>/disassemble/dump/quit (self-explanatory)
  24 *  5) > breakpoint 2 (sets bp at loaded BPF insns 2, do `run` then;
  25 *       multiple bps can be set, of course, a call to `breakpoint`
  26 *       w/o args shows currently loaded bps, `breakpoint reset` for
  27 *       resetting all breakpoints)
  28 *  6) > select 3 (`run` etc will start from the 3rd packet in the pcap)
  29 *  7) > step [-<n>, +<n>] (performs single stepping through the BPF)
  30 *
  31 * Copyright 2013 Daniel Borkmann <borkmann@redhat.com>
  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;
 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;
 138
 139static int pcap_fd = -1;
 140static unsigned int pcap_packet;
 141static size_t pcap_map_size;
 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 = NULL, *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                if (!cont)
1077                        ret = CMD_ERR;
1078                else
1079                        ret = cmd_load_bpf(cont);
1080        } else if (matches(subcmd, "pcap") == 0) {
1081                ret = cmd_load_pcap(cont);
1082        } else {
1083out:
1084                rl_printf("bpf <code>:  load bpf code\n");
1085                rl_printf("pcap <file>: load pcap file\n");
1086                ret = CMD_ERR;
1087        }
1088
1089        free(tmp);
1090        return ret;
1091}
1092
1093static int cmd_step(char *num)
1094{
1095        struct pcap_pkthdr *hdr;
1096        int steps, ret;
1097
1098        if (!bpf_prog_loaded() || !pcap_loaded())
1099                return CMD_ERR;
1100
1101        steps = strtol(num, NULL, 10);
1102        if (steps == 0 || strlen(num) == 0)
1103                steps = 1;
1104        if (steps < 0) {
1105                if (!bpf_restore_regs(steps))
1106                        return CMD_ERR;
1107                steps = 1;
1108        }
1109
1110        hdr = pcap_curr_pkt();
1111        ret = bpf_run_stepping(bpf_image, bpf_prog_len,
1112                               (uint8_t *) hdr + sizeof(*hdr),
1113                               hdr->caplen, hdr->len, steps);
1114        if (ret >= 0 || bpf_curr.Rs) {
1115                bpf_reset();
1116                if (!pcap_next_pkt()) {
1117                        rl_printf("(going back to first packet)\n");
1118                        pcap_reset_pkt();
1119                } else {
1120                        rl_printf("(next packet)\n");
1121                }
1122        }
1123
1124        return CMD_OK;
1125}
1126
1127static int cmd_select(char *num)
1128{
1129        unsigned int which, i;
1130        bool have_next = true;
1131
1132        if (!pcap_loaded() || strlen(num) == 0)
1133                return CMD_ERR;
1134
1135        which = strtoul(num, NULL, 10);
1136        if (which == 0) {
1137                rl_printf("packet count starts with 1, clamping!\n");
1138                which = 1;
1139        }
1140
1141        pcap_reset_pkt();
1142        bpf_reset();
1143
1144        for (i = 0; i < which && (have_next = pcap_next_pkt()); i++)
1145                /* noop */;
1146        if (!have_next || pcap_curr_pkt() == NULL) {
1147                rl_printf("no packet #%u available!\n", which);
1148                pcap_reset_pkt();
1149                return CMD_ERR;
1150        }
1151
1152        return CMD_OK;
1153}
1154
1155static int cmd_breakpoint(char *subcmd)
1156{
1157        if (!bpf_prog_loaded())
1158                return CMD_ERR;
1159        if (strlen(subcmd) == 0)
1160                bpf_dump_breakpoints();
1161        else if (matches(subcmd, "reset") == 0)
1162                bpf_reset_breakpoints();
1163        else {
1164                unsigned int where = strtoul(subcmd, NULL, 10);
1165
1166                if (where < bpf_prog_len) {
1167                        bpf_set_breakpoints(where);
1168                        rl_printf("breakpoint at: ");
1169                        bpf_disasm(bpf_image[where], where);
1170                }
1171        }
1172
1173        return CMD_OK;
1174}
1175
1176static int cmd_run(char *num)
1177{
1178        static uint32_t pass, fail;
1179        bool has_limit = true;
1180        int pkts = 0, i = 0;
1181
1182        if (!bpf_prog_loaded() || !pcap_loaded())
1183                return CMD_ERR;
1184
1185        pkts = strtol(num, NULL, 10);
1186        if (pkts == 0 || strlen(num) == 0)
1187                has_limit = false;
1188
1189        do {
1190                struct pcap_pkthdr *hdr = pcap_curr_pkt();
1191                int ret = bpf_run_all(bpf_image, bpf_prog_len,
1192                                      (uint8_t *) hdr + sizeof(*hdr),
1193                                      hdr->caplen, hdr->len);
1194                if (ret > 0)
1195                        pass++;
1196                else if (ret == 0)
1197                        fail++;
1198                else
1199                        return CMD_OK;
1200                bpf_reset();
1201        } while (pcap_next_pkt() && (!has_limit || (has_limit && ++i < pkts)));
1202
1203        rl_printf("bpf passes:%u fails:%u\n", pass, fail);
1204
1205        pcap_reset_pkt();
1206        bpf_reset();
1207
1208        pass = fail = 0;
1209        return CMD_OK;
1210}
1211
1212static int cmd_disassemble(char *line_string)
1213{
1214        bool single_line = false;
1215        unsigned long line;
1216
1217        if (!bpf_prog_loaded())
1218                return CMD_ERR;
1219        if (strlen(line_string) > 0 &&
1220            (line = strtoul(line_string, NULL, 10)) < bpf_prog_len)
1221                single_line = true;
1222        if (single_line)
1223                bpf_disasm(bpf_image[line], line);
1224        else
1225                bpf_disasm_all(bpf_image, bpf_prog_len);
1226
1227        return CMD_OK;
1228}
1229
1230static int cmd_dump(char *dontcare)
1231{
1232        if (!bpf_prog_loaded())
1233                return CMD_ERR;
1234
1235        bpf_dump_all(bpf_image, bpf_prog_len);
1236
1237        return CMD_OK;
1238}
1239
1240static int cmd_quit(char *dontcare)
1241{
1242        return CMD_EX;
1243}
1244
1245static const struct shell_cmd cmds[] = {
1246        { .name = "load", .func = cmd_load },
1247        { .name = "select", .func = cmd_select },
1248        { .name = "step", .func = cmd_step },
1249        { .name = "run", .func = cmd_run },
1250        { .name = "breakpoint", .func = cmd_breakpoint },
1251        { .name = "disassemble", .func = cmd_disassemble },
1252        { .name = "dump", .func = cmd_dump },
1253        { .name = "quit", .func = cmd_quit },
1254};
1255
1256static int execf(char *arg)
1257{
1258        char *cmd, *cont, *tmp = strdup(arg);
1259        int i, ret = 0, len;
1260
1261        cmd = strtok_r(tmp, " ", &cont);
1262        if (cmd == NULL)
1263                goto out;
1264        len = strlen(cmd);
1265        for (i = 0; i < array_size(cmds); i++) {
1266                if (len != strlen(cmds[i].name))
1267                        continue;
1268                if (strncmp(cmds[i].name, cmd, len) == 0) {
1269                        ret = cmds[i].func(cont);
1270                        break;
1271                }
1272        }
1273out:
1274        free(tmp);
1275        return ret;
1276}
1277
1278static char *shell_comp_gen(const char *buf, int state)
1279{
1280        static int list_index, len;
1281
1282        if (!state) {
1283                list_index = 0;
1284                len = strlen(buf);
1285        }
1286
1287        for (; list_index < array_size(cmds); ) {
1288                const char *name = cmds[list_index].name;
1289
1290                list_index++;
1291                if (strncmp(name, buf, len) == 0)
1292                        return strdup(name);
1293        }
1294
1295        return NULL;
1296}
1297
1298static char **shell_completion(const char *buf, int start, int end)
1299{
1300        char **matches = NULL;
1301
1302        if (start == 0)
1303                matches = rl_completion_matches(buf, shell_comp_gen);
1304
1305        return matches;
1306}
1307
1308static void intr_shell(int sig)
1309{
1310        if (rl_end)
1311                rl_kill_line(-1, 0);
1312
1313        rl_crlf();
1314        rl_refresh_line(0, 0);
1315        rl_free_line_state();
1316}
1317
1318static void init_shell(FILE *fin, FILE *fout)
1319{
1320        char file[128];
1321
1322        snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1323        read_history(file);
1324
1325        rl_instream = fin;
1326        rl_outstream = fout;
1327
1328        rl_readline_name = "bpf_dbg";
1329        rl_terminal_name = getenv("TERM");
1330
1331        rl_catch_signals = 0;
1332        rl_catch_sigwinch = 1;
1333
1334        rl_attempted_completion_function = shell_completion;
1335
1336        rl_bind_key('\t', rl_complete);
1337
1338        rl_bind_key_in_map('\t', rl_complete, emacs_meta_keymap);
1339        rl_bind_key_in_map('\033', rl_complete, emacs_meta_keymap);
1340
1341        snprintf(file, sizeof(file), "%s/.bpf_dbg_init", getenv("HOME"));
1342        rl_read_init_file(file);
1343
1344        rl_prep_terminal(0);
1345        rl_set_signals();
1346
1347        signal(SIGINT, intr_shell);
1348}
1349
1350static void exit_shell(FILE *fin, FILE *fout)
1351{
1352        char file[128];
1353
1354        snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1355        write_history(file);
1356
1357        clear_history();
1358        rl_deprep_terminal();
1359
1360        try_close_pcap();
1361
1362        if (fin != stdin)
1363                fclose(fin);
1364        if (fout != stdout)
1365                fclose(fout);
1366}
1367
1368static int run_shell_loop(FILE *fin, FILE *fout)
1369{
1370        char *buf;
1371
1372        init_shell(fin, fout);
1373
1374        while ((buf = readline("> ")) != NULL) {
1375                int ret = execf(buf);
1376                if (ret == CMD_EX)
1377                        break;
1378                if (ret == CMD_OK && strlen(buf) > 0)
1379                        add_history(buf);
1380
1381                free(buf);
1382        }
1383
1384        exit_shell(fin, fout);
1385        return 0;
1386}
1387
1388int main(int argc, char **argv)
1389{
1390        FILE *fin = NULL, *fout = NULL;
1391
1392        if (argc >= 2)
1393                fin = fopen(argv[1], "r");
1394        if (argc >= 3)
1395                fout = fopen(argv[2], "w");
1396
1397        return run_shell_loop(fin ? : stdin, fout ? : stdout);
1398}
1399