int archEthRecvPacket(UInt8** buffer, UInt32* length)
{
static UInt8 buf[65536];
if (ethIf.pcapHandle == NULL) {
return 0;
}
if (ethIf.packetOffset >= ethIf.packetLenth) {
ethIf.packetOffset = 0;
ethIf.packetLenth = 0;
pcapPacketInitPacket(ethIf.pkRecv, (char*)buf, sizeof(buf));
if (pcapPacketReceivePacket(ethIf.pcapHandle, ethIf.pkRecv, TRUE)) {
ethIf.packetLenth = ethIf.pkRecv->ulBytesReceived;
}
}
if (ethIf.packetOffset < ethIf.packetLenth) {
UInt8* packetStart = (UInt8*)ethIf.pkRecv->Buffer + ethIf.packetOffset;
struct bpf_hdr* hdr = (struct bpf_hdr*)packetStart;
*buffer = packetStart + hdr->bh_hdrlen;
*length = hdr->bh_caplen;
ethIf.packetOffset += Packet_WORDALIGN(hdr->bh_hdrlen + hdr->bh_caplen);
return 1;
}
return 0;
}
int _recvPacket(void *pData) {
struct bpf_hdr *hdr;
u8 *data;
int ret=0;
int size;
while (lbytes > 0) {
hdr = (struct bpf_hdr *)buf;
// DEV9_LOG("hdr %d,%d,%d\n", hdr->bh_hdrlen, hdr->bh_caplen, hdr->bh_datalen);
// DEV9_LOG("lbytes %d\n", lbytes);
data = buf+hdr->bh_hdrlen;
size = Packet_WORDALIGN(hdr->bh_hdrlen+hdr->bh_datalen);
buf+= size; lbytes-= size;
if (_filterPacket(data)) {
struct bpf_stat stat;
ret = hdr->bh_datalen;
memcpy(pData, data, ret);
if(PacketGetStats(lpAdapter,&stat)==FALSE){
printf("Warning: unable to get stats from the kernel!\n");
}
// printf("_recvPacket %d (tbytes=%d, packets=%d, lost=%d, time=%d)\n", ret, tbytes, stat.bs_recv,stat.bs_drop, timeGetTime());
// printf("%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x\n", data[5], data[4], data[3], data[2], data[1], data[0]);
break;
}
}
return ret;
}
void checkpackets() // check for and process packets
{
PacketReceivePacket(lpAdapter,lpPacket,TRUE);
char *data;
data = (char *)lpPacket->Buffer;
int tbytes;
tbytes = lpPacket->ulBytesReceived;
int reader = 0;
while(reader < tbytes)
{
bpf_hdr *header;
header=(struct bpf_hdr *)(data+reader);
int datalength = header->bh_datalen;
int totallength = header->bh_caplen;
reader = reader + header->bh_hdrlen;
char *readme;
readme = data+reader;
printpacket((unsigned char *)readme, datalength);
reader = Packet_WORDALIGN(reader+datalength);
}
}
void PrintPackets(LPPACKET lpPacket)
{
ULONG i, j, ulLines, ulen, ulBytesReceived;
char *pChar, *pLine, *base;
char *buf;
u_int off=0;
u_int tlen,tlen1;
struct bpf_hdr *hdr;
ulBytesReceived = lpPacket->ulBytesReceived;
buf = lpPacket->Buffer;
off=0;
while(off<ulBytesReceived){
if(kbhit())return;
hdr=(struct bpf_hdr *)(buf+off);
tlen1=hdr->bh_datalen;
tlen=hdr->bh_caplen;
printf("Packet length, captured portion: %ld, %ld\n", tlen1, tlen);
off+=hdr->bh_hdrlen;
ulLines = (tlen + 15) / 16;
pChar =(char*)(buf+off);
base=pChar;
off=Packet_WORDALIGN(off+tlen);
for ( i=0; i<ulLines; i++ )
{
pLine =pChar;
printf( "%08lx : ", pChar-base );
ulen=tlen;
ulen = ( ulen > 16 ) ? 16 : ulen;
tlen -= ulen;
for ( j=0; j<ulen; j++ )
printf( "%02x ", *(BYTE *)pChar++ );
if ( ulen < 16 )
printf( "%*s", (16-ulen)*3, " " );
pChar = pLine;
for ( j=0; j<ulen; j++, pChar++ )
printf( "%c", isprint( (unsigned char)*pChar ) ? *pChar : '.' );
printf( "\n" );
}
printf( "\n" );
}
}
//抓包处理函数,解析出单个Packet
void NetworkInterface::SplitPackets(LPPACKET &lpPacket)
{
ULONG ulBytesReceived;
char *pChar;
char *buf;
u_int off = 0;
//caplen为捕获的数据长度，datalen为原始数据长度
//caplen可能小于datalen
u_int caplen, datalen;
struct bpf_hdr *hdr;
//收到的字节数
ulBytesReceived = lpPacket->ulBytesReceived;
//缓冲区首地址
buf = (char*)lpPacket->Buffer;
//初始化偏移量为0
off = 0;
while (off < ulBytesReceived)
{
hdr = (struct bpf_hdr *)(buf + off);
datalen = hdr->bh_datalen;
caplen = hdr->bh_caplen;
//校验捕获的包是否完整,对于不完整的包不做处理
if(caplen != datalen)
break;
off += hdr->bh_hdrlen;
//单包数据段首地址
pChar = (char*)(buf + off);
//跳到下一个包的首地址（可能不存在）
off = Packet_WORDALIGN(off + caplen);
//对于自己的Packet不做响应
if(memcmp((pChar + 6), srcMAC, 6) == 0)
continue;
/*在此处处理每个数据包，首地址为pChar,长度为caplen*/
//在此处调用外部函数处理数据包
//PrintPacket((u_char*)pChar, caplen, 0);
if(p != NULL)
p->HandlePacket((u_char*)pChar, caplen);
}
}
void bx_win32_pktmover_c::rx_timer(void)
{
// Recieve Packet ????
char *pBuf;
unsigned char *pPacket;
unsigned int iOffset = 0;
struct bpf_hdr *hdr;
int pktlen;
PacketInitPacket(pkRecv, (char *)buffer, 256000);
if (WaitForSingleObject(lpAdapter->ReadEvent,0) == WAIT_OBJECT_0 || IsNT) {
PacketReceivePacket(lpAdapter, pkRecv, TRUE);
pBuf = (char *)pkRecv->Buffer;
iOffset = 0;
while(iOffset < pkRecv->ulBytesReceived)
{
hdr = (struct bpf_hdr *)(pBuf + iOffset);
pPacket = (unsigned char *)(pBuf + iOffset + hdr->bh_hdrlen);
if (memcmp(pPacket + 6, cMacAddr, 6) != 0) // src field != ours
{
if(memcmp(pPacket, cMacAddr, 6) == 0 || memcmp(pPacket, broadcast_macaddr, 6) == 0)
{
pktlen = hdr->bh_caplen;
if (pktlen < 60) pktlen = 60;
#if BX_ETH_WIN32_LOGGING
fprintf (pktlog_txt, "a packet from host to guest, length %u\n", pktlen);
Bit8u *charbuf = (Bit8u *)pPacket;
int n;
for (n=0; n<pktlen; n++) {
if (((n % 16) == 0) && n>0)
fprintf (pktlog_txt, "\n");
fprintf (pktlog_txt, "%02x ", (unsigned)charbuf[n]);
}
fprintf (pktlog_txt, "\n--\n");
fflush (pktlog_txt);
#endif
(*rx_handler)(rx_Arg, pPacket, pktlen);
}
}
iOffset = Packet_WORDALIGN(iOffset + (hdr->bh_hdrlen + hdr->bh_caplen));
}
}
}
/**
* Process a packet buffer (which can hold multiple packets) and feed
* every packet to process_input().
*
* @param adapter adapter handle received by a call to init_adapter
* @param lpPacket the packet buffer to process
*/
static void
ProcessPackets(void *adapter, LPPACKET lpPacket)
{
struct packet_adapter *pa = (struct packet_adapter*)adapter;
ULONG ulLines, ulBytesReceived;
char *base;
char *buf;
u_int off = 0;
u_int tlen, tlen1;
struct bpf_hdr *hdr;
void *cur_packet;
int cur_length;
if (pa == NULL) {
return;
}
ulBytesReceived = lpPacket->ulBytesReceived;
buf = (char*)lpPacket->Buffer;
off=0;
while (off < ulBytesReceived) {
hdr = (struct bpf_hdr *)(buf + off);
tlen1 = hdr->bh_datalen;
cur_length = tlen1;
tlen = hdr->bh_caplen;
off += hdr->bh_hdrlen;
ulLines = (tlen + 15) / 16;
if (ulLines > 5) {
ulLines = 5;
}
base =(char*)(buf + off);
cur_packet = base;
off = Packet_WORDALIGN(off + tlen);
pa->input(pa->input_fn_arg, cur_packet, cur_length);
}
}
void SniffPacketThread::Run()
{
u_int tlen = 0, offset = 0;
struct bpf_hdr *hdr = NULL;
while(1)
{
if(m_stop_flag) break;
try
{
if(PacketReceivePacket(m_lpa, m_lpp, TRUE) == FALSE)
continue;
}
catch(...)
{
TRACE0("Exception in packet receive thread");
return;
}
// Grab total length
tlen = m_lpp->ulBytesReceived;
// Process all the packets we've been handed
offset = 0;
while(offset < tlen)
{
hdr = (struct bpf_hdr *)(m_pbuf + offset);
offset += hdr->bh_hdrlen;
unsigned char *pkt = (unsigned char *)(m_pbuf + offset);
u_int len = hdr->bh_caplen;
// Adjust offset to next pkt w/ alignment
offset = Packet_WORDALIGN(offset + hdr->bh_caplen);
// we don't need to free the packet since it's static
// for this loop
m_parent->ProcessSniffedPacket( pkt, len );
}
Sleep(1);
}
}
static int
pcap_read_win32_npf(pcap_t *p, int cnt, pcap_handler callback, u_char *user)
{
PACKET Packet;
int cc;
int n = 0;
register u_char *bp, *ep;
u_char *datap;
struct pcap_win *pw = p->priv;
cc = p->cc;
if (p->cc == 0) {
/*
* Has "pcap_breakloop()" been called?
*/
if (p->break_loop) {
/*
* Yes - clear the flag that indicates that it
* has, and return PCAP_ERROR_BREAK to indicate
* that we were told to break out of the loop.
*/
p->break_loop = 0;
return (PCAP_ERROR_BREAK);
}
/*
* Capture the packets.
*
* The PACKET structure had a bunch of extra stuff for
* Windows 9x/Me, but the only interesting data in it
* in the versions of Windows that we support is just
* a copy of p->buffer, a copy of p->buflen, and the
* actual number of bytes read returned from
* PacketReceivePacket(), none of which has to be
* retained from call to call, so we just keep one on
* the stack.
*/
PacketInitPacket(&Packet, (BYTE *)p->buffer, p->bufsize);
if (!PacketReceivePacket(pw->adapter, &Packet, TRUE)) {
pcap_snprintf(p->errbuf, PCAP_ERRBUF_SIZE, "read error: PacketReceivePacket failed");
return (PCAP_ERROR);
}
cc = Packet.ulBytesReceived;
bp = p->buffer;
}
else
bp = p->bp;
/*
* Loop through each packet.
*/
#define bhp ((struct bpf_hdr *)bp)
ep = bp + cc;
for (;;) {
register int caplen, hdrlen;
/*
* Has "pcap_breakloop()" been called?
* If so, return immediately - if we haven't read any
* packets, clear the flag and return PCAP_ERROR_BREAK
* to indicate that we were told to break out of the loop,
* otherwise leave the flag set, so that the *next* call
* will break out of the loop without having read any
* packets, and return the number of packets we've
* processed so far.
*/
if (p->break_loop) {
if (n == 0) {
p->break_loop = 0;
return (PCAP_ERROR_BREAK);
} else {
p->bp = bp;
p->cc = (int) (ep - bp);
return (n);
}
}
if (bp >= ep)
break;
caplen = bhp->bh_caplen;
hdrlen = bhp->bh_hdrlen;
datap = bp + hdrlen;
/*
* Short-circuit evaluation: if using BPF filter
* in kernel, no need to do it now - we already know
* the packet passed the filter.
*
* XXX - bpf_filter() should always return TRUE if
* handed a null pointer for the program, but it might
* just try to "run" the filter, so we check here.
*/
if (pw->filtering_in_kernel ||
p->fcode.bf_insns == NULL ||
bpf_filter(p->fcode.bf_insns, datap, bhp->bh_datalen, caplen)) {
#ifdef ENABLE_REMOTE
switch (p->rmt_samp.method) {
case PCAP_SAMP_1_EVERY_N:
pw->samp_npkt = (pw->samp_npkt + 1) % p->rmt_samp.value;
/* Discard all packets that are not '1 out of N' */
if (pw->samp_npkt != 0) {
bp += Packet_WORDALIGN(caplen + hdrlen);
continue;
}
break;
case PCAP_SAMP_FIRST_AFTER_N_MS:
{
struct pcap_pkthdr *pkt_header = (struct pcap_pkthdr*) bp;
/*
* Check if the timestamp of the arrived
* packet is smaller than our target time.
*/
if (pkt_header->ts.tv_sec < pw->samp_time.tv_sec ||
(pkt_header->ts.tv_sec == pw->samp_time.tv_sec && pkt_header->ts.tv_usec < pw->samp_time.tv_usec)) {
bp += Packet_WORDALIGN(caplen + hdrlen);
continue;
}
/*
* The arrived packet is suitable for being
* delivered to our caller, so let's update
* the target time.
*/
pw->samp_time.tv_usec = pkt_header->ts.tv_usec + p->rmt_samp.value * 1000;
if (pw->samp_time.tv_usec > 1000000) {
pw->samp_time.tv_sec = pkt_header->ts.tv_sec + pw->samp_time.tv_usec / 1000000;
pw->samp_time.tv_usec = pw->samp_time.tv_usec % 1000000;
}
}
}
#endif /* ENABLE_REMOTE */
/*
* XXX A bpf_hdr matches a pcap_pkthdr.
*/
(*callback)(user, (struct pcap_pkthdr*)bp, datap);
bp += Packet_WORDALIGN(caplen + hdrlen);
if (++n >= cnt && !PACKET_COUNT_IS_UNLIMITED(cnt)) {
p->bp = bp;
p->cc = (int) (ep - bp);
return (n);
}
} else {
/*
* Skip this packet.
*/
bp += Packet_WORDALIGN(caplen + hdrlen);
}
}
#undef bhp
p->cc = 0;
return (n);
}
// Read next packet
UINT EthGetPacket(ETH *e, void **data)
{
BLOCK *b;
bool flag = false;
// Validate arguments
if (e == NULL || data == NULL)
{
return INFINITE;
}
if (e->HasFatalError)
{
return INFINITE;
}
if (e->SuAdapter != NULL)
{
// Read packet with SeLow
UINT size;
if (SuGetNextPacket(e->SuAdapter, data, &size) == false)
{
// Error occurred
e->HasFatalError = true;
return INFINITE;
}
return size;
}
RETRY:
// Check the presence of the packet in queue
b = GetNext(e->PacketQueue);
if (b != NULL)
{
UINT size;
size = b->Size;
*data = b->Buf;
Free(b);
if (e->PacketQueue->num_item == 0)
{
e->Empty = true;
}
return size;
}
if (e->Empty)
{
e->Empty = false;
return 0;
}
if (flag == false)
{
// Try to get next packet
PROBE_STR("EthGetPacket: PacketInitPacket");
wp->PacketInitPacket(e->Packet, e->Buffer, e->BufferSize);
PROBE_STR("EthGetPacket: PacketReceivePacket");
if (wp->PacketReceivePacket(e->Adapter, e->Packet, false) == false)
{
// Failed
return INFINITE;
}
else
{
UCHAR *buf;
UINT total;
UINT offset;
buf = (UCHAR *)e->Packet->Buffer;
total = e->Packet->ulBytesReceived;
offset = 0;
while (offset < total)
{
struct bpf_hdr *header;
UINT packet_size;
UCHAR *packet_data;
header = (struct bpf_hdr *)(buf + offset);
packet_size = header->bh_caplen;
offset += header->bh_hdrlen;
packet_data = buf + offset;
offset = Packet_WORDALIGN(offset + packet_size);
if (packet_size >= 14)
{
UCHAR *tmp;
BLOCK *b;
PROBE_DATA2("EthGetPacket: NewBlock", packet_data, packet_size);
tmp = MallocFast(packet_size);
Copy(tmp, packet_data, packet_size);
b = NewBlock(tmp, packet_size, 0);
InsertQueue(e->PacketQueue, b);
}
}
flag = true;
goto RETRY;
}
}
// No more packet
return 0;
}
/**
* Our low-level capture thread.
*
* Loop waiting for packet(s) in PacketReceivePacket(). In
* _pkt_release() the receive event-handle (adapter->Readvent) is
* set. This causes WaitForSingleObject() in PacketReceivePacket()
* to return. It is vital that the ADAPTER object isn't deallocated
* before PacketReceivePacket() returns.
*
* The return-value is > 0 if thread exited on it's own. Otherwise
* it is 0 (from GetExitCodeThread).
*/
DWORD __stdcall pkt_recv_thread (void *arg)
{
int rc = 0;
while (1)
{
const ADAPTER *adapter;
const BYTE *pkt, *pkt_end;
size_t cap_len, hdr_len;
int total_len, chunk;
PACKET npf_pkt;
if (!_pkt_inf || _pkt_inf->stop_thread) /* signals closure */
{
rc = 1;
break;
}
adapter = (const ADAPTER*) _pkt_inf->adapter;
npf_pkt.Length = _pkt_inf->npf_buf_size;
npf_pkt.Buffer = _pkt_inf->npf_buf;
if (!PacketReceivePacket(adapter, &npf_pkt, TRUE))
{
rc = 2;
break;
}
total_len = npf_pkt.ulBytesReceived;
ENTER_CRIT();
for (pkt = npf_pkt.Buffer, pkt_end = (BYTE*)npf_pkt.Buffer + total_len, chunk = 1;
pkt < pkt_end;
pkt += Packet_WORDALIGN(cap_len+hdr_len), chunk++)
{
struct pkt_ringbuf *q;
struct pkt_rx_element *head;
struct bpf_hdr *bp;
q = &_pkt_inf->pkt_queue;
bp = (struct bpf_hdr*) pkt;
cap_len = bp->bh_caplen;
hdr_len = bp->bh_hdrlen;
num_rx_bytes += bp->bh_datalen;
TCP_CONSOLE_MSG (2, ("pkt_recv_thread(): total_len %d, "
"chunk %d, cap_len %d, in-idx %d\n",
total_len, chunk, cap_len, q->in_index));
if (cap_len > ETH_MAX)
{
_pkt_inf->error = "Large size";
STAT (macstats.num_too_large++);
}
else
if (pktq_in_index(q) == q->out_index) /* queue is full, drop it */
q->num_drop++;
else
{
int pad_len, buf_max;
head = (struct pkt_rx_element*) pktq_in_buf (q);
memcpy (head->rx_buf, pkt + hdr_len, cap_len);
head->tstamp_put = bp->bh_tstamp;
head->rx_length = cap_len;
/* zero-pad up to ETH_MAX (don't destroy marker at end)
*/
buf_max = q->buf_size - RX_ELEMENT_HEAD_SIZE - 4;
pad_len = min (ETH_MAX, buf_max - cap_len);
if (pad_len > 0)
memset (&head->rx_buf[cap_len], 0, pad_len);
pktq_inc_in (q);
}
}
LEAVE_CRIT();
}
TCP_CONSOLE_MSG (2, ("pkt_recv_thread(): rc %d\n", rc));
fflush (stdout);
ARGSUSED (arg);
thr_stopped = TRUE;
return (rc);
}
static int
pcap_read_win32_npf(pcap_t *p, int cnt, pcap_handler callback, u_char *user)
{
int cc;
int n = 0;
register u_char *bp, *ep;
cc = p->cc;
if (p->cc == 0) {
/*
* Has "pcap_breakloop()" been called?
*/
if (p->break_loop) {
/*
* Yes - clear the flag that indicates that it
* has, and return -2 to indicate that we were
* told to break out of the loop.
*/
p->break_loop = 0;
return (-2);
}
/* capture the packets */
if(PacketReceivePacket(p->adapter,p->Packet,TRUE)==FALSE){
snprintf(p->errbuf, PCAP_ERRBUF_SIZE, "read error: PacketReceivePacket failed");
return (-1);
}
cc = p->Packet->ulBytesReceived;
bp = p->Packet->Buffer;
}
else
bp = p->bp;
/*
* Loop through each packet.
*/
#define bhp ((struct bpf_hdr *)bp)
ep = bp + cc;
while (1) {
register int caplen, hdrlen;
/*
* Has "pcap_breakloop()" been called?
* If so, return immediately - if we haven't read any
* packets, clear the flag and return -2 to indicate
* that we were told to break out of the loop, otherwise
* leave the flag set, so that the *next* call will break
* out of the loop without having read any packets, and
* return the number of packets we've processed so far.
*/
if (p->break_loop) {
if (n == 0) {
p->break_loop = 0;
return (-2);
} else {
p->bp = bp;
p->cc = ep - bp;
return (n);
}
}
if (bp >= ep)
break;
caplen = bhp->bh_caplen;
hdrlen = bhp->bh_hdrlen;
/*
* XXX A bpf_hdr matches a pcap_pkthdr.
*/
(*callback)(user, (struct pcap_pkthdr*)bp, bp + hdrlen);
bp += Packet_WORDALIGN(caplen + hdrlen);
if (++n >= cnt && cnt > 0) {
p->bp = bp;
p->cc = ep - bp;
return (n);
}
}
#undef bhp
p->cc = 0;
return (n);
}
static int
pcap_read_win32_npf(pcap_t *p, int cnt, pcap_handler callback, u_char *user)
{
int cc;
int n = 0;
register u_char *bp, *ep;
u_char *datap;
struct pcap_win *pw = p->priv;
cc = p->cc;
if (p->cc == 0) {
/*
* Has "pcap_breakloop()" been called?
*/
if (p->break_loop) {
/*
* Yes - clear the flag that indicates that it
* has, and return PCAP_ERROR_BREAK to indicate
* that we were told to break out of the loop.
*/
p->break_loop = 0;
return (PCAP_ERROR_BREAK);
}
/* capture the packets */
if(PacketReceivePacket(p->adapter,p->Packet,TRUE)==FALSE){
snprintf(p->errbuf, PCAP_ERRBUF_SIZE, "read error: PacketReceivePacket failed");
return (PCAP_ERROR);
}
cc = p->Packet->ulBytesReceived;
bp = p->Packet->Buffer;
}
else
bp = p->bp;
/*
* Loop through each packet.
*/
#define bhp ((struct bpf_hdr *)bp)
ep = bp + cc;
while (1) {
register int caplen, hdrlen;
/*
* Has "pcap_breakloop()" been called?
* If so, return immediately - if we haven't read any
* packets, clear the flag and return PCAP_ERROR_BREAK
* to indicate that we were told to break out of the loop,
* otherwise leave the flag set, so that the *next* call
* will break out of the loop without having read any
* packets, and return the number of packets we've
* processed so far.
*/
if (p->break_loop) {
if (n == 0) {
p->break_loop = 0;
return (PCAP_ERROR_BREAK);
} else {
p->bp = bp;
p->cc = ep - bp;
return (n);
}
}
if (bp >= ep)
break;
caplen = bhp->bh_caplen;
hdrlen = bhp->bh_hdrlen;
datap = bp + hdrlen;
/*
* Short-circuit evaluation: if using BPF filter
* in kernel, no need to do it now - we already know
* the packet passed the filter.
*
* XXX - bpf_filter() should always return TRUE if
* handed a null pointer for the program, but it might
* just try to "run" the filter, so we check here.
*/
if (pw->filtering_in_kernel ||
p->fcode.bf_insns == NULL ||
bpf_filter(p->fcode.bf_insns, datap, bhp->bh_datalen, caplen)) {
/*
* XXX A bpf_hdr matches a pcap_pkthdr.
*/
(*callback)(user, (struct pcap_pkthdr*)bp, datap);
bp += Packet_WORDALIGN(caplen + hdrlen);
if (++n >= cnt && !PACKET_COUNT_IS_UNLIMITED(cnt)) {
p->bp = bp;
p->cc = ep - bp;
return (n);
}
} else {
/*
* Skip this packet.
*/
bp += Packet_WORDALIGN(caplen + hdrlen);
}
}
#undef bhp
p->cc = 0;
return (n);
}
