1 /* 2 * NPF testing - helper routines. 3 * 4 * Public Domain. 5 */ 6 7 #ifdef _KERNEL 8 #include <sys/types.h> 9 #include <sys/kmem.h> 10 #endif 11 12 #include "npf_impl.h" 13 #include "npf_test.h" 14 15 16 #if defined(_NPF_STANDALONE) 17 struct mbuf * 18 npfkern_m_get(npf_t *npf __unused, unsigned flags, size_t space) 19 { 20 unsigned mlen = offsetof(struct mbuf, m_data0[space]); 21 struct mbuf *m; 22 23 m = calloc(1, mlen); 24 if (m) { 25 m->m_type = 1; 26 m->m_flags = flags; 27 m->m_data = m->m_data0; 28 } 29 return m; 30 } 31 #else 32 struct mbuf * 33 npfkern_m_get(npf_t *npf __unused, unsigned flags, size_t space) 34 { 35 return m_get(flags, space); 36 } 37 #endif 38 39 static void * 40 npfkern_m_getdata(const struct mbuf *m) 41 { 42 return m->m_data; 43 } 44 45 static struct mbuf * 46 npfkern_m_next(struct mbuf *m) 47 { 48 return m->m_next; 49 } 50 51 static size_t 52 npfkern_m_buflen(const struct mbuf *m) 53 { 54 return m->m_len; 55 } 56 57 size_t 58 npfkern_m_length(const struct mbuf *m) 59 { 60 const struct mbuf *m0; 61 unsigned pktlen = 0; 62 63 if ((m->m_flags & M_PKTHDR) != 0) 64 return m->m_pkthdr.len; 65 for (m0 = m; m0 != NULL; m0 = m0->m_next) 66 pktlen += m0->m_len; 67 return pktlen; 68 } 69 70 void 71 npfkern_m_freem(struct mbuf *m) 72 { 73 #ifdef _NPF_STANDALONE 74 struct mbuf *n; 75 76 do { 77 n = m->m_next; 78 m->m_type = MT_FREE; 79 free(m); 80 m = n; 81 } while (m); 82 #else 83 m_freem(m); 84 #endif 85 } 86 87 static bool 88 npfkern_m_ensure_contig(struct mbuf **m0, size_t len) 89 { 90 struct mbuf *m1; 91 unsigned tlen; 92 char *dptr; 93 94 tlen = npfkern_m_length(*m0); 95 if ((m1 = npfkern_m_get(NULL, M_PKTHDR, tlen)) == NULL) { 96 return false; 97 } 98 m1->m_pkthdr.len = m1->m_len = tlen; 99 dptr = m1->m_data; 100 for (struct mbuf *m = *m0; m != NULL; m = m->m_next) { 101 memcpy(dptr, m->m_data, m->m_len); 102 dptr += m->m_len; 103 } 104 npfkern_m_freem(*m0); 105 *m0 = m1; 106 (void)len; 107 return true; 108 } 109 110 111 struct mbuf * 112 mbuf_getwithdata(const void *data, size_t len) 113 { 114 struct mbuf *m; 115 void *dst; 116 117 m = m_gethdr(M_WAITOK, MT_HEADER); 118 assert(m != NULL); 119 dst = mtod(m, void *); 120 memcpy(dst, data, len); 121 m->m_pkthdr.len = len; 122 m->m_len = len; 123 return m; 124 } 125 126 struct mbuf * 127 mbuf_construct_ether(int proto) 128 { 129 struct mbuf *m0, *m1; 130 struct ether_header *ethdr; 131 132 m0 = m_gethdr(M_WAITOK, MT_HEADER); 133 ethdr = mtod(m0, struct ether_header *); 134 ethdr->ether_type = htons(ETHERTYPE_IP); 135 m0->m_pkthdr.len = sizeof(struct ether_header); 136 m0->m_len = sizeof(struct ether_header); 137 138 m1 = mbuf_construct(proto); 139 m0->m_next = m1; 140 m1->m_next = NULL; 141 return m0; 142 } 143 144 static int 145 mbuf_fill_proto(int proto, void *l4data) 146 { 147 struct tcphdr *th; 148 int size = 0; 149 150 switch (proto) { 151 case IPPROTO_TCP: 152 th = l4data; 153 th->th_off = sizeof(struct tcphdr) >> 2; 154 size = sizeof(struct tcphdr); 155 break; 156 case IPPROTO_UDP: 157 size = sizeof(struct udphdr); 158 break; 159 case IPPROTO_ICMP: 160 size = offsetof(struct icmp, icmp_data); 161 break; 162 } 163 return size; 164 } 165 166 struct mbuf * 167 mbuf_construct(int proto) 168 { 169 struct mbuf *m; 170 struct ip *iphdr; 171 void *l4data; 172 int size; 173 174 m = m_gethdr(M_WAITOK, MT_HEADER); 175 iphdr = mtod(m, struct ip *); 176 177 iphdr->ip_v = IPVERSION; 178 iphdr->ip_hl = sizeof(struct ip) >> 2; 179 iphdr->ip_off = 0; 180 iphdr->ip_ttl = 64; 181 iphdr->ip_p = proto; 182 183 size = sizeof(struct ip); 184 l4data = (void *)(iphdr + 1); 185 size += mbuf_fill_proto(proto, l4data); 186 iphdr->ip_len = htons(size); 187 188 m->m_pkthdr.len = size; 189 m->m_len = size; 190 m->m_next = NULL; 191 return m; 192 } 193 194 struct mbuf * 195 mbuf_construct6(int proto) 196 { 197 struct mbuf *m; 198 struct ip6_hdr *ip6; 199 void *l4data; 200 int size; 201 202 m = m_gethdr(M_WAITOK, MT_HEADER); 203 ip6 = mtod(m, struct ip6_hdr *); 204 205 ip6->ip6_vfc = IPV6_VERSION; 206 ip6->ip6_nxt = proto; 207 ip6->ip6_hlim = 64; 208 209 size = sizeof(struct ip6_hdr); 210 l4data = (void *)(ip6 + 1); 211 size += mbuf_fill_proto(proto, l4data); 212 ip6->ip6_plen = htons(size); 213 214 m->m_pkthdr.len = size; 215 m->m_len = size; 216 m->m_next = NULL; 217 return m; 218 } 219 220 void * 221 mbuf_return_hdrs(struct mbuf *m, bool ether, struct ip **ip) 222 { 223 struct ip *iphdr; 224 225 if (ether) { 226 struct mbuf *mn = m->m_next; 227 iphdr = mtod(mn, struct ip *); 228 } else { 229 iphdr = mtod(m, struct ip *); 230 } 231 *ip = iphdr; 232 return (void *)(iphdr + 1); 233 } 234 235 void * 236 mbuf_return_hdrs6(struct mbuf *m, struct ip6_hdr **ip6) 237 { 238 struct ip6_hdr *ip6hdr = mtod(m, struct ip6_hdr *); 239 240 *ip6 = ip6hdr; 241 return (void *)(ip6hdr + 1); 242 } 243 244 void 245 mbuf_icmp_append(struct mbuf *m, struct mbuf *m_orig) 246 { 247 struct ip *iphdr = mtod(m, struct ip *); 248 const size_t hlen = iphdr->ip_hl << 2; 249 void *p = (uint8_t *)iphdr + hlen; 250 struct icmp *ic = (struct icmp *)p; 251 const size_t addlen = m_length(m_orig); 252 253 iphdr->ip_len = htons(ntohs(iphdr->ip_len) + addlen); 254 memcpy(&ic->icmp_ip, mtod(m_orig, struct ip *), addlen); 255 m->m_pkthdr.len += addlen; 256 m->m_len += addlen; 257 m_freem(m_orig); 258 } 259 260 struct mbuf * 261 mbuf_get_pkt(int af, int proto, const char *src, const char *dst, 262 int sport, int dport) 263 { 264 struct mbuf *m; 265 struct ip *ip; 266 struct ip6_hdr *ip6; 267 struct tcphdr *th; 268 struct udphdr *uh; 269 void *p, *ipsrc, *ipdst; 270 271 switch (af) { 272 case AF_INET6: 273 m = mbuf_construct6(proto); 274 p = mbuf_return_hdrs6(m, &ip6); 275 ipsrc = &ip6->ip6_src; 276 ipdst = &ip6->ip6_dst; 277 break; 278 case AF_INET: 279 default: 280 m = mbuf_construct(proto); 281 p = mbuf_return_hdrs(m, false, &ip); 282 ipsrc = &ip->ip_src.s_addr; 283 ipdst = &ip->ip_dst.s_addr; 284 } 285 286 npf_inet_pton(af, src, ipsrc); 287 npf_inet_pton(af, dst, ipdst); 288 289 switch (proto) { 290 case IPPROTO_TCP: 291 th = p; 292 th->th_sport = htons(sport); 293 th->th_dport = htons(dport); 294 break; 295 case IPPROTO_UDP: 296 uh = p; 297 uh->uh_sport = htons(sport); 298 uh->uh_dport = htons(dport); 299 break; 300 default: 301 KASSERT(false); 302 } 303 return m; 304 } 305 306 npf_cache_t * 307 get_cached_pkt(struct mbuf *m, const char *ifname) 308 { 309 ifnet_t *ifp = npf_test_getif(ifname ? ifname : IFNAME_DUMMY); 310 npf_cache_t *npc = kmem_zalloc(sizeof(npf_cache_t), KM_SLEEP); 311 nbuf_t *nbuf = kmem_zalloc(sizeof(nbuf_t), KM_SLEEP); 312 int ret; 313 314 npc->npc_info = 0; 315 npc->npc_ctx = npf_getkernctx(); 316 317 nbuf_init(npc->npc_ctx, nbuf, m, ifp); 318 npc->npc_nbuf = nbuf; 319 ret = npf_cache_all(npc); 320 assert(ret); (void)ret; 321 322 return npc; 323 } 324 325 void 326 put_cached_pkt(npf_cache_t *npc) 327 { 328 struct mbuf *m = nbuf_head_mbuf(npc->npc_nbuf); 329 kmem_free(npc->npc_nbuf, sizeof(nbuf_t)); 330 kmem_free(npc, sizeof(npf_cache_t)); 331 m_freem(m); 332 } 333 334 const npf_mbufops_t npftest_mbufops = { 335 .alloc = npfkern_m_get, 336 .free = npfkern_m_freem, 337 .getdata = npfkern_m_getdata, 338 .getnext = npfkern_m_next, 339 .getlen = npfkern_m_buflen, 340 .getchainlen = npfkern_m_length, 341 .ensure_contig = npfkern_m_ensure_contig, 342 .ensure_writable = NULL, 343 }; 344