/* Attempt to convert from the PCAP_FRAMES environment variable used by Phil Wood's PCAP-Ring
to a buffer size I can pass to PCAP 1.0.0's pcap_set_buffer_size(). */
static int translate_PCAP_FRAMES(int snaplen)
{
# ifdef HAVE_LINUX_IF_PACKET_H
char *frames_str = getenv("PCAP_FRAMES");
int frame_size, block_size, frames_per_block;
int frames;
if (!frames_str)
return 0;
/* Look, I didn't make these numbers and calculations up, I'm just using them. */
frame_size = TPACKET_ALIGN(snaplen + TPACKET_ALIGN(TPACKET_HDRLEN) + sizeof(struct sockaddr_ll));
block_size = getpagesize();
while (block_size < frame_size)
block_size <<= 1;
frames_per_block = block_size / frame_size;
if (strncmp(frames_str, "max", 3) && strncmp(frames_str, "MAX", 3))
frames = strtol(frames_str, NULL, 10);
else
frames = 0x8000; /* Default maximum of 32k frames. */
printf("PCAP_FRAMES -> %d * %d / %d = %d (%d)\n", frames, block_size, frames_per_block, frames * block_size / frames_per_block, frame_size);
return frames * block_size / frames_per_block;
# else
return 0;
# endif
}
static int calculate_layout(AFPacket_Context_t *afpc, AFPacketInstance *instance, int order)
{
/* Calculate the frame size and minimum block size required. */
instance->layout.tp_frame_size = TPACKET_ALIGN(afpc->snaplen + TPACKET_ALIGN(TPACKET_ALIGN(instance->tp_hdrlen) + sizeof(struct sockaddr_ll) + ETH_HLEN) - ETH_HLEN);
instance->layout.tp_block_size = getpagesize() << order;
while (instance->layout.tp_block_size < instance->layout.tp_frame_size)
instance->layout.tp_block_size <<= 1;
instance->frames_per_block = instance->layout.tp_block_size / instance->layout.tp_frame_size;
if (instance->frames_per_block == 0)
{
DPE(afpc->errbuf, "%s: Invalid frames per block (%u/%u) for %s",
__FUNCTION__, instance->layout.tp_block_size, instance->layout.tp_frame_size, afpc->device);
return DAQ_ERROR;
}
/* Find the total number of frames required to amount to the requested per-interface memory.
Then find the number of blocks required to hold those packet buffer frames. */
instance->layout.tp_frame_nr = afpc->size / instance->layout.tp_frame_size;
instance->layout.tp_block_nr = instance->layout.tp_frame_nr / instance->frames_per_block;
/* afpc->layout.tp_frame_nr is requested to match frames_per_block*n_blocks */
instance->layout.tp_frame_nr = instance->layout.tp_block_nr * instance->frames_per_block;
if (afpc->debug)
{
printf("AFPacket Layout:\n");
printf(" Frame Size: %u\n", instance->layout.tp_frame_size);
printf(" Frames: %u\n", instance->layout.tp_frame_nr);
printf(" Block Size: %u (Order %d)\n", instance->layout.tp_block_size, order);
printf(" Blocks: %u\n", instance->layout.tp_block_nr);
}
return DAQ_SUCCESS;
}
static void walk_t3_block(struct block_desc *pbd, struct ctx *ctx,
int sock, int *fd)
{
int num_pkts = pbd->h1.num_pkts, i;
struct tpacket3_hdr *hdr;
struct sockaddr_ll *sll;
hdr = (void *) ((uint8_t *) pbd + pbd->h1.offset_to_first_pkt);
sll = (void *) ((uint8_t *) hdr + TPACKET_ALIGN(sizeof(*hdr)));
for (i = 0; i < num_pkts && likely(sigint == 0); ++i) {
uint8_t *packet = ((uint8_t *) hdr + hdr->tp_mac);
pcap_pkthdr_t phdr;
if (ctx->packet_type != -1)
if (ctx->packet_type != sll->sll_pkttype)
goto next;
ctx->pkts_seen++;
if (dump_to_pcap(ctx)) {
int ret;
tpacket3_hdr_to_pcap_pkthdr(hdr, sll, &phdr, ctx->magic);
ret = __pcap_io->write_pcap(*fd, &phdr, ctx->magic, packet,
pcap_get_length(&phdr, ctx->magic));
if (unlikely(ret != (int) pcap_get_total_length(&phdr, ctx->magic)))
panic("Write error to pcap!\n");
}
__show_frame_hdr(packet, hdr->tp_snaplen, ctx->link_type, sll,
hdr, ctx->print_mode, true, ctx->pkts_seen);
dissector_entry_point(packet, hdr->tp_snaplen, ctx->link_type,
ctx->print_mode, sll);
next:
hdr = (void *) ((uint8_t *) hdr + hdr->tp_next_offset);
sll = (void *) ((uint8_t *) hdr + TPACKET_ALIGN(sizeof(*hdr)));
if (frame_count_max != 0) {
if (unlikely(ctx->pkts_seen >= frame_count_max)) {
sigint = 1;
break;
}
}
update_pcap_next_dump(ctx, hdr->tp_snaplen, fd, sock, true);
}
}
static int afpacket_daq_inject(void *handle, const DAQ_PktHdr_t *hdr, const uint8_t *packet_data, uint32_t len, int reverse)
{
AFPacket_Context_t *afpc = (AFPacket_Context_t *) handle;
AFPacketInstance *instance;
AFPacketEntry *entry;
/* Find the instance that the packet was received on. */
for (instance = afpc->instances; instance; instance = instance->next)
{
if (instance->index == hdr->ingress_index)
break;
}
if (!instance || (!reverse && !(instance = instance->peer)))
return DAQ_ERROR;
entry = instance->tx_ring.cursor;
if (entry->hdr.h2->tp_status == TP_STATUS_AVAILABLE)
{
memcpy(entry->hdr.raw + TPACKET_ALIGN(instance->tp_hdrlen), packet_data, len);
entry->hdr.h2->tp_len = len;
entry->hdr.h2->tp_status = TP_STATUS_SEND_REQUEST;
instance->tx_ring.cursor = entry->next;
if (send(instance->fd, NULL, 0, 0) < 0)
{
DPE(afpc->errbuf, "%s: Error sending packet: %s (%d)", __FUNCTION__, strerror(errno), errno);
return DAQ_ERROR;
}
afpc->stats.packets_injected++;
}
return DAQ_SUCCESS;
}
// Acquire a frame from the ringbuffer. Start writing, given return value
// 'frame', at: (char *)frame + ((struct tpacket_hdr *)frame)->tp_mac.
void *get_tx_frame(interface *i,size_t *fsize){
struct tpacket_hdr *thdr;
void *ret;
assert(pthread_mutex_lock(&i->lock) == 0);
thdr = i->curtxm;
if(thdr == NULL){
pthread_mutex_unlock(&i->lock);
diagnostic("Can't transmit on %s (fd %d)",i->name,i->fd);
return NULL;
}
if(thdr->tp_status != TP_STATUS_AVAILABLE){
if(thdr->tp_status != TP_STATUS_WRONG_FORMAT){
pthread_mutex_unlock(&i->lock);
diagnostic("No available TX frames on %s",i->name);
return NULL;
}
thdr->tp_status = TP_STATUS_AVAILABLE;
}
// Need indicate that this one is in use, but don't want to
// indicate that it should be sent yet
thdr->tp_status = TP_STATUS_PREPARING;
// FIXME we ought be able to set this once for each packet, and be done
thdr->tp_net = thdr->tp_mac = TPACKET_ALIGN(sizeof(struct tpacket_hdr));
ret = i->curtxm;
i->curtxm += inclen(&i->txidx,&i->ttpr);
pthread_mutex_unlock(&i->lock);
*fsize = i->ttpr.tp_frame_size;
return ret;
}
static int calculate_layout(AFPacket_Context_t *afpc, struct tpacket_req *layout, unsigned int tp_hdrlen, int order)
{
unsigned int tp_hdrlen_sll, netoff, frames_per_block;
/* Calculate the frame size and minimum block size required. */
tp_hdrlen_sll = TPACKET_ALIGN(tp_hdrlen) + sizeof(struct sockaddr_ll);
netoff = TPACKET_ALIGN(tp_hdrlen_sll + ETH_HLEN) + VLAN_TAG_LEN;
layout->tp_frame_size = TPACKET_ALIGN(netoff - ETH_HLEN + afpc->snaplen);
layout->tp_block_size = getpagesize() << order;
while (layout->tp_block_size < layout->tp_frame_size)
layout->tp_block_size <<= 1;
frames_per_block = layout->tp_block_size / layout->tp_frame_size;
if (frames_per_block == 0)
{
DPE(afpc->errbuf, "%s: Invalid frames per block (%u/%u) for %s",
__FUNCTION__, layout->tp_block_size, layout->tp_frame_size, afpc->device);
return DAQ_ERROR;
}
/* Find the total number of frames required to amount to the requested per-interface memory.
Then find the number of blocks required to hold those packet buffer frames. */
layout->tp_frame_nr = afpc->size / layout->tp_frame_size;
layout->tp_block_nr = layout->tp_frame_nr / frames_per_block;
/* afpc->layout.tp_frame_nr is requested to match frames_per_block*n_blocks */
layout->tp_frame_nr = layout->tp_block_nr * frames_per_block;
if (afpc->debug)
{
printf("AFPacket Layout:\n");
printf(" Frame Size: %u\n", layout->tp_frame_size);
printf(" Frames: %u\n", layout->tp_frame_nr);
printf(" Block Size: %u (Order %d)\n", layout->tp_block_size, order);
printf(" Blocks: %u\n", layout->tp_block_nr);
}
return DAQ_SUCCESS;
}
// Returns 0 on failure, otherwise size of the ringbuffer. On a failure,
// contents of treq are unspecified. blknum == 1024 for 4MiB rings.
static size_t
size_mmap_psocket(struct tpacket_req *treq,unsigned maxframe,unsigned blknum){
unsigned fperblk;
// Must be a multiple of TPACKET_ALIGNMENT, and the following must
// hold: TPACKET_HDRLEN <= tp_frame_size <= tp_block_size.
treq->tp_frame_size = TPACKET_ALIGN(TPACKET_HDRLEN + maxframe);
if(get_block_size(treq->tp_frame_size,&treq->tp_block_size) < 0){
return 0;
}
fperblk = treq->tp_block_size / treq->tp_frame_size;
// Use the entire block, if there would otherwise be wasted space. This
// is useful to catch radiotap, small GRO etc without PACKET_COPY_THRESH.
treq->tp_frame_size = treq->tp_block_size / fperblk;
// Array of pointers to blocks, allocated via slab -- cannot be
// larger than largest slabbable allocation. FIXME do better
treq->tp_block_nr = blknum / (treq->tp_block_size / getpagesize());
// tp_frame_nr is derived from the other three parameters.
treq->tp_frame_nr = (treq->tp_block_size / treq->tp_frame_size)
* treq->tp_block_nr;
return treq->tp_block_nr * treq->tp_block_size;
}
void us_mfa::data_available(raw_socket *sock) {
#ifndef LINUX_NO_MMAP
if (sock->m_mmapped) {
int i = 0;
while (i < MAX_EAT_ONE_CYCLE && *(unsigned long *)sock->m_mmapbuf) {
tpacket_hdr *hdr = (tpacket_hdr *)sock->m_mmapbuf;
sockaddr_ll *sa = (sockaddr_ll *)(((uint8_t *)hdr)
+ TPACKET_ALIGN(sizeof(*hdr)));
uint8_t *bp = ((uint8_t *)hdr) + hdr->tp_mac;
if (sa->sll_protocol == htons(ETH_P_IPV6)
&& sa->sll_pkttype != PACKET_OUTGOING)
handle_ipv6(sa->sll_ifindex, bp, hdr->tp_len);
hdr->tp_status = 0;
sock->m_mmapbuf += sock->m_framesize;
if (sock->m_mmapbuf >= (((uint8_t *)sock->m_mmapped) + sock->m_mmappedlen))
sock->m_mmapbuf = (uint8_t *)sock->m_mmapped;
i++;
}
} else {
#endif
sockaddr_ll sa;
socklen_t salen = sizeof(sa);
int len;
while ((len = g_mrd->ipktb->recvfrom(sock->fd(),
(sockaddr *)&sa, &salen)) > 0) {
if (sa.sll_protocol == htons(ETH_P_IPV6)
&& sa.sll_pkttype != PACKET_OUTGOING)
handle_ipv6(sa.sll_ifindex,
g_mrd->ipktb->buffer(), len);
}
#ifndef LINUX_NO_MMAP
}
#endif
}
void receiveData(int n, siginfo_t *info, void *unused){
int i;
/*somehow the value is not sent correctly..., so for now we use code to indicate the fragment nr */
// printf("###received data frame nr%i % value %i###\n", i, info->si_value.sival_int);
// printf("signr %i, sending pid %i , int %i\n", info->si_signo, info->si_pid, info->si_int);
// printf("receive from frame nr %i, \n", info->si_code);
i = info->si_code;
// while(*(unsigned long*)ring[i].iov_base) { /* there is a packet, loop until all packets processed*/
struct tpacket_hdr *h=ring[i].iov_base;
struct sockaddr_ll *sll=(void *)h + TPACKET_ALIGN(sizeof(*h));
unsigned char *bp=(unsigned char *)h + h->tp_mac; /* the captured packet starts here*/
char * tmp = (char *)h;
char * res = tmp + h->tp_net;
// printf("packet received: %s\n", res);
packets++;
bytes+= h->tp_len;
/* "free this packet for further use, e.g. sender can overwrite it */
h->tp_status = READ;
received++;
if(received == TOTAL){
gettimeofday(&t_end, NULL);
long long start = 1000000 * t_start.tv_sec + t_start.tv_usec;
long long end = 1000000 * t_end.tv_sec + t_end.tv_usec;
// int secs = t_end.tv_sec - t_start.tv_sec;
// int usecs = t_end.tv_usec - t_start.tv_usec;
long long delta = end - start;
printf("totusec %lli, total %lli\n", delta, TOTAL);
// long double rate = ((double)TOTAL / (double)delta); /// 1000000;
// printf("received all packets in %i sec and %i microsec\n", secs, usecs);
// printf("packet rate Packets/sec: %ld\n", rate);
}
// i=(i==req.tp_frame_nr-1) ? 0 : i+1; /* just look whether there is already an other packet avaiable */
// }
}
static int
afpacket_daq_acquire(void *handle, int cnt,
DAQ_Analysis_Func_t callback, DAQ_Meta_Func_t metaback, void *user)
{
AFPacket_Context_t *afpc = (AFPacket_Context_t *) handle;
AFPacketInstance *instance;
DAQ_PktHdr_t daqhdr;
DAQ_Verdict verdict;
union thdr hdr;
struct pollfd pfd[AF_PACKET_MAX_INTERFACES];
const uint8_t *data;
uint32_t i;
int got_one, ignored_one;
int ret, c = 0;
unsigned int tp_len, tp_mac, tp_snaplen, tp_sec, tp_usec;
while (c < cnt || cnt <= 0)
{
got_one = 0;
ignored_one = 0;
for (instance = afpc->instances; instance; instance = instance->next)
{
/* Has breakloop() been called? */
if (afpc->break_loop)
{
afpc->break_loop = 0;
return 0;
}
hdr = instance->rx_ring.cursor->hdr;
if (instance->tp_version == TPACKET_V2 && (hdr.h2->tp_status & TP_STATUS_USER))
{
switch (instance->tp_version)
{
case TPACKET_V2:
tp_len = hdr.h2->tp_len;
tp_mac = hdr.h2->tp_mac;
tp_snaplen = hdr.h2->tp_snaplen;
tp_sec = hdr.h2->tp_sec;
tp_usec = hdr.h2->tp_nsec / 1000;
break;
default:
DPE(afpc->errbuf, "%s: Unknown TPACKET version: %u!", __FUNCTION__, instance->tp_version);
return DAQ_ERROR;
}
if (tp_mac + tp_snaplen > instance->rx_ring.layout.tp_frame_size)
{
DPE(afpc->errbuf, "%s: Corrupted frame on kernel ring (MAC offset %u + CapLen %u > FrameSize %d)",
__FUNCTION__, tp_mac, tp_snaplen, instance->rx_ring.layout.tp_frame_size);
return DAQ_ERROR;
}
data = instance->rx_ring.cursor->hdr.raw + tp_mac;
/* Make a valiant attempt at reconstructing the VLAN tag if it has been stripped. This really sucks. :( */
if ((instance->tp_version == TPACKET_V2) &&
#if defined(TP_STATUS_VLAN_VALID)
(hdr.h2->tp_vlan_tci || (hdr.h2->tp_status & TP_STATUS_VLAN_VALID)) &&
#else
hdr.h2->tp_vlan_tci &&
#endif
tp_snaplen >= (unsigned int) vlan_offset)
{
struct vlan_tag *tag;
data -= VLAN_TAG_LEN;
memmove((void *) data, data + VLAN_TAG_LEN, vlan_offset);
tag = (struct vlan_tag *) (data + vlan_offset);
tag->vlan_tpid = htons(ETH_P_8021Q);
tag->vlan_tci = htons(hdr.h2->tp_vlan_tci);
tp_snaplen += VLAN_TAG_LEN;
tp_len += VLAN_TAG_LEN;
}
verdict = DAQ_VERDICT_PASS;
if (afpc->fcode.bf_insns && sfbpf_filter(afpc->fcode.bf_insns, data, tp_len, tp_snaplen) == 0)
{
ignored_one = 1;
afpc->stats.packets_filtered++;
goto send_packet;
}
got_one = 1;
daqhdr.ts.tv_sec = tp_sec;
daqhdr.ts.tv_usec = tp_usec;
daqhdr.caplen = tp_snaplen;
daqhdr.pktlen = tp_len;
daqhdr.ingress_index = instance->index;
daqhdr.egress_index = instance->peer ? instance->peer->index : DAQ_PKTHDR_UNKNOWN;
daqhdr.ingress_group = DAQ_PKTHDR_UNKNOWN;
daqhdr.egress_group = DAQ_PKTHDR_UNKNOWN;
daqhdr.flags = 0;
daqhdr.opaque = 0;
daqhdr.priv_ptr = NULL;
daqhdr.address_space_id = 0;
if (callback)
{
verdict = callback(user, &daqhdr, data);
if (verdict >= MAX_DAQ_VERDICT)
verdict = DAQ_VERDICT_PASS;
afpc->stats.verdicts[verdict]++;
verdict = verdict_translation_table[verdict];
}
afpc->stats.packets_received++;
c++;
send_packet:
if (verdict == DAQ_VERDICT_PASS && instance->peer)
{
AFPacketEntry *entry = instance->peer->tx_ring.cursor;
int rc;
if (entry->hdr.h2->tp_status == TP_STATUS_AVAILABLE)
{
memcpy(entry->hdr.raw + TPACKET_ALIGN(instance->peer->tp_hdrlen), data, tp_snaplen);
entry->hdr.h2->tp_len = tp_snaplen;
entry->hdr.h2->tp_status = TP_STATUS_SEND_REQUEST;
rc = send(instance->peer->fd, NULL, 0, 0);
instance->peer->tx_ring.cursor = entry->next;
}
/* Else, don't forward the packet... */
}
/* Release the TPACKET buffer back to the kernel. */
switch (instance->tp_version)
{
case TPACKET_V2:
hdr.h2->tp_status = TP_STATUS_KERNEL;
break;
}
instance->rx_ring.cursor = instance->rx_ring.cursor->next;
}
}
if (!got_one && !ignored_one)
{
for (i = 0, instance = afpc->instances; instance; i++, instance = instance->next)
{
pfd[i].fd = instance->fd;
pfd[i].revents = 0;
pfd[i].events = POLLIN;
}
ret = poll(pfd, afpc->intf_count, afpc->timeout);
/* If we were interrupted by a signal, start the loop over. The user should call daq_breakloop to actually exit. */
if (ret < 0 && errno != EINTR)
{
DPE(afpc->errbuf, "%s: Poll failed: %s (%d)", __FUNCTION__, strerror(errno), errno);
return DAQ_ERROR;
}
/* If the poll times out, return control to the caller. */
if (ret == 0)
break;
/* If some number of of sockets have events returned, check them all for badness. */
if (ret > 0)
{
for (i = 0; i < afpc->intf_count; i++)
{
if (pfd[i].revents & (POLLHUP | POLLRDHUP | POLLERR | POLLNVAL))
{
if (pfd[i].revents & (POLLHUP | POLLRDHUP))
DPE(afpc->errbuf, "%s: Hang-up on a packet socket", __FUNCTION__);
else if (pfd[i].revents & POLLERR)
DPE(afpc->errbuf, "%s: Encountered error condition on a packet socket", __FUNCTION__);
else if (pfd[i].revents & POLLNVAL)
DPE(afpc->errbuf, "%s: Invalid polling request on a packet socket", __FUNCTION__);
return DAQ_ERROR;
}
}
}
}
}
return 0;
}
int main ( int argc, char **argv )
{
struct pollfd pfd;
struct sockaddr_ll addr;
int i;
signal(SIGINT, sigproc);
/* Open the packet socket */
if ( (fd=socket(PF_PACKET, SOCK_DGRAM, 0))<0 ) {
perror("socket()");
return 1;
}
/* Setup the fd for mmap() ring buffer */
req.tp_block_size=4096;
req.tp_frame_size=1024;
req.tp_block_nr=64;
req.tp_frame_nr=4*64;
if ( (setsockopt(fd,
SOL_PACKET,
PACKET_RX_RING,
(char *)&req,
sizeof(req))) != 0 ) {
perror("setsockopt()");
close(fd);
return 1;
};
/* mmap() the sucker */
map=mmap(NULL,
req.tp_block_size * req.tp_block_nr,
PROT_READ|PROT_WRITE|PROT_EXEC, MAP_SHARED, fd, 0);
if ( map==MAP_FAILED ) {
perror("mmap()");
close(fd);
return 1;
}
/* Setup our ringbuffer */
ring=malloc(req.tp_frame_nr * sizeof(struct iovec));
for(i=0; i<req.tp_frame_nr; i++) {
ring[i].iov_base=(void *)((long)map)+(i*req.tp_frame_size);
ring[i].iov_len=req.tp_frame_size;
}
/* bind the packet socket */
memset(&addr, 0, sizeof(addr));
addr.sll_family=AF_PACKET;
addr.sll_protocol=htons(0x03);
addr.sll_ifindex=0;
addr.sll_hatype=0;
addr.sll_pkttype=0;
addr.sll_halen=0;
if ( bind(fd, (struct sockaddr *)&addr, sizeof(addr)) ) {
munmap(map, req.tp_block_size * req.tp_block_nr);
perror("bind()");
close(fd);
return 1;
}
for(i=0;;) {
while(*(unsigned long*)ring[i].iov_base) {
struct tpacket_hdr *h=ring[i].iov_base;
struct sockaddr_ll *sll=(void *)h + TPACKET_ALIGN(sizeof(*h));
unsigned char *bp=(unsigned char *)h + h->tp_mac;
printf("%u.%.6u: if%u %s %u bytes\n",
h->tp_sec, h->tp_usec,
sll->sll_ifindex,
names[sll->sll_pkttype],
h->tp_len);
/* tell the kernel this packet is done with */
h->tp_status=0;
__sync_synchronize(); // in GCC 4.4+ //mb(); /* memory barrier */
i=(i==req.tp_frame_nr-1) ? 0 : i+1;
}
/* Sleep when nothings happening */
pfd.fd=fd;
pfd.events=POLLIN|POLLERR;
pfd.revents=0;
poll(&pfd, 1, -1);
}
return 0;
}
int
pcap_ring_recv(pcap_t *p, int cnt, pcap_handler callback, u_char *user)
{
struct iovec *ring = p->rg.iovec;
time_t poll_timeout = 0; /* milliseconds */
int n = 0;
#ifdef RING_STATS
/* p->md.timeout is in milliseconds */
time_t pdelta = 0; /* seconds */
int consec = 0;
if (!p->rg.statperiod)
{
if ((p->md.timeout) || (cnt>0) ) START_SECS(p) = 0;
}
/* else {
* START_SECS(p) = 0;
* }
2006 */
#else /*RING_STATS not defined*/
if ((p->md.timeout) || (cnt>0) ) START_SECS(p) = 0;
#endif /*RING_STATS*/
if (!p->rg.iovec) return (-1); /* called inappropriately (no ring defined) */
if (cnt<=0) cnt=-1; /* read foreaver */
for (;;)
{
while (PACKET_AVAILABLE)
{
struct tpacket_hdr *h = ring[p->rg.iovhead].iov_base;
struct sockaddr_ll *sll = (void*)h + TPACKET_ALIGN(sizeof(*h));
unsigned char *bp = (unsigned char*)h + h->tp_mac;
int delta; // recomputed for each packet
struct pcap_pkthdr h1;
struct sll_header *hdrp;
p->md.stat.ps_recv++;
p->rg.pollAgain = 0;
#ifdef RING_STATS
p->rg.rs.r_recv++;
p->rg.rs.r_bytes += h->tp_len;
consec++;
if (consec > p->rg.rs.r_consec)
p->rg.rs.r_consec = consec;
#endif /*RING_STATS*/
/*
if (p->md.use_bpf == 2)
{
if (sll->sll_pkttype != PACKET_OUTGOING)
{
memset(bp-14, 0, 6);
if (sll->sll_pkttype == PACKET_BROADCAST) memset(bp-14, 0xFF, 6);
else if (sll->sll_pkttype == PACKET_MULTICAST) *(bp-14) = 1;
else *(bp-14+5) = 1;
memcpy(bp-14+6, sll->sll_addr, 6);
}
else
{
memcpy(bp-14, sll->sll_addr, 6);
memset(bp-14+6, 0, 6);
}
*(unsigned short*)(bp-2) = sll->sll_protocol;
}
*/
if (p->md.cooked)
{
hdrp = (struct sll_header *)((char *)bp - sizeof(struct sll_header));
switch (sll->sll_pkttype) {
case PACKET_HOST:
hdrp->sll_pkttype = htons(LINUX_SLL_HOST);
break;
case PACKET_BROADCAST:
hdrp->sll_pkttype = htons(LINUX_SLL_BROADCAST);
break;
case PACKET_MULTICAST:
hdrp->sll_pkttype = htons(LINUX_SLL_MULTICAST);
break;
case PACKET_OTHERHOST:
hdrp->sll_pkttype = htons(LINUX_SLL_OTHERHOST);
break;
case PACKET_OUTGOING:
hdrp->sll_pkttype = htons(LINUX_SLL_OUTGOING);
break;
default:
hdrp->sll_pkttype = -1;
break;
}
hdrp->sll_hatype = htons(sll->sll_hatype);
hdrp->sll_halen = htons(sll->sll_halen);
memcpy(hdrp->sll_addr, sll->sll_addr,
(sll->sll_halen > SLL_ADDRLEN) ? SLL_ADDRLEN : sll->sll_halen);
hdrp->sll_protocol = sll->sll_protocol;
}
if ((p->fcode.bf_insns == NULL ||
bpf_filter(p->fcode.bf_insns, bp, h->tp_len, h->tp_snaplen)) &&
!(!p->md.sock_packet && sll->sll_ifindex == p->md.lo_ifindex &&
sll->sll_pkttype == PACKET_OUTGOING)
)
{
h1.ts.tv_sec = h->tp_sec;
h1.ts.tv_usec = h->tp_usec;
h1.caplen = h->tp_snaplen;
h1.len = h->tp_len;
/*
if (p->md.use_bpf == 2)
{
h1.caplen += 14;
h1.len += 14;
bp -= 14;
}
*/
if (p->md.cooked)
{
h1.caplen += p->md.offset;
h1.len += p->md.offset;
bp -= p->md.offset;
}
(*callback)(user, (void *)&h1, bp);
n++;
}
#ifdef RING_STATS
else
{
p->rg.rs.r_ignore++;
}
#endif
if (!(START_SECS(p)))
{
START_SECS(p) = h->tp_sec;
START_USECS(p) = h->tp_usec;
if (!p->rg.tea) p->rg.tea = h->tp_sec + p->rg.statperiod;
}
STOP_SECS(p) = h->tp_sec;
STOP_USECS(p) = h->tp_usec;
#ifdef RING_DEBUG
fprintf (stderr, "\nstart_secs=%d, stop_secs=%d\n", START_SECS(p), STOP_SECS(p));
#endif
h->tp_status = 0;
/* mb(); */
p->rg.iovhead = (p->rg.iovhead == p->rg.iovmax) ? 0 : p->rg.iovhead+1;
/* check for user specified timeout (in seconds) PCAP_TIMEOUT > 0 */
if (p->rg.timeout)
{
delta = (STOP_SECS(p)) - p->rg.timeout;
if (delta >= 0 )
{
#ifdef RING_STATS
if (p->rg.statperiod)
(void) packet_ring_stats (p, p->rg.statbits);
consec = 0;
#endif /*RING_STATS*/
snprintf(p->errbuf, sizeof(p->errbuf),
"User specified timeout occured");
errno = ETIMEDOUT;
return (-1);
}
}
#ifdef RING_STATS
{
//struct timeval tdelta = {0};
//TV_SUB(p->rg.rs.r_stop, p->rg.rs.r_start, tdelta)
if (p->rg.statperiod)
if (STOP_SECS(p) >= p->rg.tea)
{
do {
p->rg.tea += p->rg.statperiod;
pdelta += p->rg.statperiod;
} while ( p->rg.tea < STOP_SECS(p) );
#ifdef RING_DEBUG
fprintf (stderr, "dump: p->rg.tea: %d, START_SECS: %d, STOP_SECS: %d, pdelta: %d, statperiod: %d\n",
p->rg.tea, START_SECS(p), STOP_SECS(p), pdelta, p->rg.statperiod);
#endif /*RING_DEBUG*/
(void) packet_ring_stats (p, p->rg.statbits);
fflush (stderr);
consec = 0;
}
}
#endif /*RING_STATS*/
/* check to see if need to return after some delta time */
if (p->md.timeout > 0)
{
#ifdef RING_DEBUG
fprintf (stderr, "in md.timeout check\n");
#endif /*RING_DEBUG*/
/* should we use pdelta? */
#ifdef RING_STATS
if (p->rg.statperiod)
{
if (pdelta > 0)
{
poll_timeout = (p->md.timeout - (pdelta * 1000));
#ifdef RING_DEBUG
fprintf (stderr, "stat: md.timeout: %d, pdelta: %d, poll_timeout: %d\n", p->md.timeout, pdelta, poll_timeout);
#endif /*RING_DEBUG*/
if ((poll_timeout) <= 0) return n;
}
}
/* nope, calculate the time gone by */
else
#endif /*RING_STATS*/
{
delta = ((STOP_SECS(p) - START_SECS(p))*1000000)
+(STOP_USECS(p) - START_USECS(p));
if (delta > 0)
{
poll_timeout = p->md.timeout - (delta/1000);
#ifdef RING_DEBUG
fprintf (stderr, "goneby: md.timeout: %d, delta: %d, poll_timeout: %d\n", p->md.timeout, delta, poll_timeout);
#endif
if ((poll_timeout) <= 0)
return n;
}
}
}
/* check if need to return after some number of pkts */
if (cnt > 0)
{
if (n == cnt)
{
#ifdef RING_DEBUG
fprintf (stderr, "count satisfied: n is %d, ps_recv is %d\n",
n, p->md.stat.ps_recv);
#endif
#ifdef RING_STATS
if (p->rg.statperiod)
{
(void) packet_ring_stats (p, p->rg.statbits);
consec = 0;
}
#endif /*RING_STATS*/
return n;
}
}
} /* end of packet collection loop, get here when no packets on ring */
if (p->md.timeout == -1)
{
return n; /* calling program will poll, non-blocking IO */
}
/* p->md.timeout == 0, wait forever for packets */
/* p->md.timeout > 0, will return once we have exhausted the timeout */
while (!PACKET_AVAILABLE)
{
int pres;
struct pollfd pfd;
int delta;
#ifdef RING_TIME_STATS
struct timeval tv1, tv2;
struct timezone tz;
#endif
#ifdef RING_STATS
consec = 0;
#endif
pfd.fd = p->fd;
pollagain:
/*
* Has "pcap_breakloop()" been called?
*/
if (p->break_loop) {
/*
* Yes - clear the flag that indicates that it
* has, and return -2 as an indication that we
* were told to break out of the loop.
*/
p->break_loop = 0;
return -2;
}
p->rg.pollAgain++;
pfd.revents = 0;
pfd.events = POLLIN;
#ifdef RING_STATS
p->rg.rs.r_polls++;
#endif
#ifdef RING_TIME_STATS
if (gettimeofday (&tv1, &tz))
{
fprintf (stderr, "ring_stats: gettimeofday1, %s\n",
pcap_strerror(errno));
}
#endif
if (p->md.timeout == 0)
{ /* wait forever or until a packet arrives */
#ifdef RING_DEBUG
fprintf (stderr, "poll: p->md.timeout == 0, waiting for a packet.\n");
#endif
pres = poll(&pfd, 1, -1);
}
else
{ /* check if we have polled enough to exhaust the timeout */
delta = (p->md.timeout - poll_timeout);
#ifdef RING_DEBUG
fprintf (stderr, "poll: p->md.timeout == %d, delta: %d, poll_timeout: %d.\n",
p->md.timeout, delta, poll_timeout);
#endif
if (delta < 1) return (n) ;
/* else, wait for delta msecs on the clock */
pres = poll(&pfd, 1, delta);
}
#ifdef RING_TIME_STATS
if (gettimeofday (&tv2, &tz))
{
fprintf (stderr, "ring_stats: gettimeofday2, %s\n",
pcap_strerror(errno));
return (-1);
}
delta = ((tv2.tv_sec - tv1.tv_sec)*1000000) + tv2.tv_usec - tv1.tv_usec;
p->rg.rs.r_waits.tv_usec += (delta-((delta/1000000)*1000000));
if (p->rg.rs.r_waits.tv_usec >= 1000000)
{
delta = p->rg.rs.r_waits.tv_usec/1000000;
p->rg.rs.r_waits.tv_sec += delta;
p->rg.rs.r_waits.tv_usec -= (delta * 1000000);
}
STOP_SECS(p) = tv2.tv_sec;
STOP_USECS(p) = tv2.tv_usec;
#endif
if ((pres > 0) && (pfd.revents&POLLIN))
{ /* should mean that a packet is ready to be pulled off the ring */
if (p->rg.pollAgain<LOST_MY_HEAD) continue;
/*
* why am I here? Cause just previously poll returned i went to check
* the ring but there was no data! Has not happened in a long time.
*/
#ifdef RING_STATS
if (p->rg.rs.r_polls)
{
p->rg.rs.r_specious++;
}
#endif
if (packet_head_check(p) >= 0) continue;
if (p->rg.pollAgain < 120) continue;
pcap_pstats (p);
fprintf (stderr,
"pcap_ring_recv: Kernel not updating ring buffer, shutting down! \n");
return(-1);
}
else
{ /* something bad happened or we timed out*/
if (pres == 0)
{ /* ms went to zero, timeout */
p->rg.pollAgain = 0;
return n;
}
if (pres < 0)
{ /* system call failed */
if (errno == EINTR)
{
p->rg.pollAgain = 0;
goto pollagain;
}
sprintf(p->errbuf, "poll: bad juju, %s", pcap_strerror(errno));
return n ? : -1;
}
{ /* we have a packet event, pres > 0 and bad news */
int err;
socklen_t elen = sizeof(err);
if (getsockopt(p->fd, SOL_SOCKET, SO_ERROR, &err, &elen))
{
sprintf(p->errbuf, "getsockopt(SO_ERROR): %s", pcap_strerror(errno));
return n ? : -1;
}
if (err)
{
struct ifreq ifr;
if (err != ENODEV || p->md.device == NULL)
{
sprintf(p->errbuf, "read error: %s", pcap_strerror(err));
return n ? : -1;
}
fprintf(stderr, "Device %s is down.\n", p->md.device);
ifr.ifr_ifindex = 0;
strcpy(ifr.ifr_name, p->md.device);
if (ioctl(p->fd, SIOCGIFINDEX, &ifr) < 0)
{
if (errno != ENODEV)
{
sprintf(p->errbuf, "ioctl: %s", pcap_strerror(errno));
return n ? : -1;
}
fprintf(stderr, "Device %s does not exist anymore.\n", p->md.device);
return n ? : -1;
}
}
else
{
fprintf(stderr,
"Poll of device %s returns POLLERR, but SO_ERROR not set.\n",
p->md.device);
}
return n ? : -1;
} /* end of packet event */
int fm_raw_pkt_sock_open(char *dev_name, fm_raw_sock_data_t *info)
{
int raw_sock;
struct ifreq ifr;
struct sockaddr_ll sll;
struct packet_mreq mr;
struct tpacket_req req;
int idx, i, k;
printf("%s: starting.\n", __FUNCTION__);
/* open raw packet socket */
//raw_sock = socket(PF_PACKET, SOCK_RAW, 0);
raw_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
if(raw_sock < 0) {
printf("%s: raw socket: %s\n", __FUNCTION__, strerror(errno));
return raw_sock;
}
/* find kernel ifindex of device */
memset(&ifr, 0, sizeof(struct ifreq));
strncpy(ifr.ifr_name, dev_name, sizeof(ifr.ifr_name) - 1);
if(ioctl(raw_sock, SIOCGIFINDEX, &ifr) < 0) {
printf("%s: ioctl(SIOCGIFINDEX): %s\n",
__FUNCTION__, strerror(errno));
goto err_out;
}
/* bind socket to device */
memset(&sll, 0, sizeof(sll));
sll.sll_family = AF_PACKET;
sll.sll_ifindex = ifr.ifr_ifindex;
sll.sll_protocol = htons(ETH_P_ALL);
if(bind(raw_sock, (struct sockaddr*)&sll, sizeof(sll))) {
printf("%s: bind: %s\n",
__FUNCTION__, strerror(errno));
goto err_out;
}
/* set device promiscuous */
memset(&mr, 0, sizeof(mr));
mr.mr_ifindex = ifr.ifr_ifindex;
mr.mr_type = PACKET_MR_PROMISC;
if(setsockopt(raw_sock, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
(char*)&mr, sizeof(mr)) < 0) {
printf("%s: setsockopt promisc: %s\n",
__FUNCTION__, strerror(errno));
goto err_out;
}
#ifdef ENABLE_VLAN
int val, hdr_len;
socklen_t len;
/* Probe whether kernel supports TPACKET_V2 */
val = TPACKET_V2;
len = sizeof(val);
if (getsockopt(raw_sock, SOL_PACKET, PACKET_HDRLEN, &val, &len) < 0) {
if (errno == ENOPROTOOPT)
goto err_out;
printf("%s: Can't get TPACKET_V2 header len on socket %d: %s\n",
__FUNCTION__, raw_sock, strerror(errno));
goto err_out;
}
hdr_len = val;
printf("%s, TPACKET_V2 header lenth is: %d\n", __FUNCTION__, hdr_len);
/* Set raw packet socket version to V2 */
val = TPACKET_V2;
if (setsockopt(raw_sock, SOL_PACKET, PACKET_VERSION,
&val, sizeof(val)) < 0) {
printf("%s: Can't activate TPACKET_V2 on socket %d: %s\n",
__FUNCTION__, raw_sock, strerror(errno));
goto err_out;
}
/* Reserve space for VLAN tag reconstruction */
val = 4;
if (setsockopt(raw_sock, SOL_PACKET, PACKET_RESERVE,
&val, sizeof(val)) < 0) {
printf("%s: Can't set up reserve on socket %d: %s\n",
__FUNCTION__, raw_sock, strerror(errno));
goto err_out;
}
/* Enable auxiliary data to receive VLAN tag */
val = 1;
if (setsockopt(raw_sock, SOL_PACKET, PACKET_AUXDATA,
&val, sizeof(val)) == -1 && errno != ENOPROTOOPT) {
// if (setsockopt(raw_sock, SOL_PACKET, PACKET_AUXDATA,
// &val, sizeof(val)) < 0) {
printf("%s: setsockopt PACKET_AUXDATA: %s\n",
__FUNCTION__, strerror(errno));
goto err_out;
}
#endif
info->fd = raw_sock;
/* set up the packet ring */
req.tp_block_size = getpagesize();
req.tp_block_nr = info->buf_size/req.tp_block_size;
#ifdef ENABLE_VLAN
req.tp_frame_size = TPACKET_ALIGN(TPACKET2_HDRLEN) +
#else
req.tp_frame_size = TPACKET_ALIGN(TPACKET_HDRLEN) +
#endif
TPACKET_ALIGN(RAW_SOCK_MAX_FRM_SIZ+16);
req.tp_frame_nr = req.tp_block_nr * (req.tp_block_size / req.tp_frame_size);
if(setsockopt(raw_sock, SOL_PACKET, PACKET_RX_RING,
(void*)&req, sizeof(req))) {
printf("%s: setsockopt(PACKET_RX_RING): %s\n",
__FUNCTION__, strerror(errno));
return -1;
}
/* map packet ring to user space */
info->base = mmap(NULL, info->buf_size, (PROT_READ|PROT_WRITE),
(MAP_SHARED|MAP_LOCKED), raw_sock, 0);
if(info->base == MAP_FAILED) {
printf("%s: mmap: %s\n", __FUNCTION__, strerror(errno));
return -1;
}
/* set up iovecs to frames in packet buffer */
info->iovec = malloc(req.tp_frame_nr * sizeof(struct iovec));
if(!info->iovec) {
printf("%s: out of memory!\n", __FUNCTION__);
return -1;
}
/* initialize all iovecs */
for(idx=0, i=0; i<req.tp_block_nr ; i++) { /* for all blocks/pages */
for(k=0; k<(req.tp_block_size / req.tp_frame_size); k++, idx++) {
/* for each frame, whithin the block */
info->iovec[idx].iov_base = (char*)info->base +
(req.tp_block_size * i) + (req.tp_frame_size * k);
info->iovec[idx].iov_len = req.tp_frame_size;
}
}
info->iovhead = 0;
info->iovmax = req.tp_frame_nr - 1;
return raw_sock;
err_out:
if(raw_sock > 0)
close(raw_sock);
return -1;
}
int main ( int argc, char **argv )
{
struct pollfd pfd;
int i;
char opt;
int before= 0, after= 0, print= 0;
/* setup the signal handler for SIGANA,
* SIGANA is received when a new packet is available
*/
struct sigaction sig;
sig.sa_sigaction = receiveData;
sig.sa_flags = SA_SIGINFO;
sigaction(SIGANA, &sig, NULL);
/* Open the packet socket */
print = 1;
printf("Creating socket\n");
if ( (fd=socket(PF_ANA, SOCK_DGRAM, 0))<0 ) {
perror("socket()");
return 1;
}
/* Create the buffer in kernel space */
printf("Calling setsockopt PACKET_RX_RING\n");
req.tp_block_size=4096;
req.tp_frame_size=1024;
req.tp_block_nr=4;
req.tp_frame_nr=4*4;
if ( (setsockopt(fd, SOL_PACKET, PACKET_RX_RING, (char *)&req,sizeof(req))) != 0 ) {
perror("setsockopt(PACKET_RX_RING)");
close(fd);
return 1;
};
/* mmap() the memory */
fprintf(stderr, "Mmaping ring buffer\n");
map=mmap(NULL, req.tp_block_size * req.tp_block_nr, PROT_READ|PROT_WRITE|PROT_EXEC, MAP_SHARED, fd, 0);
if ( map==MAP_FAILED ) {
perror("mmap()");
close(fd);
return 1;
}
/* Setup our ringbuffer */
ring=malloc(req.tp_frame_nr * sizeof(struct iovec));
for(i=0; i<req.tp_frame_nr; i++) {
ring[i].iov_base=(void *)((long)map)+(i*req.tp_frame_size);
ring[i].iov_len=req.tp_frame_size;
}
/* send the pid to the kernel on which we want to receive the echos */
int pid = getpid();
if ( (setsockopt(fd, SOL_PACKET, USER_PID, (char *)&pid, sizeof(pid))) != 0 ) {
perror("setsockopt(USER_PID)");
close(fd);
return 1;
};
/* while true, send the messages */
i = 0;
int z = 0;
gettimeofday(&t_start, NULL);
for(z = 0; z < TOTAL; z++) {
// printf("send to frame nr: %i\n", i);
struct tpacket_hdr *h=ring[i].iov_base;
while(h->tp_status == WRITTEN){ /* memory ring full */
printf("error: ring is full\n");
sleep(1); /*just wait */
}
struct sockaddr_ll *sll=(void *)h + TPACKET_ALIGN(sizeof(*h));
int netoff = TPACKET_ALIGN(TPACKET_HDRLEN);
char * msg = "hello world";
char * tmp = (char *)h;
memcpy(tmp + netoff, "hello world", 12);
h->tp_len = 12; /* the length */
h->tp_snaplen = 0;
h->tp_mac = 0;
h->tp_net = netoff; /* offset where the message starts */
h->tp_status = WRITTEN; /* we have written something in this memory location */
/* the packet is now in memory -> inform the kernel, that there is a new packet
* we cheat here: instead of putting the frame index in the value field, we set value to NULL
* but the value len to the frame index
*/
if ( (setsockopt(fd, SOL_PACKET, ANA_PACKET_READY, NULL, i)) != 0 ) {
perror("setsockopt(ANA_PACKET_READY)");
close(fd);
return -1;
}
i=(i==req.tp_frame_nr-1) ? 0 : i+1;
// sleep(1);
}
return 0;
}
static void walk_t3_block(struct block_desc *pbd, struct ctx *ctx,
int sock, int *fd, unsigned long *frame_count)
{
uint8_t *packet;
int num_pkts = pbd->h1.num_pkts, i, ret;
struct tpacket3_hdr *hdr;
pcap_pkthdr_t phdr;
struct sockaddr_ll *sll;
hdr = (void *) ((uint8_t *) pbd + pbd->h1.offset_to_first_pkt);
sll = (void *) ((uint8_t *) hdr + TPACKET_ALIGN(sizeof(*hdr)));
for (i = 0; i < num_pkts && likely(sigint == 0); ++i) {
__label__ next;
packet = ((uint8_t *) hdr + hdr->tp_mac);
if (ctx->packet_type != -1)
if (ctx->packet_type != sll->sll_pkttype)
goto next;
(*frame_count)++;
if (dump_to_pcap(ctx)) {
tpacket3_hdr_to_pcap_pkthdr(hdr, sll, &phdr, ctx->magic);
ret = __pcap_io->write_pcap(*fd, &phdr, ctx->magic, packet,
pcap_get_length(&phdr, ctx->magic));
if (unlikely(ret != pcap_get_total_length(&phdr, ctx->magic)))
panic("Write error to pcap!\n");
}
__show_frame_hdr(sll, hdr, ctx->print_mode, true);
dissector_entry_point(packet, hdr->tp_snaplen, ctx->link_type,
ctx->print_mode);
next:
hdr = (void *) ((uint8_t *) hdr + hdr->tp_next_offset);
sll = (void *) ((uint8_t *) hdr + TPACKET_ALIGN(sizeof(*hdr)));
if (frame_count_max != 0) {
if (unlikely(*frame_count >= frame_count_max)) {
sigint = 1;
break;
}
}
if (dump_to_pcap(ctx)) {
if (ctx->dump_mode == DUMP_INTERVAL_SIZE) {
interval += hdr->tp_snaplen;
if (interval > ctx->dump_interval) {
next_dump = true;
interval = 0;
}
}
if (next_dump) {
*fd = next_multi_pcap_file(ctx, *fd);
next_dump = false;
if (unlikely(ctx->verbose))
print_pcap_file_stats(sock, ctx);
}
}
}
}
/*Copyright (c) 2006, Regents of the University of California All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Los Alamos National Laboratory nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef lint
static const char rcsid[] _U_ = "@#( $Header: /n/CVS/sirt/libpcap/pcap-ring.c,v 0.9 2005/06/10 23:06:31 cpw Exp $ (CPW)";
#endif
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <unistd.h>
#include <string.h>
# include <stdlib.h>
# include <errno.h>
# include <sys/uio.h>
# include <net/if.h>
# include <sys/ioctl.h>
# include <sys/poll.h>
# include <sys/mman.h>
#ifdef HAVE_64BIT_TIME_T
#define LOCK_PREFIX "lock ; "
#endif
# include <linux/if_ether.h>
# include <netinet/in.h>
#include "pcap-int.h"
#include "sll.h"
#include <sys/utsname.h>
#ifdef DO_RING
#undef RING_DEBUG
//#define RING_DEBUG
#define LOST_MY_HEAD 2 /* find iovhead if pollAgain == LOST_MY_HEAD */
#ifdef PACKET_RX_RING
#define MAX_SLAB_SIZE 131072 // valid for 2.4 and 2.6 kernels; /proc/slabinfo
#define MAX_RING_MEMORY ((unsigned long)(2147483648)) // max ring size
#define MAX_ORDER 8 /* #include kernel source <linux/mmzone.h> */
int
packet_tring_setup(pcap_t *p)
{
int i, k, idx, frames_per_block;
struct tpacket_req req;
unsigned long mem;
req.tp_frame_size = TPACKET_ALIGN(TPACKET_HDRLEN) + TPACKET_ALIGN(p->snapshot+p->md.offset);
if (p->rg.mem > 0) { /* use user-defined max */
mem = p->rg.mem * 1024;
} else if (p->rg.mem == 0) {
return -1; /* PCAP_MEMORY=0 negates mmaped mode */
} else {
struct utsname utsbuf;
if (uname(&utsbuf)) {
mem = 65536;
}
else {
if (utsbuf.release[0] == '2' && utsbuf.release[2] == '4') {
mem=275219456;
} else if (utsbuf.release[0] == '2' && utsbuf.release[2] == '6') {
mem=MAX_RING_MEMORY; /* 2147483648 */
} else {
mem= 1048576;
}
}
}
/* Use as many blocks as possible */
req.tp_block_nr = MAX_SLAB_SIZE / sizeof(void*);
/* In case we compile 32-bit, but run on a 64-bit kernel, scale by 2 */
req.tp_block_nr /= 2;
/* Get lower-bound of block size */
req.tp_block_size = mem / req.tp_block_nr;
/* Now round up to nearest order of page size */
int order;
for (order = 0; order <= MAX_ORDER; order++) {
if ( (getpagesize() << order) >= req.tp_block_size) {
break;
}
}
req.tp_block_size = getpagesize() << order;
/* And then recompute number of blocks precisely */
req.tp_block_nr = mem / req.tp_block_size;
/* Round-up rather than down */
if (req.tp_block_nr * req.tp_block_size < mem) req.tp_block_nr++;
frames_per_block = req.tp_block_size / req.tp_frame_size;
req.tp_frame_nr = frames_per_block * req.tp_block_nr;
p->rg.ct = req.tp_frame_nr;
//fprintf (stderr, "DEBUG(exit):block_size = %d, block_nr = %d, frame_size = %d, frame_nr = %d, mem = %g\n", req.tp_block_size, req.tp_block_nr, req.tp_frame_size, req.tp_frame_nr, (double)req.tp_block_size * req.tp_block_nr);
if (setsockopt(p->fd, SOL_PACKET, PACKET_RX_RING, (void*)&req,
sizeof(req))) {
perror("Error: setsockopt(PACKET_RX_RING)");
return -2;
}
struct iovec *ring;
unsigned long int devstats[IFACE_MAXFIELDS];
ring = (struct iovec *) malloc(p->rg.ct * sizeof(struct iovec));
if (!ring)
{
perror ("Error: Ring setup failure, malloc");
return -2;
}
p->rg.buf = (void*)mmap(0, req.tp_block_nr * req.tp_block_size, PROT_READ|PROT_WRITE,
MAP_SHARED, p->fd, 0);
if ((long)p->rg.buf == -1L) {
perror("Error: Could not allocate shared memory");
memset(&req, 0, sizeof(req));
p->rg.buf=NULL;
if (setsockopt(p->fd, SOL_PACKET, PACKET_RX_RING, (void*)&req,
sizeof(req)))
{
perror("Error: Failed to destroy ring");
}
free (ring);
return -2;
}
idx=0;
for (i=0; i<req.tp_block_nr; i++) {
for (k=0; k<frames_per_block; k++) {
ring[idx].iov_base = p->rg.buf + req.tp_block_size*i + k*req.tp_frame_size;
ring[idx].iov_len = req.tp_frame_size;
*(unsigned long*)ring[idx].iov_base = 0;
idx++;
}
}
p->rg.bufsize = req.tp_block_nr * req.tp_block_size;
p->rg.iovhead = 0;
p->rg.iovmax = p->rg.ct - 1;
p->rg.iovec = ring;
p->rg.fname = NULL;
p->rg.pollAgain = 0;
START_SECS(p) = 0;
if (packet_dev_stats(p, &devstats[0]))
{
p->rg.ifpkts = devstats[tx_packets] + devstats[rx_packets];
p->rg.ifbytes = devstats[tx_bytes] + devstats[rx_bytes];
p->rg.multi = devstats[rx_multicast];
p->rg.rxerrs = devstats[rx_errors];
p->rg.rxdrops = devstats[rx_dropped];
p->rg.txerrs = devstats[tx_errors];
p->rg.txdrops = devstats[tx_dropped];
p->rg.have_ds = 1;
}
else
{
p->rg.have_ds = 0;
}
return 0;
}
static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt)
{
struct sock *sk;
struct packet_sock *po;
struct sockaddr_ll *sll;
struct tpacket_hdr *h;
u8 * skb_head = skb->data;
int skb_len = skb->len;
unsigned snaplen;
unsigned long status = TP_STATUS_LOSING|TP_STATUS_USER;
unsigned short macoff, netoff;
struct sk_buff *copy_skb = NULL;
if (skb->pkt_type == PACKET_LOOPBACK)
goto drop;
sk = pt->af_packet_priv;
po = pkt_sk(sk);
if (dev->hard_header) {
if (sk->sk_type != SOCK_DGRAM)
skb_push(skb, skb->data - skb->mac.raw);
else if (skb->pkt_type == PACKET_OUTGOING) {
/* Special case: outgoing packets have ll header at head */
skb_pull(skb, skb->nh.raw - skb->data);
if (skb->ip_summed == CHECKSUM_HW)
status |= TP_STATUS_CSUMNOTREADY;
}
}
snaplen = skb->len;
if (sk->sk_filter) {
unsigned res = run_filter(skb, sk, snaplen);
if (res == 0)
goto drop_n_restore;
if (snaplen > res)
snaplen = res;
}
if (sk->sk_type == SOCK_DGRAM) {
macoff = netoff = TPACKET_ALIGN(TPACKET_HDRLEN) + 16;
} else {
unsigned maclen = skb->nh.raw - skb->data;
netoff = TPACKET_ALIGN(TPACKET_HDRLEN + (maclen < 16 ? 16 : maclen));
macoff = netoff - maclen;
}
if (macoff + snaplen > po->frame_size) {
if (po->copy_thresh &&
atomic_read(&sk->sk_rmem_alloc) + skb->truesize <
(unsigned)sk->sk_rcvbuf) {
if (skb_shared(skb)) {
copy_skb = skb_clone(skb, GFP_ATOMIC);
} else {
copy_skb = skb_get(skb);
skb_head = skb->data;
}
if (copy_skb)
skb_set_owner_r(copy_skb, sk);
}
snaplen = po->frame_size - macoff;
if ((int)snaplen < 0)
snaplen = 0;
}
if (snaplen > skb->len-skb->data_len)
snaplen = skb->len-skb->data_len;
spin_lock(&sk->sk_receive_queue.lock);
h = (struct tpacket_hdr *)packet_lookup_frame(po, po->head);
if (h->tp_status)
goto ring_is_full;
po->head = po->head != po->frame_max ? po->head+1 : 0;
po->stats.tp_packets++;
if (copy_skb) {
status |= TP_STATUS_COPY;
__skb_queue_tail(&sk->sk_receive_queue, copy_skb);
}
if (!po->stats.tp_drops)
status &= ~TP_STATUS_LOSING;
spin_unlock(&sk->sk_receive_queue.lock);
memcpy((u8*)h + macoff, skb->data, snaplen);
h->tp_len = skb->len;
h->tp_snaplen = snaplen;
h->tp_mac = macoff;
h->tp_net = netoff;
if (skb->stamp.tv_sec == 0) {
do_gettimeofday(&skb->stamp);
sock_enable_timestamp(sk);
}
h->tp_sec = skb->stamp.tv_sec;
h->tp_usec = skb->stamp.tv_usec;
sll = (struct sockaddr_ll*)((u8*)h + TPACKET_ALIGN(sizeof(*h)));
sll->sll_halen = 0;
if (dev->hard_header_parse)
sll->sll_halen = dev->hard_header_parse(skb, sll->sll_addr);
sll->sll_family = AF_PACKET;
sll->sll_hatype = dev->type;
sll->sll_protocol = skb->protocol;
sll->sll_pkttype = skb->pkt_type;
sll->sll_ifindex = dev->ifindex;
h->tp_status = status;
mb();
{
struct page *p_start, *p_end;
u8 *h_end = (u8 *)h + macoff + snaplen - 1;
p_start = virt_to_page(h);
p_end = virt_to_page(h_end);
while (p_start <= p_end) {
flush_dcache_page(p_start);
p_start++;
}
}
sk->sk_data_ready(sk, 0);
drop_n_restore:
if (skb_head != skb->data && skb_shared(skb)) {
skb->data = skb_head;
skb->len = skb_len;
}
drop:
kfree_skb(skb);
return 0;
ring_is_full:
po->stats.tp_drops++;
spin_unlock(&sk->sk_receive_queue.lock);
sk->sk_data_ready(sk, 0);
if (copy_skb)
kfree_skb(copy_skb);
goto drop_n_restore;
}
// Open a rawsock for TX or RX
void* ringRawsockOpen(ring_rawsock_t *rawsock, const char *ifname, bool rx_mode, bool tx_mode, U16 ethertype, U32 frame_size, U32 num_frames)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK);
if (!simpleRawsockOpen((simple_rawsock_t*)rawsock, ifname, rx_mode,
tx_mode, ethertype, frame_size, num_frames))
{
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
rawsock->pMem = (void*)(-1);
// Use version 2 headers for the MMAP packet stuff - avoids 32/64
// bit problems, gives nanosecond timestamps, and allows rx of vlan id
int val = TPACKET_V2;
if (setsockopt(rawsock->sock, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)) < 0) {
AVB_LOGF_ERROR("Creating rawsock; get PACKET_VERSION: %s", strerror(errno));
ringRawsockClose(rawsock);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
// Get the size of the headers in the ring
unsigned len = sizeof(val);
if (getsockopt(rawsock->sock, SOL_PACKET, PACKET_HDRLEN, &val, &len) < 0) {
AVB_LOGF_ERROR("Creating rawsock; get PACKET_HDRLEN: %s", strerror(errno));
ringRawsockClose(rawsock);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
rawsock->bufHdrSize = TPACKET_ALIGN(val);
if (rawsock->base.rxMode) {
rawsock->bufHdrSize = rawsock->bufHdrSize + TPACKET_ALIGN(sizeof(struct sockaddr_ll));
}
rawsock->bufferSize = rawsock->base.frameSize + rawsock->bufHdrSize;
rawsock->frameCount = num_frames;
AVB_LOGF_DEBUG("frameSize=%d, bufHdrSize=%d(%d+%zu) bufferSize=%d, frameCount=%d",
rawsock->base.frameSize, rawsock->bufHdrSize, val, sizeof(struct sockaddr_ll),
rawsock->bufferSize, rawsock->frameCount);
// Get number of bytes in a memory page. The blocks we ask for
// must be a multiple of pagesize. (Actually, it should be
// (pagesize * 2^N) to avoid wasting memory.)
int pagesize = getpagesize();
rawsock->blockSize = pagesize * 4;
AVB_LOGF_DEBUG("pagesize=%d blockSize=%d", pagesize, rawsock->blockSize);
// Calculate number of buffers and frames based on blocks
int buffersPerBlock = rawsock->blockSize / rawsock->bufferSize;
rawsock->blockCount = rawsock->frameCount / buffersPerBlock + 1;
rawsock->frameCount = buffersPerBlock * rawsock->blockCount;
AVB_LOGF_DEBUG("frameCount=%d, buffersPerBlock=%d, blockCount=%d",
rawsock->frameCount, buffersPerBlock, rawsock->blockCount);
// Fill in the kernel structure with our calculated values
struct tpacket_req s_packet_req;
memset(&s_packet_req, 0, sizeof(s_packet_req));
s_packet_req.tp_block_size = rawsock->blockSize;
s_packet_req.tp_frame_size = rawsock->bufferSize;
s_packet_req.tp_block_nr = rawsock->blockCount;
s_packet_req.tp_frame_nr = rawsock->frameCount;
// Ask the kernel to create the TX_RING or RX_RING
if (rawsock->base.txMode) {
if (setsockopt(rawsock->sock, SOL_PACKET, PACKET_TX_RING,
(char*)&s_packet_req, sizeof(s_packet_req)) < 0) {
AVB_LOGF_ERROR("Creating rawsock; TX_RING: %s", strerror(errno));
ringRawsockClose(rawsock);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
AVB_LOGF_DEBUG("PACKET_%s_RING OK", "TX");
}
else {
if (setsockopt(rawsock->sock, SOL_PACKET, PACKET_RX_RING,
(char*)&s_packet_req, sizeof(s_packet_req)) < 0) {
AVB_LOGF_ERROR("Creating rawsock, RX_RING: %s", strerror(errno));
ringRawsockClose(rawsock);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
AVB_LOGF_DEBUG("PACKET_%s_RING OK", "TX");
}
// Call MMAP to get access to the memory used for the ring
rawsock->memSize = rawsock->blockCount * rawsock->blockSize;
AVB_LOGF_DEBUG("memSize=%zu (%d, %d), sock=%d",
rawsock->memSize,
rawsock->blockCount,
rawsock->blockSize,
rawsock->sock);
rawsock->pMem = mmap((void*)0, rawsock->memSize, PROT_READ|PROT_WRITE, MAP_SHARED, rawsock->sock, (off_t)0);
if (rawsock->pMem == (void*)(-1)) {
AVB_LOGF_ERROR("Creating rawsock; MMAP: %s", strerror(errno));
ringRawsockClose(rawsock);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return NULL;
}
AVB_LOGF_DEBUG("mmap: %p", rawsock->pMem);
// Initialize the memory
memset(rawsock->pMem, 0, rawsock->memSize);
// Initialize the state of the ring
rawsock->blockIndex = 0;
rawsock->bufferIndex = 0;
rawsock->buffersOut = 0;
rawsock->buffersReady = 0;
// fill virtual functions table
rawsock_cb_t *cb = &rawsock->base.cb;
cb->close = ringRawsockClose;
cb->getTxFrame = ringRawsockGetTxFrame;
cb->relTxFrame = ringRawsockRelTxFrame;
cb->txFrameReady = ringRawsockTxFrameReady;
cb->send = ringRawsockSend;
cb->txBufLevel = ringRawsockTxBufLevel;
cb->rxBufLevel = ringRawsockRxBufLevel;
cb->getRxFrame = ringRawsockGetRxFrame;
cb->rxParseHdr = ringRawsockRxParseHdr;
cb->relRxFrame = ringRawsockRelRxFrame;
cb->getTXOutOfBuffers = ringRawsockGetTXOutOfBuffers;
cb->getTXOutOfBuffersCyclic = ringRawsockGetTXOutOfBuffersCyclic;
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
return rawsock;
}
void q_ring_setup(q_ring_t n, q_ring_group_t g, uint32_t direct) {
struct sockaddr_ll addr;
struct packet_mreq mr;
q_ring_data_t ring;
int32_t val, hdr_size, i;
// Frame size must be atleast the size of the MTU, if it was smaller
// no packets could be retreived/sent
static_assert(Q_RING_FRAME_SIZE > Q_RING_MTU);
// Open raw socket and set TPACKET option
if ((g->fd = socket(AF_PACKET, SOCK_RAW, 0)) < 0)
error("socket: %s", strerror(errno));
val = TPACKET_V2;
if (setsockopt(g->fd, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)) < 0)
error("setsockopt: %s", strerror(errno));
// Initialize ring with frame size as the MTU, the number of blocks per frame
// should be large enough to prevent dropped packets in circular buffer
g->req.tp_frame_size = Q_RING_FRAME_SIZE;
g->req.tp_block_size = Q_RING_FRAME_SIZE * Q_RING_FRAME_PER_BLOCK;
g->req.tp_block_nr = Q_RING_BLOCK_COUNT;
g->req.tp_frame_nr = Q_RING_BLOCK_COUNT * Q_RING_FRAME_PER_BLOCK;
if ((setsockopt(g->fd, SOL_PACKET, direct, (char*)&g->req, sizeof(g->req))) < 0)
error("setsockopt(%d): %s", direct, strerror(errno));
// Create mmap()'d memory for kernel to read/write directly
g->map_len = g->req.tp_block_size * g->req.tp_block_nr;
g->map = mmap(NULL, g->map_len, PROT_READ | PROT_WRITE, MAP_SHARED, g->fd, 0);
if (g->map == MAP_FAILED)
error("mmap: %s", strerror(errno));
// Initialize a pointer to each frame in the ring, the frame must
// be TPACKET_ALIGN'd
ring = calloc(g->req.tp_frame_nr * sizeof(*ring), 1);
hdr_size = TPACKET_ALIGN(sizeof(*ring[0].hdr));
for (i = 0; i < g->req.tp_frame_nr; i++) {
ring[i].blk = g->map + (i * g->req.tp_frame_size);
ring[i].hdr = (void*)(ring[i].blk);
ring[i].sll = (void*)(ring[i].blk + hdr_size);
ring[i].buf = (void*)(ring[i].blk + hdr_size);
ring[i].len = g->req.tp_frame_size - hdr_size;
}
// Sets the start and end of the circular buffer
g->r_start = &ring[0];
g->r_end = &ring[g->req.tp_frame_nr];
g->r = g->r_start;
// Tell kernel to listen for all ethernet packets
memset(&addr, 0, sizeof(addr));
addr.sll_family = AF_PACKET;
addr.sll_protocol = htons(ETH_P_ALL);
addr.sll_ifindex = n->ifindex;
if (bind(g->fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
error("bind: %s", strerror(errno));
// Turn on promiscuous mode to read packets with any destination address,
// including packets not destined to host
memset(&mr, 0, sizeof (mr));
mr.mr_ifindex = n->ifindex;
mr.mr_type = PACKET_MR_PROMISC;
if (setsockopt(g->fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof (mr)) < 0)
error("setsockopt: %s", strerror(errno));
}
int main ( int argc, char **argv )
{
struct pollfd pfd;
struct sockaddr_ll addr;
struct ifreq s_ifr;
int i;
signal(SIGINT, sigproc);
/* Open the packet socket */
if ( (fd=socket(PF_PACKET, SOCK_RAW, 0))<0 ) {
perror("socket()");
return 1;
}
/* Setup the fd for mmap() ring buffer */
/* size-max: 8192 /proc/slabinfo kmalloc-<size>
* ptr-size: 8 sizeof(void *)
* page-size: 4096 getpagesize()
* max-order: 11 mmzone.h kernel header
* frame-size: 2048
* --
* block-num: size-max / ptr-size
* block-size: page-size << max-order
* --
* max-buffer: block-num * block-size
* frame-num: max-buffer / frame-size
*
*/
req.tp_block_size=getpagesize() << (MAX_ORDER-1); // power of 2
req.tp_frame_size=2048; // multiple of TPACKET_ALIGNMENT (16) > TPACKET_HDRLEN (if_packet.h)
req.tp_block_nr=128; // <= block-num at max, power of 2
req.tp_frame_nr=req.tp_block_size / req.tp_frame_size * req.tp_block_nr;
if ( (setsockopt(fd,
SOL_PACKET,
PACKET_RX_RING,
(char *)&req,
sizeof(req))) != 0 ) {
perror("setsockopt()");
close(fd);
return 1;
};
/* mmap() the sucker */
map=mmap(NULL,
req.tp_block_size * req.tp_block_nr,
PROT_READ|PROT_WRITE|PROT_EXEC, MAP_SHARED, fd, 0);
if ( map==MAP_FAILED ) {
perror("mmap()");
close(fd);
return 1;
}
/* Setup our ringbuffer */
ring=malloc(req.tp_frame_nr * sizeof(struct iovec));
for(i=0; i<req.tp_frame_nr; i++) {
ring[i].iov_base=(void *)((long)map)+(i*req.tp_frame_size);
ring[i].iov_len=req.tp_frame_size;
}
if(argc > 1)
strncpy(s_ifr.ifr_name, argv[1], sizeof(s_ifr.ifr_name));
else
strncpy(s_ifr.ifr_name, "eth0", sizeof(s_ifr.ifr_name));
ioctl(fd, SIOCGIFINDEX, &s_ifr);
/* bind the packet socket */
memset(&addr, 0, sizeof(addr));
addr.sll_family=AF_PACKET;
addr.sll_protocol=htons(ETH_P_IP);
addr.sll_ifindex=s_ifr.ifr_ifindex;
addr.sll_hatype=0;
addr.sll_pkttype=0;
addr.sll_halen=0;
if ( bind(fd, (struct sockaddr *)&addr, sizeof(addr)) ) {
munmap(map, req.tp_block_size * req.tp_block_nr);
perror("bind()");
close(fd);
return 1;
}
for(i=0;;) {
while(*(unsigned long*)ring[i].iov_base) {
struct tpacket_hdr *h=ring[i].iov_base;
struct sockaddr_ll *sll=(void *)h + TPACKET_ALIGN(sizeof(*h));
unsigned char *bp=(unsigned char *)h + h->tp_mac;
/*
printf("%u.%.6u: if%u %s %u bytes\n",
h->tp_sec, h->tp_usec,
sll->sll_ifindex,
names[sll->sll_pkttype],
h->tp_len);
*/
/* tell the kernel this packet is done with */
h->tp_status=0;
//mb(); /* memory barrier */
i=(i==req.tp_frame_nr-1) ? 0 : i+1;
}
/* Sleep when nothings happening */
pfd.fd=fd;
pfd.events=POLLIN|POLLERR;
pfd.revents=0;
poll(&pfd, 1, -1);
}
return 0;
}
