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
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, *tmp = strdup(arg);
1067 int ret = CMD_OK;
1068
1069 subcmd = strtok_r(tmp, " ", &cont);
1070 if (subcmd == NULL)
1071 goto out;
1072 if (matches(subcmd, "bpf") == 0) {
1073 bpf_reset();
1074 bpf_reset_breakpoints();
1075
1076 ret = cmd_load_bpf(cont);
1077 } else if (matches(subcmd, "pcap") == 0) {
1078 ret = cmd_load_pcap(cont);
1079 } else {
1080out:
1081 rl_printf("bpf <code>: load bpf code\n");
1082 rl_printf("pcap <file>: load pcap file\n");
1083 ret = CMD_ERR;
1084 }
1085
1086 free(tmp);
1087 return ret;
1088}
1089
1090static int cmd_step(char *num)
1091{
1092 struct pcap_pkthdr *hdr;
1093 int steps, ret;
1094
1095 if (!bpf_prog_loaded() || !pcap_loaded())
1096 return CMD_ERR;
1097
1098 steps = strtol(num, NULL, 10);
1099 if (steps == 0 || strlen(num) == 0)
1100 steps = 1;
1101 if (steps < 0) {
1102 if (!bpf_restore_regs(steps))
1103 return CMD_ERR;
1104 steps = 1;
1105 }
1106
1107 hdr = pcap_curr_pkt();
1108 ret = bpf_run_stepping(bpf_image, bpf_prog_len,
1109 (uint8_t *) hdr + sizeof(*hdr),
1110 hdr->caplen, hdr->len, steps);
1111 if (ret >= 0 || bpf_curr.Rs) {
1112 bpf_reset();
1113 if (!pcap_next_pkt()) {
1114 rl_printf("(going back to first packet)\n");
1115 pcap_reset_pkt();
1116 } else {
1117 rl_printf("(next packet)\n");
1118 }
1119 }
1120
1121 return CMD_OK;
1122}
1123
1124static int cmd_select(char *num)
1125{
1126 unsigned int which, i;
1127 bool have_next = true;
1128
1129 if (!pcap_loaded() || strlen(num) == 0)
1130 return CMD_ERR;
1131
1132 which = strtoul(num, NULL, 10);
1133 if (which == 0) {
1134 rl_printf("packet count starts with 1, clamping!\n");
1135 which = 1;
1136 }
1137
1138 pcap_reset_pkt();
1139 bpf_reset();
1140
1141 for (i = 0; i < which && (have_next = pcap_next_pkt()); i++)
1142 ;
1143 if (!have_next || pcap_curr_pkt() == NULL) {
1144 rl_printf("no packet #%u available!\n", which);
1145 pcap_reset_pkt();
1146 return CMD_ERR;
1147 }
1148
1149 return CMD_OK;
1150}
1151
1152static int cmd_breakpoint(char *subcmd)
1153{
1154 if (!bpf_prog_loaded())
1155 return CMD_ERR;
1156 if (strlen(subcmd) == 0)
1157 bpf_dump_breakpoints();
1158 else if (matches(subcmd, "reset") == 0)
1159 bpf_reset_breakpoints();
1160 else {
1161 unsigned int where = strtoul(subcmd, NULL, 10);
1162
1163 if (where < bpf_prog_len) {
1164 bpf_set_breakpoints(where);
1165 rl_printf("breakpoint at: ");
1166 bpf_disasm(bpf_image[where], where);
1167 }
1168 }
1169
1170 return CMD_OK;
1171}
1172
1173static int cmd_run(char *num)
1174{
1175 static uint32_t pass, fail;
1176 bool has_limit = true;
1177 int pkts = 0, i = 0;
1178
1179 if (!bpf_prog_loaded() || !pcap_loaded())
1180 return CMD_ERR;
1181
1182 pkts = strtol(num, NULL, 10);
1183 if (pkts == 0 || strlen(num) == 0)
1184 has_limit = false;
1185
1186 do {
1187 struct pcap_pkthdr *hdr = pcap_curr_pkt();
1188 int ret = bpf_run_all(bpf_image, bpf_prog_len,
1189 (uint8_t *) hdr + sizeof(*hdr),
1190 hdr->caplen, hdr->len);
1191 if (ret > 0)
1192 pass++;
1193 else if (ret == 0)
1194 fail++;
1195 else
1196 return CMD_OK;
1197 bpf_reset();
1198 } while (pcap_next_pkt() && (!has_limit || (has_limit && ++i < pkts)));
1199
1200 rl_printf("bpf passes:%u fails:%u\n", pass, fail);
1201
1202 pcap_reset_pkt();
1203 bpf_reset();
1204
1205 pass = fail = 0;
1206 return CMD_OK;
1207}
1208
1209static int cmd_disassemble(char *line_string)
1210{
1211 bool single_line = false;
1212 unsigned long line;
1213
1214 if (!bpf_prog_loaded())
1215 return CMD_ERR;
1216 if (strlen(line_string) > 0 &&
1217 (line = strtoul(line_string, NULL, 10)) < bpf_prog_len)
1218 single_line = true;
1219 if (single_line)
1220 bpf_disasm(bpf_image[line], line);
1221 else
1222 bpf_disasm_all(bpf_image, bpf_prog_len);
1223
1224 return CMD_OK;
1225}
1226
1227static int cmd_dump(char *dontcare)
1228{
1229 if (!bpf_prog_loaded())
1230 return CMD_ERR;
1231
1232 bpf_dump_all(bpf_image, bpf_prog_len);
1233
1234 return CMD_OK;
1235}
1236
1237static int cmd_quit(char *dontcare)
1238{
1239 return CMD_EX;
1240}
1241
1242static const struct shell_cmd cmds[] = {
1243 { .name = "load", .func = cmd_load },
1244 { .name = "select", .func = cmd_select },
1245 { .name = "step", .func = cmd_step },
1246 { .name = "run", .func = cmd_run },
1247 { .name = "breakpoint", .func = cmd_breakpoint },
1248 { .name = "disassemble", .func = cmd_disassemble },
1249 { .name = "dump", .func = cmd_dump },
1250 { .name = "quit", .func = cmd_quit },
1251};
1252
1253static int execf(char *arg)
1254{
1255 char *cmd, *cont, *tmp = strdup(arg);
1256 int i, ret = 0, len;
1257
1258 cmd = strtok_r(tmp, " ", &cont);
1259 if (cmd == NULL)
1260 goto out;
1261 len = strlen(cmd);
1262 for (i = 0; i < array_size(cmds); i++) {
1263 if (len != strlen(cmds[i].name))
1264 continue;
1265 if (strncmp(cmds[i].name, cmd, len) == 0) {
1266 ret = cmds[i].func(cont);
1267 break;
1268 }
1269 }
1270out:
1271 free(tmp);
1272 return ret;
1273}
1274
1275static char *shell_comp_gen(const char *buf, int state)
1276{
1277 static int list_index, len;
1278
1279 if (!state) {
1280 list_index = 0;
1281 len = strlen(buf);
1282 }
1283
1284 for (; list_index < array_size(cmds); ) {
1285 const char *name = cmds[list_index].name;
1286
1287 list_index++;
1288 if (strncmp(name, buf, len) == 0)
1289 return strdup(name);
1290 }
1291
1292 return NULL;
1293}
1294
1295static char **shell_completion(const char *buf, int start, int end)
1296{
1297 char **matches = NULL;
1298
1299 if (start == 0)
1300 matches = rl_completion_matches(buf, shell_comp_gen);
1301
1302 return matches;
1303}
1304
1305static void intr_shell(int sig)
1306{
1307 if (rl_end)
1308 rl_kill_line(-1, 0);
1309
1310 rl_crlf();
1311 rl_refresh_line(0, 0);
1312 rl_free_line_state();
1313}
1314
1315static void init_shell(FILE *fin, FILE *fout)
1316{
1317 char file[128];
1318
1319 snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1320 read_history(file);
1321
1322 rl_instream = fin;
1323 rl_outstream = fout;
1324
1325 rl_readline_name = "bpf_dbg";
1326 rl_terminal_name = getenv("TERM");
1327
1328 rl_catch_signals = 0;
1329 rl_catch_sigwinch = 1;
1330
1331 rl_attempted_completion_function = shell_completion;
1332
1333 rl_bind_key('\t', rl_complete);
1334
1335 rl_bind_key_in_map('\t', rl_complete, emacs_meta_keymap);
1336 rl_bind_key_in_map('\033', rl_complete, emacs_meta_keymap);
1337
1338 snprintf(file, sizeof(file), "%s/.bpf_dbg_init", getenv("HOME"));
1339 rl_read_init_file(file);
1340
1341 rl_prep_terminal(0);
1342 rl_set_signals();
1343
1344 signal(SIGINT, intr_shell);
1345}
1346
1347static void exit_shell(FILE *fin, FILE *fout)
1348{
1349 char file[128];
1350
1351 snprintf(file, sizeof(file), "%s/.bpf_dbg_history", getenv("HOME"));
1352 write_history(file);
1353
1354 clear_history();
1355 rl_deprep_terminal();
1356
1357 try_close_pcap();
1358
1359 if (fin != stdin)
1360 fclose(fin);
1361 if (fout != stdout)
1362 fclose(fout);
1363}
1364
1365static int run_shell_loop(FILE *fin, FILE *fout)
1366{
1367 char *buf;
1368
1369 init_shell(fin, fout);
1370
1371 while ((buf = readline("> ")) != NULL) {
1372 int ret = execf(buf);
1373 if (ret == CMD_EX)
1374 break;
1375 if (ret == CMD_OK && strlen(buf) > 0)
1376 add_history(buf);
1377
1378 free(buf);
1379 }
1380
1381 exit_shell(fin, fout);
1382 return 0;
1383}
1384
1385int main(int argc, char **argv)
1386{
1387 FILE *fin = NULL, *fout = NULL;
1388
1389 if (argc >= 2)
1390 fin = fopen(argv[1], "r");
1391 if (argc >= 3)
1392 fout = fopen(argv[2], "w");
1393
1394 return run_shell_loop(fin ? : stdin, fout ? : stdout);
1395}
1396
