netif_init_bpf(netif_t *netif, const char *name, uint16_t port, struct sock_filter *filter, int filter_len)
#endif
{
bool result = true;
struct ifaddrs *ifas;
struct ifaddrs *ifa;
#if __FreeBSD__
struct ifreq ifr; /* ioctl call to get VID */
struct vlanreq vreq; /* ioctl call to get VID */
#elif __linux__
struct ifreq ifr; /* ioctl call to get MAC address*/
struct rtnl_link_stats *stats;
struct vlan_ioctl_args ifv; /* ioctl call to get VID */
struct sockaddr_ll addr; /* to bind packet socket to interface */
struct sockaddr_in dummy_addr; /* to bind dummy socket to interface */
struct sock_fprog prog; /* to attach to filter */
#endif
/* string copy name */
strncpy(netif->name, name, NETIF_NAME_SIZE);
/* set to zero */
memcpy(netif->mac.addr, MAC_ADDRESS_NULL.addr, MAC_ADDRESS_LEN);
netif->vlan = NULL;
netif->ipv4 = NULL;
netif->ipv6 = NULL;
/*** get the index of the interface ***/
netif->index = if_nametoindex(netif->name);
if (netif->index == 0) {
LOG_PRINTLN(LOG_NETWORK_INTERFACE, LOG_ERROR, ("interface '%s' is not known", netif->name));
return false;
}
/* create socket */
if ((netif->socket = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) < 0) {
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("socket failed"));
return false;
}
/* get interface addresses */
if (getifaddrs(&ifas) != 0) {
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("getifaddrs failed"));
return false;
}
for (ifa = ifas; ifa != NULL; ifa = ifa->ifa_next) {
if ((ifa->ifa_addr) == NULL) continue;
if ((ifa->ifa_flags & IFF_UP) == 0) continue;
/* network interface name matches */
if (strcmp(name, ifa->ifa_name) == 0) {
switch (ifa->ifa_addr->sa_family) {
case AF_INET: netif_add_ipv4_address(netif, IPV4_ADDRESS(&(INADDR(ifa->ifa_addr)->sin_addr)),
IPV4_ADDRESS(&(INADDR(ifa->ifa_netmask)->sin_addr)),
IPV4_ADDRESS(&(INADDR(ifa->ifa_broadaddr)->sin_addr)),
NULL);
break;
case AF_INET6: netif_add_ipv6_address(netif, IPV6_ADDRESS(&(INADDR6(ifa->ifa_addr)->sin6_addr)),
IPV6_ADDRESS(&(INADDR6(ifa->ifa_netmask)->sin6_addr)),
IPV6_STATE_VALID);
break;
#if __FreeBSD__
case AF_LINK: netif_add_mac_address(netif, MAC_ADDRESS(LLADDR(LADDR(ifa->ifa_addr))));
bzero((char *) &ifr, sizeof(ifr));
bzero((char *) &vreq, sizeof(vreq));
strncpy(ifr.ifr_name, netif->name, NETIF_NAME_SIZE);
ifr.ifr_data = (caddr_t) &vreq;
if (ioctl(sockfd, SIOCGETVLAN, &ifr) != -1) {
netif_add_vid(netif, vreq.vlr_tag);
}
break;
#elif __linux__
case AF_PACKET: stats = ifa->ifa_data;
LOG_PRINTLN(LOG_NETWORK_INTERFACE, LOG_DEBUG, ("tx packet: %" PRIu32 " rx packet: %" PRIu32 " tx bytes: %" PRIu32 " rx bytes: %" PRIu32,
stats->tx_packets, stats->rx_packets, stats->tx_bytes, stats->rx_bytes));
/* get MAC address, see netdevice(7) */
bzero((char *) &ifr, sizeof(ifr));
strncpy(ifr.ifr_name, netif->name, NETIF_NAME_SIZE);
if (ioctl(netif->socket, SIOCGIFHWADDR, &ifr) != -1) {
netif_add_mac_address(netif, MAC_ADDRESS(LLADDR(LADDR(ifr.ifr_hwaddr.sa_data))));
bzero((char *) &ifv, sizeof(ifv));
ifv.cmd = GET_VLAN_VID_CMD;
strncpy(ifv.device1, netif->name, sizeof(ifv.device1));
if (ioctl(netif->socket, SIOCGIFVLAN, &ifv) != -1) {
netif_add_vid(netif, ifv.u.VID);
}
} else {
LOG_PRINTLN(LOG_NETWORK_INTERFACE, LOG_ERROR, ("couldn't get MAC address"));
result = false;
goto netif_init_exit;
}
break;
#endif
default: continue;
}
}
}
/* bind packet socket to interface */
bzero(&addr, sizeof(addr));
addr.sll_family = AF_PACKET;
addr.sll_protocol = htons(ETH_P_ALL);
addr.sll_ifindex = netif->index;
if (bind(netif->socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
close(netif->socket);
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("can't bind packet socket to interface"));
return false;
}
#if __FreeBSD__
#elif __linux__
if (filter != NULL) {
prog.filter = filter;
prog.len = filter_len;
if (setsockopt(netif->socket, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0) {
close(netif->socket);
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("can't add BPF filter"));
return false;
}
}
#endif
if (netif->ipv4 != NULL && port > 0) {
/* create dummy socket ***/
if ((netif->dummy_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
close(netif->socket);
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("can't create dummy socket"));
return false;
}
/* bind dummy socket to interface */
bzero(&dummy_addr, sizeof(dummy_addr));
dummy_addr.sin_family = AF_INET;
dummy_addr.sin_addr.s_addr = netif->ipv4->address.addr32;
dummy_addr.sin_port = htons(port);
if (bind(netif->dummy_socket, (struct sockaddr *) &dummy_addr, sizeof(dummy_addr)) < 0) {
close(netif->socket);
close(netif->dummy_socket);
LOG_ERRNO(LOG_NETWORK_INTERFACE, LOG_ERROR, errno, ("can't bind dummy socket to interface"));
return false;
}
}
/* clear receive buffer */
if (!netif_clear_receive_buffer(netif)) {
return false;
}
netif_init_exit:
freeifaddrs(ifas);
return result;
}
int vm86_emulate(struct vm86_context *vm86ctx)
{
int eaten = 1;
int data32 = 0, addr32 = 0, rep = 0, segp = 0;
int done = 0;
uint16_t ip = LOWORD(vm86ctx->eip),
sp = LOWORD(vm86ctx->esp);
do {
switch(*(uint8_t *)LADDR(vm86ctx->cs, ip)) {
case 0x66: /*32-bit data*/ data32=1; break;
case 0x67: /*32-bit addr*/ addr32=1; break;
case 0xf2: /*REPNZ/REPNE*/ rep=1; break;
case 0xf3: /*REP/REPZ/REPE*/ rep=1; break;
case 0x2e: /*CS*/ segp=56; break;
case 0x3e: /*DS*/ segp=76; break;
case 0x26: /*ES*/ segp=72; break;
case 0x36: /*SS*/ segp=68; break;
case 0x65: /*GS*/ segp=84; break;
case 0x64: /*FS*/ segp=80; break;
case 0xf0: /*LOCK*/ break;
default: done = 1; break;
}
if(done)
break;
ip++;
} while(1);
switch(*(uint8_t *)LADDR(vm86ctx->cs, ip)) {
case 0xfa: /*CLI*/
vm86ctx->eflags &= ~EFLAGS_IF;
ip++;
break;
case 0xfb: /*STI*/
vm86ctx->eflags |= EFLAGS_IF;
ip++;
break;
case 0x9c: /*PUSHF*/
if(data32) {
sp -= 4;
*(uint32_t *)LADDR(vm86ctx->ss, sp) = vm86ctx->eflags & 0x001cffff;
} else {
sp -= 2;
*(uint16_t *)LADDR(vm86ctx->ss, sp) = (uint16_t)vm86ctx->eflags;
}
ip++;
break;
case 0x9d: /*POPF*/
if(data32) {
uint32_t eflags = *(uint32_t *)LADDR(vm86ctx->ss, sp);
vm86ctx->eflags &= 0x1b3000/*VM, RF, IOPL, VIP, VIF*/;
eflags &= ~0x1b3000;
vm86ctx->eflags |= eflags;
sp += 4;
} else {
uint32_t eflags = *(uint16_t *)LADDR(vm86ctx->ss, sp);
vm86ctx->eflags &= 0xffff3000/*IOPL*/;
eflags &= ~0xffff3000;
vm86ctx->eflags |= eflags;
sp += 2;
}
ip++;
break;
case 0xcd: /*INT*/
sp -= 2;
*(uint16_t *)LADDR(vm86ctx->ss, sp) = (uint16_t)vm86ctx->eflags;
sp -= 2;
*(uint16_t *)LADDR(vm86ctx->ss, sp) = vm86ctx->cs;
sp -= 2;
*(uint16_t *)LADDR(vm86ctx->ss, sp) = ip + 2;
{
uint16_t *ivt = (uint16_t *)0;
uint8_t x = *(uint8_t *)LADDR(vm86ctx->cs, ip + 1);
ip = ivt[x * 2 + 0];
vm86ctx->cs = ivt[x * 2 + 1];
}
break;
case 0xcf: /*IRET*/
if(data32) {
ip = *(uint32_t *)LADDR(vm86ctx->ss, sp);
sp += 4;
vm86ctx->cs = *(uint32_t *)LADDR(vm86ctx->ss, sp);
sp += 4;
uint32_t eflags = *(uint32_t *)LADDR(vm86ctx->ss, sp);
vm86ctx->eflags &= 0x1a3000/*VM, IOPL, VIP, VIF*/;
eflags &= ~0x1a3000;
vm86ctx->eflags |= eflags;
sp += 4;
} else {
ip = *(uint16_t *)LADDR(vm86ctx->ss, sp);
sp += 2;
vm86ctx->cs = *(uint16_t *)LADDR(vm86ctx->ss, sp);
sp += 2;
uint32_t eflags = *(uint16_t *)LADDR(vm86ctx->ss, sp);
vm86ctx->eflags &= 0xffff3000/*IOPL*/;
eflags &= ~0xffff3000;
vm86ctx->eflags |= eflags;
sp += 2;
}
break;
case 0xe6:/*OUT imm8, AL*/
outportb(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1),
LOBYTE(LOWORD(vm86ctx->eax)));
ip += 2;
break;
case 0xe7:/*OUT imm8, (E)AX*/
if(data32) {
outportl(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1),
vm86ctx->eax);
} else {
outportw(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1),
LOWORD(vm86ctx->eax));
}
ip += 2;
break;
case 0xee:/*OUT DX, AL*/
outportb(LOWORD(vm86ctx->edx), LOBYTE(LOWORD(vm86ctx->eax)));
ip++;
break;
case 0xef:/*OUT DX, (E)AX*/
if(data32) {
outportl(LOWORD(vm86ctx->edx), vm86ctx->eax);
} else {
outportw(LOWORD(vm86ctx->edx), LOWORD(vm86ctx->eax));
}
ip++;
break;
case 0xe4:/*IN AL, imm8*/
{
uint8_t al = inportb(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1));
vm86ctx->eax = (vm86ctx->eax & 0xffffff00) | al;
}
ip += 2;
break;
case 0xe5:/*IN (E)AX, imm8*/
if(data32) {
vm86ctx->eax = inportl(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1));
} else {
uint16_t ax = inportw(*(uint8_t *)LADDR(vm86ctx->cs, ip + 1));
vm86ctx->eax = (vm86ctx->eax & 0xffff0000) | ax;
}
ip += 2;
break;
case 0xec:/*IN AL, DX*/
{
uint8_t al = inportb(LOWORD(vm86ctx->edx));
vm86ctx->eax = (vm86ctx->eax & 0xffffff00) | al;
}
ip += 1;
break;
case 0xed:/*IN (E)AX, DX*/
if(data32) {
vm86ctx->eax = inportl(LOWORD(vm86ctx->edx));
} else {
uint16_t ax = inportw(LOWORD(vm86ctx->edx));
vm86ctx->eax = (vm86ctx->eax & 0xffff0000) | ax;
}
ip++;
break;
case 0x6c:/*INSB; INS m8, DX*/
{
if(addr32) {
uint32_t ecx = rep ? vm86ctx->ecx : 1;
while(ecx) {
*(uint8_t *)LADDR(vm86ctx->es, vm86ctx->edi/*XXX*/) =
inportb(LOWORD(vm86ctx->edx));
ecx--;
vm86ctx->edi += (vm86ctx->eflags & 0x400)?-1:1;
}
if(rep)
vm86ctx->ecx = ecx;
} else {
uint16_t cx=rep ? LOWORD(vm86ctx->ecx) : 1,
di=LOWORD(vm86ctx->edi);
while(cx) {
*(uint8_t *)LADDR(vm86ctx->es, di) =
inportb(LOWORD(vm86ctx->edx));
cx--;
di += (vm86ctx->eflags & 0x400)?-1:1;
}
if(rep)
vm86ctx->ecx = (vm86ctx->ecx & 0xffff0000) | cx;
vm86ctx->edi = (vm86ctx->edi & 0xffff0000) | di;
}
}
ip++;
break;
case 0x6d:/*INSW; INSD; INS m16/m32, DX*/
{
if(addr32) {
uint32_t ecx = rep ? vm86ctx->ecx : 1;
while(ecx) {
if(data32) {
*(uint32_t *)LADDR(vm86ctx->es, vm86ctx->edi/*XXX*/) =
inportl(LOWORD(vm86ctx->edx));
vm86ctx->edi += (vm86ctx->eflags & 0x400)?-4:4;
} else {
*(uint16_t *)LADDR(vm86ctx->es, vm86ctx->edi/*XXX*/) =
inportw(LOWORD(vm86ctx->edx));
vm86ctx->edi += (vm86ctx->eflags & 0x400)?-2:2;
}
ecx--;
}
if(rep)
vm86ctx->ecx = ecx;
} else {
uint16_t cx=rep ? LOWORD(vm86ctx->ecx) : 1,
di=LOWORD(vm86ctx->edi);
while(cx) {
if(data32) {
*(uint32_t *)LADDR(vm86ctx->es, di) =
inportl(LOWORD(vm86ctx->edx));
di += (vm86ctx->eflags & 0x400)?-4:4;
} else {
*(uint16_t *)LADDR(vm86ctx->es, di) =
inportw(LOWORD(vm86ctx->edx));
di += (vm86ctx->eflags & 0x400)?-2:2;
}
cx--;
}
if(rep)
vm86ctx->ecx = (vm86ctx->ecx & 0xffff0000) | cx;
vm86ctx->edi = (vm86ctx->edi & 0xffff0000) | di;
}
}
ip++;
break;
case 0x6e:/*OUTSB; OUTS DX, m8*/
{
uint16_t seg = vm86ctx->ds;
if(segp)
seg = *(uint16_t *)(((uint8_t *)vm86ctx)+segp);
if(addr32) {
uint32_t ecx = rep ? vm86ctx->ecx : 1;
while(ecx) {
outportb(LOWORD(vm86ctx->edx),
*(uint8_t *)LADDR(seg, vm86ctx->esi/*XXX*/));
ecx--;
vm86ctx->esi += (vm86ctx->eflags & 0x400)?-1:1;
}
if(rep)
vm86ctx->ecx = ecx;
} else {
uint16_t cx=rep ? LOWORD(vm86ctx->ecx) : 1,
si=LOWORD(vm86ctx->esi);
while(cx) {
outportb(LOWORD(vm86ctx->edx), *(uint8_t *)LADDR(seg, si));
cx--;
si += (vm86ctx->eflags & 0x400)?-1:1;
}
if(rep)
vm86ctx->ecx = (vm86ctx->ecx & 0xffff0000) | cx;
vm86ctx->esi = (vm86ctx->esi & 0xffff0000) | si;
}
}
ip++;
break;
case 0x6f:/*OUTSW; OUTSD; OUTS DX, m16/32*/
{
uint16_t seg = vm86ctx->ds;
if(segp)
seg = *(uint16_t *)(((uint8_t *)vm86ctx)+segp);
if(addr32) {
uint32_t ecx = rep ? vm86ctx->ecx : 1;
while(ecx) {
if(data32) {
outportl(LOWORD(vm86ctx->edx),
*(uint32_t *)LADDR(seg, vm86ctx->esi/*XXX*/));
vm86ctx->esi += (vm86ctx->eflags & 0x400)?-4:4;
} else {
outportw(LOWORD(vm86ctx->edx),
*(uint16_t *)LADDR(seg, vm86ctx->esi/*XXX*/));
vm86ctx->esi += (vm86ctx->eflags & 0x400)?-2:2;
}
ecx--;
}
if(rep)
vm86ctx->ecx = ecx;
} else {
uint16_t cx=rep ? LOWORD(vm86ctx->ecx) : 1,
si=LOWORD(vm86ctx->esi);
while(cx) {
if(data32) {
outportl(LOWORD(vm86ctx->edx), *(uint32_t *)LADDR(seg, si));
si += (vm86ctx->eflags & 0x400)?-4:4;
} else {
outportw(LOWORD(vm86ctx->edx), *(uint16_t *)LADDR(seg, si));
si += (vm86ctx->eflags & 0x400)?-2:2;
}
cx--;
}
if(rep)
vm86ctx->ecx = (vm86ctx->ecx & 0xffff0000) | cx;
vm86ctx->esi = (vm86ctx->esi & 0xffff0000) | si;
}
}
ip++;
break;
default:
eaten = 0;
break;
}
vm86ctx->eip = (vm86ctx->eip & 0xffff0000) | ip;
vm86ctx->esp = (vm86ctx->esp & 0xffff0000) | sp;
return eaten;
}
