0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030
0031
0032
0033
0034 #include <stdio.h>
0035 #include <unistd.h>
0036 #include <stdlib.h>
0037 #include <ctype.h>
0038 #include <stdbool.h>
0039 #include <stdarg.h>
0040 #include <setjmp.h>
0041 #include <linux/filter.h>
0042 #include <linux/if_packet.h>
0043 #include <readline/readline.h>
0044 #include <readline/history.h>
0045 #include <sys/types.h>
0046 #include <sys/socket.h>
0047 #include <sys/stat.h>
0048 #include <sys/mman.h>
0049 #include <fcntl.h>
0050 #include <errno.h>
0051 #include <signal.h>
0052 #include <arpa/inet.h>
0053 #include <net/ethernet.h>
0054
0055 #define TCPDUMP_MAGIC 0xa1b2c3d4
0056
0057 #define BPF_LDX_B (BPF_LDX | BPF_B)
0058 #define BPF_LDX_W (BPF_LDX | BPF_W)
0059 #define BPF_JMP_JA (BPF_JMP | BPF_JA)
0060 #define BPF_JMP_JEQ (BPF_JMP | BPF_JEQ)
0061 #define BPF_JMP_JGT (BPF_JMP | BPF_JGT)
0062 #define BPF_JMP_JGE (BPF_JMP | BPF_JGE)
0063 #define BPF_JMP_JSET (BPF_JMP | BPF_JSET)
0064 #define BPF_ALU_ADD (BPF_ALU | BPF_ADD)
0065 #define BPF_ALU_SUB (BPF_ALU | BPF_SUB)
0066 #define BPF_ALU_MUL (BPF_ALU | BPF_MUL)
0067 #define BPF_ALU_DIV (BPF_ALU | BPF_DIV)
0068 #define BPF_ALU_MOD (BPF_ALU | BPF_MOD)
0069 #define BPF_ALU_NEG (BPF_ALU | BPF_NEG)
0070 #define BPF_ALU_AND (BPF_ALU | BPF_AND)
0071 #define BPF_ALU_OR (BPF_ALU | BPF_OR)
0072 #define BPF_ALU_XOR (BPF_ALU | BPF_XOR)
0073 #define BPF_ALU_LSH (BPF_ALU | BPF_LSH)
0074 #define BPF_ALU_RSH (BPF_ALU | BPF_RSH)
0075 #define BPF_MISC_TAX (BPF_MISC | BPF_TAX)
0076 #define BPF_MISC_TXA (BPF_MISC | BPF_TXA)
0077 #define BPF_LD_B (BPF_LD | BPF_B)
0078 #define BPF_LD_H (BPF_LD | BPF_H)
0079 #define BPF_LD_W (BPF_LD | BPF_W)
0080
0081 #ifndef array_size
0082 # define array_size(x) (sizeof(x) / sizeof((x)[0]))
0083 #endif
0084
0085 #ifndef __check_format_printf
0086 # define __check_format_printf(pos_fmtstr, pos_fmtargs) \
0087 __attribute__ ((format (printf, (pos_fmtstr), (pos_fmtargs))))
0088 #endif
0089
0090 enum {
0091 CMD_OK,
0092 CMD_ERR,
0093 CMD_EX,
0094 };
0095
0096 struct shell_cmd {
0097 const char *name;
0098 int (*func)(char *args);
0099 };
0100
0101 struct pcap_filehdr {
0102 uint32_t magic;
0103 uint16_t version_major;
0104 uint16_t version_minor;
0105 int32_t thiszone;
0106 uint32_t sigfigs;
0107 uint32_t snaplen;
0108 uint32_t linktype;
0109 };
0110
0111 struct pcap_timeval {
0112 int32_t tv_sec;
0113 int32_t tv_usec;
0114 };
0115
0116 struct pcap_pkthdr {
0117 struct pcap_timeval ts;
0118 uint32_t caplen;
0119 uint32_t len;
0120 };
0121
0122 struct bpf_regs {
0123 uint32_t A;
0124 uint32_t X;
0125 uint32_t M[BPF_MEMWORDS];
0126 uint32_t R;
0127 bool Rs;
0128 uint16_t Pc;
0129 };
0130
0131 static struct sock_filter bpf_image[BPF_MAXINSNS + 1];
0132 static unsigned int bpf_prog_len;
0133
0134 static int bpf_breakpoints[64];
0135 static struct bpf_regs bpf_regs[BPF_MAXINSNS + 1];
0136 static struct bpf_regs bpf_curr;
0137 static unsigned int bpf_regs_len;
0138
0139 static int pcap_fd = -1;
0140 static unsigned int pcap_packet;
0141 static size_t pcap_map_size;
0142 static char *pcap_ptr_va_start, *pcap_ptr_va_curr;
0143
0144 static const char * const op_table[] = {
0145 [BPF_ST] = "st",
0146 [BPF_STX] = "stx",
0147 [BPF_LD_B] = "ldb",
0148 [BPF_LD_H] = "ldh",
0149 [BPF_LD_W] = "ld",
0150 [BPF_LDX] = "ldx",
0151 [BPF_LDX_B] = "ldxb",
0152 [BPF_JMP_JA] = "ja",
0153 [BPF_JMP_JEQ] = "jeq",
0154 [BPF_JMP_JGT] = "jgt",
0155 [BPF_JMP_JGE] = "jge",
0156 [BPF_JMP_JSET] = "jset",
0157 [BPF_ALU_ADD] = "add",
0158 [BPF_ALU_SUB] = "sub",
0159 [BPF_ALU_MUL] = "mul",
0160 [BPF_ALU_DIV] = "div",
0161 [BPF_ALU_MOD] = "mod",
0162 [BPF_ALU_NEG] = "neg",
0163 [BPF_ALU_AND] = "and",
0164 [BPF_ALU_OR] = "or",
0165 [BPF_ALU_XOR] = "xor",
0166 [BPF_ALU_LSH] = "lsh",
0167 [BPF_ALU_RSH] = "rsh",
0168 [BPF_MISC_TAX] = "tax",
0169 [BPF_MISC_TXA] = "txa",
0170 [BPF_RET] = "ret",
0171 };
0172
0173 static __check_format_printf(1, 2) int rl_printf(const char *fmt, ...)
0174 {
0175 int ret;
0176 va_list vl;
0177
0178 va_start(vl, fmt);
0179 ret = vfprintf(rl_outstream, fmt, vl);
0180 va_end(vl);
0181
0182 return ret;
0183 }
0184
0185 static int matches(const char *cmd, const char *pattern)
0186 {
0187 int len = strlen(cmd);
0188
0189 if (len > strlen(pattern))
0190 return -1;
0191
0192 return memcmp(pattern, cmd, len);
0193 }
0194
0195 static void hex_dump(const uint8_t *buf, size_t len)
0196 {
0197 int i;
0198
0199 rl_printf("%3u: ", 0);
0200 for (i = 0; i < len; i++) {
0201 if (i && !(i % 16))
0202 rl_printf("\n%3u: ", i);
0203 rl_printf("%02x ", buf[i]);
0204 }
0205 rl_printf("\n");
0206 }
0207
0208 static bool bpf_prog_loaded(void)
0209 {
0210 if (bpf_prog_len == 0)
0211 rl_printf("no bpf program loaded!\n");
0212
0213 return bpf_prog_len > 0;
0214 }
0215
0216 static void bpf_disasm(const struct sock_filter f, unsigned int i)
0217 {
0218 const char *op, *fmt;
0219 int val = f.k;
0220 char buf[256];
0221
0222 switch (f.code) {
0223 case BPF_RET | BPF_K:
0224 op = op_table[BPF_RET];
0225 fmt = "#%#x";
0226 break;
0227 case BPF_RET | BPF_A:
0228 op = op_table[BPF_RET];
0229 fmt = "a";
0230 break;
0231 case BPF_RET | BPF_X:
0232 op = op_table[BPF_RET];
0233 fmt = "x";
0234 break;
0235 case BPF_MISC_TAX:
0236 op = op_table[BPF_MISC_TAX];
0237 fmt = "";
0238 break;
0239 case BPF_MISC_TXA:
0240 op = op_table[BPF_MISC_TXA];
0241 fmt = "";
0242 break;
0243 case BPF_ST:
0244 op = op_table[BPF_ST];
0245 fmt = "M[%d]";
0246 break;
0247 case BPF_STX:
0248 op = op_table[BPF_STX];
0249 fmt = "M[%d]";
0250 break;
0251 case BPF_LD_W | BPF_ABS:
0252 op = op_table[BPF_LD_W];
0253 fmt = "[%d]";
0254 break;
0255 case BPF_LD_H | BPF_ABS:
0256 op = op_table[BPF_LD_H];
0257 fmt = "[%d]";
0258 break;
0259 case BPF_LD_B | BPF_ABS:
0260 op = op_table[BPF_LD_B];
0261 fmt = "[%d]";
0262 break;
0263 case BPF_LD_W | BPF_LEN:
0264 op = op_table[BPF_LD_W];
0265 fmt = "#len";
0266 break;
0267 case BPF_LD_W | BPF_IND:
0268 op = op_table[BPF_LD_W];
0269 fmt = "[x+%d]";
0270 break;
0271 case BPF_LD_H | BPF_IND:
0272 op = op_table[BPF_LD_H];
0273 fmt = "[x+%d]";
0274 break;
0275 case BPF_LD_B | BPF_IND:
0276 op = op_table[BPF_LD_B];
0277 fmt = "[x+%d]";
0278 break;
0279 case BPF_LD | BPF_IMM:
0280 op = op_table[BPF_LD_W];
0281 fmt = "#%#x";
0282 break;
0283 case BPF_LDX | BPF_IMM:
0284 op = op_table[BPF_LDX];
0285 fmt = "#%#x";
0286 break;
0287 case BPF_LDX_B | BPF_MSH:
0288 op = op_table[BPF_LDX_B];
0289 fmt = "4*([%d]&0xf)";
0290 break;
0291 case BPF_LD | BPF_MEM:
0292 op = op_table[BPF_LD_W];
0293 fmt = "M[%d]";
0294 break;
0295 case BPF_LDX | BPF_MEM:
0296 op = op_table[BPF_LDX];
0297 fmt = "M[%d]";
0298 break;
0299 case BPF_JMP_JA:
0300 op = op_table[BPF_JMP_JA];
0301 fmt = "%d";
0302 val = i + 1 + f.k;
0303 break;
0304 case BPF_JMP_JGT | BPF_X:
0305 op = op_table[BPF_JMP_JGT];
0306 fmt = "x";
0307 break;
0308 case BPF_JMP_JGT | BPF_K:
0309 op = op_table[BPF_JMP_JGT];
0310 fmt = "#%#x";
0311 break;
0312 case BPF_JMP_JGE | BPF_X:
0313 op = op_table[BPF_JMP_JGE];
0314 fmt = "x";
0315 break;
0316 case BPF_JMP_JGE | BPF_K:
0317 op = op_table[BPF_JMP_JGE];
0318 fmt = "#%#x";
0319 break;
0320 case BPF_JMP_JEQ | BPF_X:
0321 op = op_table[BPF_JMP_JEQ];
0322 fmt = "x";
0323 break;
0324 case BPF_JMP_JEQ | BPF_K:
0325 op = op_table[BPF_JMP_JEQ];
0326 fmt = "#%#x";
0327 break;
0328 case BPF_JMP_JSET | BPF_X:
0329 op = op_table[BPF_JMP_JSET];
0330 fmt = "x";
0331 break;
0332 case BPF_JMP_JSET | BPF_K:
0333 op = op_table[BPF_JMP_JSET];
0334 fmt = "#%#x";
0335 break;
0336 case BPF_ALU_NEG:
0337 op = op_table[BPF_ALU_NEG];
0338 fmt = "";
0339 break;
0340 case BPF_ALU_LSH | BPF_X:
0341 op = op_table[BPF_ALU_LSH];
0342 fmt = "x";
0343 break;
0344 case BPF_ALU_LSH | BPF_K:
0345 op = op_table[BPF_ALU_LSH];
0346 fmt = "#%d";
0347 break;
0348 case BPF_ALU_RSH | BPF_X:
0349 op = op_table[BPF_ALU_RSH];
0350 fmt = "x";
0351 break;
0352 case BPF_ALU_RSH | BPF_K:
0353 op = op_table[BPF_ALU_RSH];
0354 fmt = "#%d";
0355 break;
0356 case BPF_ALU_ADD | BPF_X:
0357 op = op_table[BPF_ALU_ADD];
0358 fmt = "x";
0359 break;
0360 case BPF_ALU_ADD | BPF_K:
0361 op = op_table[BPF_ALU_ADD];
0362 fmt = "#%d";
0363 break;
0364 case BPF_ALU_SUB | BPF_X:
0365 op = op_table[BPF_ALU_SUB];
0366 fmt = "x";
0367 break;
0368 case BPF_ALU_SUB | BPF_K:
0369 op = op_table[BPF_ALU_SUB];
0370 fmt = "#%d";
0371 break;
0372 case BPF_ALU_MUL | BPF_X:
0373 op = op_table[BPF_ALU_MUL];
0374 fmt = "x";
0375 break;
0376 case BPF_ALU_MUL | BPF_K:
0377 op = op_table[BPF_ALU_MUL];
0378 fmt = "#%d";
0379 break;
0380 case BPF_ALU_DIV | BPF_X:
0381 op = op_table[BPF_ALU_DIV];
0382 fmt = "x";
0383 break;
0384 case BPF_ALU_DIV | BPF_K:
0385 op = op_table[BPF_ALU_DIV];
0386 fmt = "#%d";
0387 break;
0388 case BPF_ALU_MOD | BPF_X:
0389 op = op_table[BPF_ALU_MOD];
0390 fmt = "x";
0391 break;
0392 case BPF_ALU_MOD | BPF_K:
0393 op = op_table[BPF_ALU_MOD];
0394 fmt = "#%d";
0395 break;
0396 case BPF_ALU_AND | BPF_X:
0397 op = op_table[BPF_ALU_AND];
0398 fmt = "x";
0399 break;
0400 case BPF_ALU_AND | BPF_K:
0401 op = op_table[BPF_ALU_AND];
0402 fmt = "#%#x";
0403 break;
0404 case BPF_ALU_OR | BPF_X:
0405 op = op_table[BPF_ALU_OR];
0406 fmt = "x";
0407 break;
0408 case BPF_ALU_OR | BPF_K:
0409 op = op_table[BPF_ALU_OR];
0410 fmt = "#%#x";
0411 break;
0412 case BPF_ALU_XOR | BPF_X:
0413 op = op_table[BPF_ALU_XOR];
0414 fmt = "x";
0415 break;
0416 case BPF_ALU_XOR | BPF_K:
0417 op = op_table[BPF_ALU_XOR];
0418 fmt = "#%#x";
0419 break;
0420 default:
0421 op = "nosup";
0422 fmt = "%#x";
0423 val = f.code;
0424 break;
0425 }
0426
0427 memset(buf, 0, sizeof(buf));
0428 snprintf(buf, sizeof(buf), fmt, val);
0429 buf[sizeof(buf) - 1] = 0;
0430
0431 if ((BPF_CLASS(f.code) == BPF_JMP && BPF_OP(f.code) != BPF_JA))
0432 rl_printf("l%d:\t%s %s, l%d, l%d\n", i, op, buf,
0433 i + 1 + f.jt, i + 1 + f.jf);
0434 else
0435 rl_printf("l%d:\t%s %s\n", i, op, buf);
0436 }
0437
0438 static void bpf_dump_curr(struct bpf_regs *r, struct sock_filter *f)
0439 {
0440 int i, m = 0;
0441
0442 rl_printf("pc: [%u]\n", r->Pc);
0443 rl_printf("code: [%u] jt[%u] jf[%u] k[%u]\n",
0444 f->code, f->jt, f->jf, f->k);
0445 rl_printf("curr: ");
0446 bpf_disasm(*f, r->Pc);
0447
0448 if (f->jt || f->jf) {
0449 rl_printf("jt: ");
0450 bpf_disasm(*(f + f->jt + 1), r->Pc + f->jt + 1);
0451 rl_printf("jf: ");
0452 bpf_disasm(*(f + f->jf + 1), r->Pc + f->jf + 1);
0453 }
0454
0455 rl_printf("A: [%#08x][%u]\n", r->A, r->A);
0456 rl_printf("X: [%#08x][%u]\n", r->X, r->X);
0457 if (r->Rs)
0458 rl_printf("ret: [%#08x][%u]!\n", r->R, r->R);
0459
0460 for (i = 0; i < BPF_MEMWORDS; i++) {
0461 if (r->M[i]) {
0462 m++;
0463 rl_printf("M[%d]: [%#08x][%u]\n", i, r->M[i], r->M[i]);
0464 }
0465 }
0466 if (m == 0)
0467 rl_printf("M[0,%d]: [%#08x][%u]\n", BPF_MEMWORDS - 1, 0, 0);
0468 }
0469
0470 static void bpf_dump_pkt(uint8_t *pkt, uint32_t pkt_caplen, uint32_t pkt_len)
0471 {
0472 if (pkt_caplen != pkt_len)
0473 rl_printf("cap: %u, len: %u\n", pkt_caplen, pkt_len);
0474 else
0475 rl_printf("len: %u\n", pkt_len);
0476
0477 hex_dump(pkt, pkt_caplen);
0478 }
0479
0480 static void bpf_disasm_all(const struct sock_filter *f, unsigned int len)
0481 {
0482 unsigned int i;
0483
0484 for (i = 0; i < len; i++)
0485 bpf_disasm(f[i], i);
0486 }
0487
0488 static void bpf_dump_all(const struct sock_filter *f, unsigned int len)
0489 {
0490 unsigned int i;
0491
0492 rl_printf("/* { op, jt, jf, k }, */\n");
0493 for (i = 0; i < len; i++)
0494 rl_printf("{ %#04x, %2u, %2u, %#010x },\n",
0495 f[i].code, f[i].jt, f[i].jf, f[i].k);
0496 }
0497
0498 static bool bpf_runnable(struct sock_filter *f, unsigned int len)
0499 {
0500 int sock, ret, i;
0501 struct sock_fprog bpf = {
0502 .filter = f,
0503 .len = len,
0504 };
0505
0506 sock = socket(AF_INET, SOCK_DGRAM, 0);
0507 if (sock < 0) {
0508 rl_printf("cannot open socket!\n");
0509 return false;
0510 }
0511 ret = setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &bpf, sizeof(bpf));
0512 close(sock);
0513 if (ret < 0) {
0514 rl_printf("program not allowed to run by kernel!\n");
0515 return false;
0516 }
0517 for (i = 0; i < len; i++) {
0518 if (BPF_CLASS(f[i].code) == BPF_LD &&
0519 f[i].k > SKF_AD_OFF) {
0520 rl_printf("extensions currently not supported!\n");
0521 return false;
0522 }
0523 }
0524
0525 return true;
0526 }
0527
0528 static void bpf_reset_breakpoints(void)
0529 {
0530 int i;
0531
0532 for (i = 0; i < array_size(bpf_breakpoints); i++)
0533 bpf_breakpoints[i] = -1;
0534 }
0535
0536 static void bpf_set_breakpoints(unsigned int where)
0537 {
0538 int i;
0539 bool set = false;
0540
0541 for (i = 0; i < array_size(bpf_breakpoints); i++) {
0542 if (bpf_breakpoints[i] == (int) where) {
0543 rl_printf("breakpoint already set!\n");
0544 set = true;
0545 break;
0546 }
0547
0548 if (bpf_breakpoints[i] == -1 && set == false) {
0549 bpf_breakpoints[i] = where;
0550 set = true;
0551 }
0552 }
0553
0554 if (!set)
0555 rl_printf("too many breakpoints set, reset first!\n");
0556 }
0557
0558 static void bpf_dump_breakpoints(void)
0559 {
0560 int i;
0561
0562 rl_printf("breakpoints: ");
0563
0564 for (i = 0; i < array_size(bpf_breakpoints); i++) {
0565 if (bpf_breakpoints[i] < 0)
0566 continue;
0567 rl_printf("%d ", bpf_breakpoints[i]);
0568 }
0569
0570 rl_printf("\n");
0571 }
0572
0573 static void bpf_reset(void)
0574 {
0575 bpf_regs_len = 0;
0576
0577 memset(bpf_regs, 0, sizeof(bpf_regs));
0578 memset(&bpf_curr, 0, sizeof(bpf_curr));
0579 }
0580
0581 static void bpf_safe_regs(void)
0582 {
0583 memcpy(&bpf_regs[bpf_regs_len++], &bpf_curr, sizeof(bpf_curr));
0584 }
0585
0586 static bool bpf_restore_regs(int off)
0587 {
0588 unsigned int index = bpf_regs_len - 1 + off;
0589
0590 if (index == 0) {
0591 bpf_reset();
0592 return true;
0593 } else if (index < bpf_regs_len) {
0594 memcpy(&bpf_curr, &bpf_regs[index], sizeof(bpf_curr));
0595 bpf_regs_len = index;
0596 return true;
0597 } else {
0598 rl_printf("reached bottom of register history stack!\n");
0599 return false;
0600 }
0601 }
0602
0603 static uint32_t extract_u32(uint8_t *pkt, uint32_t off)
0604 {
0605 uint32_t r;
0606
0607 memcpy(&r, &pkt[off], sizeof(r));
0608
0609 return ntohl(r);
0610 }
0611
0612 static uint16_t extract_u16(uint8_t *pkt, uint32_t off)
0613 {
0614 uint16_t r;
0615
0616 memcpy(&r, &pkt[off], sizeof(r));
0617
0618 return ntohs(r);
0619 }
0620
0621 static uint8_t extract_u8(uint8_t *pkt, uint32_t off)
0622 {
0623 return pkt[off];
0624 }
0625
0626 static void set_return(struct bpf_regs *r)
0627 {
0628 r->R = 0;
0629 r->Rs = true;
0630 }
0631
0632 static void bpf_single_step(struct bpf_regs *r, struct sock_filter *f,
0633 uint8_t *pkt, uint32_t pkt_caplen,
0634 uint32_t pkt_len)
0635 {
0636 uint32_t K = f->k;
0637 int d;
0638
0639 switch (f->code) {
0640 case BPF_RET | BPF_K:
0641 r->R = K;
0642 r->Rs = true;
0643 break;
0644 case BPF_RET | BPF_A:
0645 r->R = r->A;
0646 r->Rs = true;
0647 break;
0648 case BPF_RET | BPF_X:
0649 r->R = r->X;
0650 r->Rs = true;
0651 break;
0652 case BPF_MISC_TAX:
0653 r->X = r->A;
0654 break;
0655 case BPF_MISC_TXA:
0656 r->A = r->X;
0657 break;
0658 case BPF_ST:
0659 r->M[K] = r->A;
0660 break;
0661 case BPF_STX:
0662 r->M[K] = r->X;
0663 break;
0664 case BPF_LD_W | BPF_ABS:
0665 d = pkt_caplen - K;
0666 if (d >= sizeof(uint32_t))
0667 r->A = extract_u32(pkt, K);
0668 else
0669 set_return(r);
0670 break;
0671 case BPF_LD_H | BPF_ABS:
0672 d = pkt_caplen - K;
0673 if (d >= sizeof(uint16_t))
0674 r->A = extract_u16(pkt, K);
0675 else
0676 set_return(r);
0677 break;
0678 case BPF_LD_B | BPF_ABS:
0679 d = pkt_caplen - K;
0680 if (d >= sizeof(uint8_t))
0681 r->A = extract_u8(pkt, K);
0682 else
0683 set_return(r);
0684 break;
0685 case BPF_LD_W | BPF_IND:
0686 d = pkt_caplen - (r->X + K);
0687 if (d >= sizeof(uint32_t))
0688 r->A = extract_u32(pkt, r->X + K);
0689 break;
0690 case BPF_LD_H | BPF_IND:
0691 d = pkt_caplen - (r->X + K);
0692 if (d >= sizeof(uint16_t))
0693 r->A = extract_u16(pkt, r->X + K);
0694 else
0695 set_return(r);
0696 break;
0697 case BPF_LD_B | BPF_IND:
0698 d = pkt_caplen - (r->X + K);
0699 if (d >= sizeof(uint8_t))
0700 r->A = extract_u8(pkt, r->X + K);
0701 else
0702 set_return(r);
0703 break;
0704 case BPF_LDX_B | BPF_MSH:
0705 d = pkt_caplen - K;
0706 if (d >= sizeof(uint8_t)) {
0707 r->X = extract_u8(pkt, K);
0708 r->X = (r->X & 0xf) << 2;
0709 } else
0710 set_return(r);
0711 break;
0712 case BPF_LD_W | BPF_LEN:
0713 r->A = pkt_len;
0714 break;
0715 case BPF_LDX_W | BPF_LEN:
0716 r->A = pkt_len;
0717 break;
0718 case BPF_LD | BPF_IMM:
0719 r->A = K;
0720 break;
0721 case BPF_LDX | BPF_IMM:
0722 r->X = K;
0723 break;
0724 case BPF_LD | BPF_MEM:
0725 r->A = r->M[K];
0726 break;
0727 case BPF_LDX | BPF_MEM:
0728 r->X = r->M[K];
0729 break;
0730 case BPF_JMP_JA:
0731 r->Pc += K;
0732 break;
0733 case BPF_JMP_JGT | BPF_X:
0734 r->Pc += r->A > r->X ? f->jt : f->jf;
0735 break;
0736 case BPF_JMP_JGT | BPF_K:
0737 r->Pc += r->A > K ? f->jt : f->jf;
0738 break;
0739 case BPF_JMP_JGE | BPF_X:
0740 r->Pc += r->A >= r->X ? f->jt : f->jf;
0741 break;
0742 case BPF_JMP_JGE | BPF_K:
0743 r->Pc += r->A >= K ? f->jt : f->jf;
0744 break;
0745 case BPF_JMP_JEQ | BPF_X:
0746 r->Pc += r->A == r->X ? f->jt : f->jf;
0747 break;
0748 case BPF_JMP_JEQ | BPF_K:
0749 r->Pc += r->A == K ? f->jt : f->jf;
0750 break;
0751 case BPF_JMP_JSET | BPF_X:
0752 r->Pc += r->A & r->X ? f->jt : f->jf;
0753 break;
0754 case BPF_JMP_JSET | BPF_K:
0755 r->Pc += r->A & K ? f->jt : f->jf;
0756 break;
0757 case BPF_ALU_NEG:
0758 r->A = -r->A;
0759 break;
0760 case BPF_ALU_LSH | BPF_X:
0761 r->A <<= r->X;
0762 break;
0763 case BPF_ALU_LSH | BPF_K:
0764 r->A <<= K;
0765 break;
0766 case BPF_ALU_RSH | BPF_X:
0767 r->A >>= r->X;
0768 break;
0769 case BPF_ALU_RSH | BPF_K:
0770 r->A >>= K;
0771 break;
0772 case BPF_ALU_ADD | BPF_X:
0773 r->A += r->X;
0774 break;
0775 case BPF_ALU_ADD | BPF_K:
0776 r->A += K;
0777 break;
0778 case BPF_ALU_SUB | BPF_X:
0779 r->A -= r->X;
0780 break;
0781 case BPF_ALU_SUB | BPF_K:
0782 r->A -= K;
0783 break;
0784 case BPF_ALU_MUL | BPF_X:
0785 r->A *= r->X;
0786 break;
0787 case BPF_ALU_MUL | BPF_K:
0788 r->A *= K;
0789 break;
0790 case BPF_ALU_DIV | BPF_X:
0791 case BPF_ALU_MOD | BPF_X:
0792 if (r->X == 0) {
0793 set_return(r);
0794 break;
0795 }
0796 goto do_div;
0797 case BPF_ALU_DIV | BPF_K:
0798 case BPF_ALU_MOD | BPF_K:
0799 if (K == 0) {
0800 set_return(r);
0801 break;
0802 }
0803 do_div:
0804 switch (f->code) {
0805 case BPF_ALU_DIV | BPF_X:
0806 r->A /= r->X;
0807 break;
0808 case BPF_ALU_DIV | BPF_K:
0809 r->A /= K;
0810 break;
0811 case BPF_ALU_MOD | BPF_X:
0812 r->A %= r->X;
0813 break;
0814 case BPF_ALU_MOD | BPF_K:
0815 r->A %= K;
0816 break;
0817 }
0818 break;
0819 case BPF_ALU_AND | BPF_X:
0820 r->A &= r->X;
0821 break;
0822 case BPF_ALU_AND | BPF_K:
0823 r->A &= K;
0824 break;
0825 case BPF_ALU_OR | BPF_X:
0826 r->A |= r->X;
0827 break;
0828 case BPF_ALU_OR | BPF_K:
0829 r->A |= K;
0830 break;
0831 case BPF_ALU_XOR | BPF_X:
0832 r->A ^= r->X;
0833 break;
0834 case BPF_ALU_XOR | BPF_K:
0835 r->A ^= K;
0836 break;
0837 }
0838 }
0839
0840 static bool bpf_pc_has_breakpoint(uint16_t pc)
0841 {
0842 int i;
0843
0844 for (i = 0; i < array_size(bpf_breakpoints); i++) {
0845 if (bpf_breakpoints[i] < 0)
0846 continue;
0847 if (bpf_breakpoints[i] == pc)
0848 return true;
0849 }
0850
0851 return false;
0852 }
0853
0854 static bool bpf_handle_breakpoint(struct bpf_regs *r, struct sock_filter *f,
0855 uint8_t *pkt, uint32_t pkt_caplen,
0856 uint32_t pkt_len)
0857 {
0858 rl_printf("-- register dump --\n");
0859 bpf_dump_curr(r, &f[r->Pc]);
0860 rl_printf("-- packet dump --\n");
0861 bpf_dump_pkt(pkt, pkt_caplen, pkt_len);
0862 rl_printf("(breakpoint)\n");
0863 return true;
0864 }
0865
0866 static int bpf_run_all(struct sock_filter *f, uint16_t bpf_len, uint8_t *pkt,
0867 uint32_t pkt_caplen, uint32_t pkt_len)
0868 {
0869 bool stop = false;
0870
0871 while (bpf_curr.Rs == false && stop == false) {
0872 bpf_safe_regs();
0873
0874 if (bpf_pc_has_breakpoint(bpf_curr.Pc))
0875 stop = bpf_handle_breakpoint(&bpf_curr, f, pkt,
0876 pkt_caplen, pkt_len);
0877
0878 bpf_single_step(&bpf_curr, &f[bpf_curr.Pc], pkt, pkt_caplen,
0879 pkt_len);
0880 bpf_curr.Pc++;
0881 }
0882
0883 return stop ? -1 : bpf_curr.R;
0884 }
0885
0886 static int bpf_run_stepping(struct sock_filter *f, uint16_t bpf_len,
0887 uint8_t *pkt, uint32_t pkt_caplen,
0888 uint32_t pkt_len, int next)
0889 {
0890 bool stop = false;
0891 int i = 1;
0892
0893 while (!bpf_curr.Rs && !stop) {
0894 bpf_safe_regs();
0895
0896 if (i++ == next)
0897 stop = bpf_handle_breakpoint(&bpf_curr, f, pkt,
0898 pkt_caplen, pkt_len);
0899
0900 bpf_single_step(&bpf_curr, &f[bpf_curr.Pc], pkt, pkt_caplen,
0901 pkt_len);
0902 bpf_curr.Pc++;
0903 }
0904
0905 return stop ? -1 : bpf_curr.R;
0906 }
0907
0908 static bool pcap_loaded(void)
0909 {
0910 if (pcap_fd < 0)
0911 rl_printf("no pcap file loaded!\n");
0912
0913 return pcap_fd >= 0;
0914 }
0915
0916 static struct pcap_pkthdr *pcap_curr_pkt(void)
0917 {
0918 return (void *) pcap_ptr_va_curr;
0919 }
0920
0921 static bool pcap_next_pkt(void)
0922 {
0923 struct pcap_pkthdr *hdr = pcap_curr_pkt();
0924
0925 if (pcap_ptr_va_curr + sizeof(*hdr) -
0926 pcap_ptr_va_start >= pcap_map_size)
0927 return false;
0928 if (hdr->caplen == 0 || hdr->len == 0 || hdr->caplen > hdr->len)
0929 return false;
0930 if (pcap_ptr_va_curr + sizeof(*hdr) + hdr->caplen -
0931 pcap_ptr_va_start >= pcap_map_size)
0932 return false;
0933
0934 pcap_ptr_va_curr += (sizeof(*hdr) + hdr->caplen);
0935 return true;
0936 }
0937
0938 static void pcap_reset_pkt(void)
0939 {
0940 pcap_ptr_va_curr = pcap_ptr_va_start + sizeof(struct pcap_filehdr);
0941 }
0942
0943 static int try_load_pcap(const char *file)
0944 {
0945 struct pcap_filehdr *hdr;
0946 struct stat sb;
0947 int ret;
0948
0949 pcap_fd = open(file, O_RDONLY);
0950 if (pcap_fd < 0) {
0951 rl_printf("cannot open pcap [%s]!\n", strerror(errno));
0952 return CMD_ERR;
0953 }
0954
0955 ret = fstat(pcap_fd, &sb);
0956 if (ret < 0) {
0957 rl_printf("cannot fstat pcap file!\n");
0958 return CMD_ERR;
0959 }
0960
0961 if (!S_ISREG(sb.st_mode)) {
0962 rl_printf("not a regular pcap file, duh!\n");
0963 return CMD_ERR;
0964 }
0965
0966 pcap_map_size = sb.st_size;
0967 if (pcap_map_size <= sizeof(struct pcap_filehdr)) {
0968 rl_printf("pcap file too small!\n");
0969 return CMD_ERR;
0970 }
0971
0972 pcap_ptr_va_start = mmap(NULL, pcap_map_size, PROT_READ,
0973 MAP_SHARED | MAP_LOCKED, pcap_fd, 0);
0974 if (pcap_ptr_va_start == MAP_FAILED) {
0975 rl_printf("mmap of file failed!");
0976 return CMD_ERR;
0977 }
0978
0979 hdr = (void *) pcap_ptr_va_start;
0980 if (hdr->magic != TCPDUMP_MAGIC) {
0981 rl_printf("wrong pcap magic!\n");
0982 return CMD_ERR;
0983 }
0984
0985 pcap_reset_pkt();
0986
0987 return CMD_OK;
0988
0989 }
0990
0991 static void try_close_pcap(void)
0992 {
0993 if (pcap_fd >= 0) {
0994 munmap(pcap_ptr_va_start, pcap_map_size);
0995 close(pcap_fd);
0996
0997 pcap_ptr_va_start = pcap_ptr_va_curr = NULL;
0998 pcap_map_size = 0;
0999 pcap_packet = 0;
1000 pcap_fd = -1;
1001 }
1002 }
1003
1004 static 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
1051 static 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
1064 static 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 {
1083 out:
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
1093 static 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
1127 static 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 ;
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
1155 static 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
1176 static 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 || (++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
1212 static 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
1230 static 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
1240 static int cmd_quit(char *dontcare)
1241 {
1242 return CMD_EX;
1243 }
1244
1245 static 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
1256 static 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 }
1273 out:
1274 free(tmp);
1275 return ret;
1276 }
1277
1278 static 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
1298 static 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
1308 static 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
1318 static 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
1350 static 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
1368 static 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
1388 int 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 }