Home | History | Annotate | Line # | Download | only in libnpftest
npf_mbuf_subr.c revision 1.3 
      1 /*	$NetBSD: npf_mbuf_subr.c,v 1.3 2012/07/01 23:21:07 rmind Exp $	*/
      2 
      3 /*
      4  * NPF testing - helper routines.
      5  *
      6  * Public Domain.
      7  */
      8 
      9 #include <sys/types.h>
     10 #include <sys/kmem.h>
     11 
     12 #include "npf_impl.h"
     13 #include "npf_test.h"
     14 
     15 struct mbuf *
     16 mbuf_getwithdata(const void *data, size_t len)
     17 {
     18 	struct mbuf *m;
     19 	void *dst;
     20 
     21 	m = m_gethdr(M_WAITOK, MT_HEADER);
     22 	assert(m != NULL);
     23 	dst = mtod(m, void *);
     24 	memcpy(dst, data, len);
     25 	m->m_len = len;
     26 	return m;
     27 }
     28 
     29 struct mbuf *
     30 mbuf_construct_ether(int proto)
     31 {
     32 	struct mbuf *m0, *m1;
     33 	struct ether_header *ethdr;
     34 
     35 	m0 = m_gethdr(M_WAITOK, MT_HEADER);
     36 	ethdr = mtod(m0, struct ether_header *);
     37 	ethdr->ether_type = htons(ETHERTYPE_IP);
     38 	m0->m_len = sizeof(struct ether_header);
     39 
     40 	m1 = mbuf_construct(proto);
     41 	m0->m_next = m1;
     42 	m1->m_next = NULL;
     43 	return m0;
     44 }
     45 
     46 static int
     47 mbuf_fill_proto(int proto, void *l4data)
     48 {
     49 	struct tcphdr *th;
     50 	int size = 0;
     51 
     52 	switch (proto) {
     53 	case IPPROTO_TCP:
     54 		th = l4data;
     55 		th->th_off = sizeof(struct tcphdr) >> 2;
     56 		size = sizeof(struct tcphdr);
     57 		break;
     58 	case IPPROTO_UDP:
     59 		size = sizeof(struct udphdr);
     60 		break;
     61 	case IPPROTO_ICMP:
     62 		size = offsetof(struct icmp, icmp_data);
     63 		break;
     64 	}
     65 	return size;
     66 }
     67 
     68 struct mbuf *
     69 mbuf_construct(int proto)
     70 {
     71 	struct mbuf *m;
     72 	struct ip *iphdr;
     73 	void *l4data;
     74 	int size;
     75 
     76 	m = m_gethdr(M_WAITOK, MT_HEADER);
     77 	iphdr = mtod(m, struct ip *);
     78 
     79 	iphdr->ip_v = IPVERSION;
     80 	iphdr->ip_hl = sizeof(struct ip) >> 2;
     81 	iphdr->ip_off = 0;
     82 	iphdr->ip_ttl = 64;
     83 	iphdr->ip_p = proto;
     84 
     85 	size = sizeof(struct ip);
     86 	l4data = (void *)(iphdr + 1);
     87 	size += mbuf_fill_proto(proto, l4data);
     88 	iphdr->ip_len = htons(size);
     89 
     90 	m->m_len = size;
     91 	m->m_next = NULL;
     92 	return m;
     93 }
     94 
     95 struct mbuf *
     96 mbuf_construct6(int proto)
     97 {
     98 	struct mbuf *m;
     99 	struct ip6_hdr *ip6;
    100 	void *l4data;
    101 	int size;
    102 
    103 	m = m_gethdr(M_WAITOK, MT_HEADER);
    104 	ip6 = mtod(m, struct ip6_hdr *);
    105 
    106 	ip6->ip6_vfc = IPV6_VERSION;
    107 	ip6->ip6_nxt = proto;
    108 	ip6->ip6_hlim = 64;
    109 
    110 	size = sizeof(struct ip6_hdr);
    111 	l4data = (void *)(ip6 + 1);
    112 	size += mbuf_fill_proto(proto, l4data);
    113 	ip6->ip6_plen = htons(size);
    114 
    115 	m->m_len = size;
    116 	m->m_next = NULL;
    117 	return m;
    118 }
    119 
    120 void *
    121 mbuf_return_hdrs(struct mbuf *m, bool ether, struct ip **ip)
    122 {
    123 	struct ip *iphdr;
    124 
    125 	if (ether) {
    126 		struct mbuf *mn = m->m_next;
    127 		iphdr = mtod(mn, struct ip *);
    128 	} else {
    129 		iphdr = mtod(m, struct ip *);
    130 	}
    131 	*ip = iphdr;
    132 	return (void *)(iphdr + 1);
    133 }
    134 
    135 void
    136 mbuf_icmp_append(struct mbuf *m, struct mbuf *m_orig)
    137 {
    138 	struct ip *iphdr = mtod(m, struct ip *);
    139 	const size_t hlen = iphdr->ip_hl << 2;
    140 	struct icmp *ic = (struct icmp *)((uint8_t *)iphdr + hlen);
    141 	const size_t addlen = m_orig->m_len;
    142 
    143 	iphdr->ip_len = htons(ntohs(iphdr->ip_len) + addlen);
    144 	memcpy(&ic->icmp_ip, mtod(m_orig, struct ip *), addlen);
    145 	m->m_len += addlen;
    146 	m_freem(m_orig);
    147 }
    148