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;
}
