u_int32 xhalf(PMDL p, u_int32 k, int *err) { size_t len, len0; u_char *CurBuf, *NextBuf; PMDL p0; *err = 1; MDLIDX(len, p, k, CurBuf); CurBuf += k; if (len - k >= 2) { *err = 0; return EXTRACT_SHORT(CurBuf); } p0 = p->Next; if (p0 == NULL) return 0; NdisQueryMdl(p0, &NextBuf, &len0, NormalPagePriority); if (NextBuf == NULL || len0 < 1) return 0; *err = 0; return (CurBuf[0] << 8) | NextBuf[0]; }
uint32_t bpf_run_filter(const struct sock_fprog * fcode, uint8_t * packet, size_t plen) { /* XXX: caplen == len */ uint32_t A, X; uint32_t k; struct sock_filter *bpf; int32_t mem[BPF_MEMWORDS]; if (fcode == NULL || fcode->filter == NULL || fcode->len == 0) return 0xFFFFFFFF; A = 0; X = 0; bpf = fcode->filter; --bpf; while (1) { ++bpf; switch (bpf->code) { default: return 0; case BPF_RET | BPF_K: return (uint32_t) bpf->k; case BPF_RET | BPF_A: return (uint32_t) A; case BPF_LD | BPF_W | BPF_ABS: k = bpf->k; if (k + sizeof(int32_t) > plen) return 0; A = EXTRACT_LONG(&packet[k]); continue; case BPF_LD | BPF_H | BPF_ABS: k = bpf->k; if (k + sizeof(short) > plen) return 0; A = EXTRACT_SHORT(&packet[k]); continue; case BPF_LD | BPF_B | BPF_ABS: k = bpf->k; if (k >= plen) return 0; A = packet[k]; continue; case BPF_LD | BPF_W | BPF_LEN: A = plen; continue; case BPF_LDX | BPF_W | BPF_LEN: X = plen; continue; case BPF_LD | BPF_W | BPF_IND: k = X + bpf->k; if (k + sizeof(int32_t) > plen) return 0; A = EXTRACT_LONG(&packet[k]); continue; case BPF_LD | BPF_H | BPF_IND: k = X + bpf->k; if (k + sizeof(short) > plen) return 0; A = EXTRACT_SHORT(&packet[k]); continue; case BPF_LD | BPF_B | BPF_IND: k = X + bpf->k; if (k >= plen) return 0; A = packet[k]; continue; case BPF_LDX | BPF_MSH | BPF_B: k = bpf->k; if (k >= plen) return 0; X = (packet[bpf->k] & 0xf) << 2; continue; case BPF_LD | BPF_IMM: A = bpf->k; continue; case BPF_LDX | BPF_IMM: X = bpf->k; continue; case BPF_LD | BPF_MEM: A = mem[bpf->k]; continue; case BPF_LDX | BPF_MEM: X = mem[bpf->k]; continue; case BPF_ST: mem[bpf->k] = A; continue; case BPF_STX: mem[bpf->k] = X; continue; case BPF_JMP | BPF_JA: bpf += bpf->k; continue; case BPF_JMP | BPF_JGT | BPF_K: bpf += (A > bpf->k) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JGE | BPF_K: bpf += (A >= bpf->k) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JEQ | BPF_K: bpf += (A == bpf->k) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JSET | BPF_K: bpf += (A & bpf->k) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JGT | BPF_X: bpf += (A > X) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JGE | BPF_X: bpf += (A >= X) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JEQ | BPF_X: bpf += (A == X) ? bpf->jt : bpf->jf; continue; case BPF_JMP | BPF_JSET | BPF_X: bpf += (A & X) ? bpf->jt : bpf->jf; continue; case BPF_ALU | BPF_ADD | BPF_X: A += X; continue; case BPF_ALU | BPF_SUB | BPF_X: A -= X; continue; case BPF_ALU | BPF_MUL | BPF_X: A *= X; continue; case BPF_ALU | BPF_DIV | BPF_X: if (X == 0) return 0; A /= X; continue; case BPF_ALU | BPF_AND | BPF_X: A &= X; continue; case BPF_ALU | BPF_OR | BPF_X: A |= X; continue; case BPF_ALU | BPF_LSH | BPF_X: A <<= X; continue; case BPF_ALU | BPF_RSH | BPF_X: A >>= X; continue; case BPF_ALU | BPF_ADD | BPF_K: A += bpf->k; continue; case BPF_ALU | BPF_SUB | BPF_K: A -= bpf->k; continue; case BPF_ALU | BPF_MUL | BPF_K: A *= bpf->k; continue; case BPF_ALU | BPF_DIV | BPF_K: A /= bpf->k; continue; case BPF_ALU | BPF_AND | BPF_K: A &= bpf->k; continue; case BPF_ALU | BPF_OR | BPF_K: A |= bpf->k; continue; case BPF_ALU | BPF_LSH | BPF_K: A <<= bpf->k; continue; case BPF_ALU | BPF_RSH | BPF_K: A >>= bpf->k; continue; case BPF_ALU | BPF_NEG: A = -A; continue; case BPF_MISC | BPF_TAX: X = A; continue; case BPF_MISC | BPF_TXA: A = X; continue; } } }
int sendToAllNode(register const uint8 *p,unsigned int buflen, struct timeval ts) { struct filter_node * root_node = master_filter.filter_first_node, * node_tmp; struct filter_item * pc; register uint32 A = 0, X = 0; register int k = 0, iterateur = 0; sint32 mem[BPF_MEMWORDS]; while(root_node != NULL) { master_filter.current_breakpoint = -1; pc = root_node->item; restart_mainloop: while(1) { /*printf("FILTER : %s\n",filter_item_image(pc));*/ switch (pc->code) { default: fprintf(stderr,"(capturefilterlib) sendToAllNode, warning unknwon bpf instruction\n"); return -1; case BPF_RET|BPF_K: if( ((u_int)pc->k) != 0) { /*printf("SEND %u\n",buflen);*/ buflen -= link_info.header_size; if( write(pc->k,&ts,sizeof(struct timeval)) < 0 || write(pc->k,&buflen,sizeof(unsigned int)) < 0 || write(pc->k,&p[link_info.header_size],buflen) < 0 ) { /*soit le wait a fermé le canal, soit on a reçu une interruption en provenance du CONTROL*/ /*TODO WRITE, reprendre l'ecriture ou elle s'est arretée*/ } buflen += link_info.header_size; } while(master_filter.current_breakpoint >= 0) { if(master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct != NULL) { /*restore break point*/ pc = master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct->item; A = master_filter.breakpoint_stack[master_filter.current_breakpoint].A; X = master_filter.breakpoint_stack[master_filter.current_breakpoint].X; k = master_filter.breakpoint_stack[master_filter.current_breakpoint].k; /*copy the mem*/ for(iterateur = 0;iterateur < BPF_MEMWORDS;iterateur+=1) { mem[iterateur] = master_filter.breakpoint_stack[master_filter.current_breakpoint].mem[iterateur]; } /*prepare the next break point*/ master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct = master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct->next; goto restart_mainloop; } master_filter.current_breakpoint--; } goto MAIN_WHILE_CONTINUE; case BPF_RET|BPF_A: if( ((u_int)A) != 0) { /*printf("SEND %u\n",buflen);*/ buflen -= link_info.header_size; if( write(pc->k,&ts,sizeof(struct timeval)) < 0 || write(pc->k,&buflen,sizeof(unsigned int)) < 0 || write(pc->k,&p[link_info.header_size],buflen) < 0 ) { /*soit le wait a fermé le canal, soit on a reçu une interruption en provenance du CONTROL*/ /*TODO WRITE, reprendre l'ecriture ou elle s'est arretée*/ } buflen += link_info.header_size; } while(master_filter.current_breakpoint >= 0) { if(master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct != NULL) { /*restore break point*/ pc = master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct->item; A = master_filter.breakpoint_stack[master_filter.current_breakpoint].A; X = master_filter.breakpoint_stack[master_filter.current_breakpoint].X; k = master_filter.breakpoint_stack[master_filter.current_breakpoint].k; /*copy the mem*/ for(iterateur = 0;iterateur < BPF_MEMWORDS;iterateur+=1) { mem[iterateur] = master_filter.breakpoint_stack[master_filter.current_breakpoint].mem[iterateur]; } /*prepare the next break point*/ master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct = master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct->next; goto restart_mainloop; } master_filter.current_breakpoint--; } goto MAIN_WHILE_CONTINUE; case BPF_LD|BPF_W|BPF_ABS: k = pc->k; if (k + sizeof(sint32) > buflen) { return 0; } A = EXTRACT_LONG(&p[k]); break; case BPF_LD|BPF_H|BPF_ABS: k = pc->k; if (k + sizeof(short) > buflen) { return 0; } A = EXTRACT_SHORT(&p[k]); break; case BPF_LD|BPF_B|BPF_ABS: k = pc->k; if (k >= buflen) { return 0; } A = p[k]; break; case BPF_LD|BPF_W|BPF_LEN: A = buflen; break; case BPF_LDX|BPF_W|BPF_LEN: X = buflen; break; case BPF_LD|BPF_W|BPF_IND: k = X + pc->k; if (k + sizeof(sint32) > buflen) { return 0; } A = EXTRACT_LONG(&p[k]); break; case BPF_LD|BPF_H|BPF_IND: k = X + pc->k; if (k + sizeof(short) > buflen) { return 0; } A = EXTRACT_SHORT(&p[k]); break; case BPF_LD|BPF_B|BPF_IND: k = X + pc->k; if (k >= buflen) { return 0; } A = p[k]; break; case BPF_LDX|BPF_MSH|BPF_B: k = pc->k; if (k >= buflen) { return 0; } X = (p[pc->k] & 0xf) << 2; break; case BPF_LD|BPF_IMM: A = pc->k; break; case BPF_LDX|BPF_IMM: X = pc->k; break; case BPF_LD|BPF_MEM: A = mem[pc->k]; break; case BPF_LDX|BPF_MEM: X = mem[pc->k]; /*printf("X = %u\n",X);*/ break; case BPF_ST: mem[pc->k] = A; break; case BPF_STX: mem[pc->k] = X; break; case BPF_JMP|BPF_JA: /*pc += pc->k;*/ break; case BPF_JMP|BPF_JGT|BPF_K: node_tmp = (A > pc->k) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JGE|BPF_K: node_tmp = (A >= pc->k) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JEQ|BPF_K: node_tmp = (A == pc->k) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JSET|BPF_K: node_tmp = (A & pc->k) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JGT|BPF_X: node_tmp = (A > X) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JGE|BPF_X: node_tmp = (A >= X) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JEQ|BPF_X: node_tmp = (A == X) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_JMP|BPF_JSET|BPF_X: node_tmp = (A & X) ? pc->next_child_t : pc->next_child_f; goto jump; case BPF_ALU|BPF_ADD|BPF_X: A += X; break; case BPF_ALU|BPF_SUB|BPF_X: A -= X; break; case BPF_ALU|BPF_MUL|BPF_X: A *= X; break; case BPF_ALU|BPF_DIV|BPF_X: if (X == 0) return 0; A /= X; break; case BPF_ALU|BPF_AND|BPF_X: A &= X; break; case BPF_ALU|BPF_OR|BPF_X: A |= X; break; case BPF_ALU|BPF_LSH|BPF_X: A <<= X; break; case BPF_ALU|BPF_RSH|BPF_X: A >>= X; break; case BPF_ALU|BPF_ADD|BPF_K: A += pc->k; break; case BPF_ALU|BPF_SUB|BPF_K: A -= pc->k; break; case BPF_ALU|BPF_MUL|BPF_K: A *= pc->k; break; case BPF_ALU|BPF_DIV|BPF_K: A /= pc->k; break; case BPF_ALU|BPF_AND|BPF_K: A &= pc->k; break; case BPF_ALU|BPF_OR|BPF_K: A |= pc->k; break; case BPF_ALU|BPF_LSH|BPF_K: A <<= pc->k; break; case BPF_ALU|BPF_RSH|BPF_K: A >>= pc->k; break; case BPF_ALU|BPF_NEG: A = -A; break; case BPF_MISC|BPF_TAX: X = A; break; case BPF_MISC|BPF_TXA: A = X; break; } pc = pc->next_child_t->item; continue; jump: /*need to create a break point?*/ if(node_tmp->next != NULL) { master_filter.current_breakpoint++; if(master_filter.current_breakpoint == master_filter.max_filter_level_count) { fprintf(stderr,"(capturefilterlib) sendToAllNode, breakpoint stack overflow\n"); return -1; } master_filter.breakpoint_stack[master_filter.current_breakpoint].next_instruct = node_tmp->next; master_filter.breakpoint_stack[master_filter.current_breakpoint].A = A; master_filter.breakpoint_stack[master_filter.current_breakpoint].X = X; master_filter.breakpoint_stack[master_filter.current_breakpoint].k = k; /*copy the memory*/ for(iterateur = 0;iterateur < BPF_MEMWORDS;iterateur+=1) { master_filter.breakpoint_stack[master_filter.current_breakpoint].mem[iterateur] = mem[iterateur]; } } pc = node_tmp->item; } MAIN_WHILE_CONTINUE: root_node = root_node->next; } return 0; }