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