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