1 /* $NetBSD: npf_mbuf_subr.c,v 1.5 2014/02/13 03:34:40 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_return_hdrs6(struct mbuf *m, struct ip6_hdr **ip6) 141 { 142 struct ip6_hdr *ip6hdr = mtod(m, struct ip6_hdr *); 143 144 *ip6 = ip6hdr; 145 return (void *)(ip6hdr + 1); 146 } 147 148 void 149 mbuf_icmp_append(struct mbuf *m, struct mbuf *m_orig) 150 { 151 struct ip *iphdr = mtod(m, struct ip *); 152 const size_t hlen = iphdr->ip_hl << 2; 153 struct icmp *ic = (struct icmp *)((uint8_t *)iphdr + hlen); 154 const size_t addlen = m_length(m_orig); 155 156 iphdr->ip_len = htons(ntohs(iphdr->ip_len) + addlen); 157 memcpy(&ic->icmp_ip, mtod(m_orig, struct ip *), addlen); 158 m->m_pkthdr.len += addlen; 159 m->m_len += addlen; 160 m_freem(m_orig); 161 } 162