1 /* $NetBSD: npf_mbuf_subr.c,v 1.1 2012/04/14 21:57:29 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(void *data, size_t len) 17 { 18 struct mbuf *m; 19 20 m = kmem_zalloc(sizeof(struct mbuf), KM_SLEEP); 21 assert(m != NULL); 22 m->m_data = data; 23 m->m_len = len; 24 return m; 25 } 26 27 struct mbuf * 28 mbuf_construct_ether(int proto) 29 { 30 struct mbuf *m0, *m1; 31 struct ether_header *ethdr; 32 33 m0 = m_gethdr(M_WAITOK, MT_HEADER); 34 ethdr = mtod(m0, struct ether_header *); 35 ethdr->ether_type = htons(ETHERTYPE_IP); 36 m0->m_len = sizeof(struct ether_header); 37 38 m1 = mbuf_construct(proto); 39 m0->m_next = m1; 40 m1->m_next = NULL; 41 return m0; 42 } 43 44 struct mbuf * 45 mbuf_construct(int proto) 46 { 47 struct mbuf *m; 48 struct ip *iphdr; 49 struct tcphdr *th; 50 int size; 51 52 m = m_gethdr(M_WAITOK, MT_HEADER); 53 iphdr = mtod(m, struct ip *); 54 55 iphdr->ip_v = IPVERSION; 56 iphdr->ip_hl = sizeof(struct ip) >> 2; 57 iphdr->ip_off = 0; 58 iphdr->ip_ttl = 64; 59 iphdr->ip_p = proto; 60 61 size = sizeof(struct ip); 62 63 switch (proto) { 64 case IPPROTO_TCP: 65 th = (void *)(iphdr + 1); 66 th->th_off = sizeof(struct tcphdr) >> 2; 67 size += sizeof(struct tcphdr); 68 break; 69 case IPPROTO_UDP: 70 size += sizeof(struct udphdr); 71 break; 72 case IPPROTO_ICMP: 73 size += offsetof(struct icmp, icmp_data); 74 break; 75 } 76 iphdr->ip_len = htons(size); 77 78 m->m_len = size; 79 m->m_next = NULL; 80 return m; 81 } 82 83 void * 84 mbuf_return_hdrs(struct mbuf *m, bool ether, struct ip **ip) 85 { 86 struct ip *iphdr; 87 88 if (ether) { 89 struct mbuf *mn = m->m_next; 90 iphdr = mtod(mn, struct ip *); 91 } else { 92 iphdr = mtod(m, struct ip *); 93 } 94 *ip = iphdr; 95 return (void *)(iphdr + 1); 96 } 97 98 void 99 mbuf_icmp_append(struct mbuf *m, struct mbuf *m_orig) 100 { 101 struct ip *iphdr = mtod(m, struct ip *); 102 const size_t hlen = iphdr->ip_hl << 2; 103 struct icmp *ic = (struct icmp *)((uint8_t *)iphdr + hlen); 104 const size_t addlen = m_orig->m_len; 105 106 iphdr->ip_len = htons(ntohs(iphdr->ip_len) + addlen); 107 memcpy(&ic->icmp_ip, mtod(m_orig, struct ip *), addlen); 108 m->m_len += addlen; 109 m_freem(m_orig); 110 } 111