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