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(int flags, int 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(int flags, int 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(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 *m0 = m1; 105 (void)len; 106 return true; 107 } 108 109 110 struct mbuf * 111 mbuf_getwithdata(const void *data, size_t len) 112 { 113 struct mbuf *m; 114 void *dst; 115 116 m = m_gethdr(M_WAITOK, MT_HEADER); 117 assert(m != NULL); 118 dst = mtod(m, void *); 119 memcpy(dst, data, len); 120 m->m_pkthdr.len = len; 121 m->m_len = len; 122 return m; 123 } 124 125 struct mbuf * 126 mbuf_construct_ether(int proto) 127 { 128 struct mbuf *m0, *m1; 129 struct ether_header *ethdr; 130 131 m0 = m_gethdr(M_WAITOK, MT_HEADER); 132 ethdr = mtod(m0, struct ether_header *); 133 ethdr->ether_type = htons(ETHERTYPE_IP); 134 m0->m_pkthdr.len = sizeof(struct ether_header); 135 m0->m_len = sizeof(struct ether_header); 136 137 m1 = mbuf_construct(proto); 138 m0->m_next = m1; 139 m1->m_next = NULL; 140 return m0; 141 } 142 143 static int 144 mbuf_fill_proto(int proto, void *l4data) 145 { 146 struct tcphdr *th; 147 int size = 0; 148 149 switch (proto) { 150 case IPPROTO_TCP: 151 th = l4data; 152 th->th_off = sizeof(struct tcphdr) >> 2; 153 size = sizeof(struct tcphdr); 154 break; 155 case IPPROTO_UDP: 156 size = sizeof(struct udphdr); 157 break; 158 case IPPROTO_ICMP: 159 size = offsetof(struct icmp, icmp_data); 160 break; 161 } 162 return size; 163 } 164 165 struct mbuf * 166 mbuf_construct(int proto) 167 { 168 struct mbuf *m; 169 struct ip *iphdr; 170 void *l4data; 171 int size; 172 173 m = m_gethdr(M_WAITOK, MT_HEADER); 174 iphdr = mtod(m, struct ip *); 175 176 iphdr->ip_v = IPVERSION; 177 iphdr->ip_hl = sizeof(struct ip) >> 2; 178 iphdr->ip_off = 0; 179 iphdr->ip_ttl = 64; 180 iphdr->ip_p = proto; 181 182 size = sizeof(struct ip); 183 l4data = (void *)(iphdr + 1); 184 size += mbuf_fill_proto(proto, l4data); 185 iphdr->ip_len = htons(size); 186 187 m->m_pkthdr.len = size; 188 m->m_len = size; 189 m->m_next = NULL; 190 return m; 191 } 192 193 struct mbuf * 194 mbuf_construct6(int proto) 195 { 196 struct mbuf *m; 197 struct ip6_hdr *ip6; 198 void *l4data; 199 int size; 200 201 m = m_gethdr(M_WAITOK, MT_HEADER); 202 ip6 = mtod(m, struct ip6_hdr *); 203 204 ip6->ip6_vfc = IPV6_VERSION; 205 ip6->ip6_nxt = proto; 206 ip6->ip6_hlim = 64; 207 208 size = sizeof(struct ip6_hdr); 209 l4data = (void *)(ip6 + 1); 210 size += mbuf_fill_proto(proto, l4data); 211 ip6->ip6_plen = htons(size); 212 213 m->m_pkthdr.len = size; 214 m->m_len = size; 215 m->m_next = NULL; 216 return m; 217 } 218 219 void * 220 mbuf_return_hdrs(struct mbuf *m, bool ether, struct ip **ip) 221 { 222 struct ip *iphdr; 223 224 if (ether) { 225 struct mbuf *mn = m->m_next; 226 iphdr = mtod(mn, struct ip *); 227 } else { 228 iphdr = mtod(m, struct ip *); 229 } 230 *ip = iphdr; 231 return (void *)(iphdr + 1); 232 } 233 234 void * 235 mbuf_return_hdrs6(struct mbuf *m, struct ip6_hdr **ip6) 236 { 237 struct ip6_hdr *ip6hdr = mtod(m, struct ip6_hdr *); 238 239 *ip6 = ip6hdr; 240 return (void *)(ip6hdr + 1); 241 } 242 243 void 244 mbuf_icmp_append(struct mbuf *m, struct mbuf *m_orig) 245 { 246 struct ip *iphdr = mtod(m, struct ip *); 247 const size_t hlen = iphdr->ip_hl << 2; 248 void *p = (uint8_t *)iphdr + hlen; 249 struct icmp *ic = (struct icmp *)p; 250 const size_t addlen = m_length(m_orig); 251 252 iphdr->ip_len = htons(ntohs(iphdr->ip_len) + addlen); 253 memcpy(&ic->icmp_ip, mtod(m_orig, struct ip *), addlen); 254 m->m_pkthdr.len += addlen; 255 m->m_len += addlen; 256 m_freem(m_orig); 257 } 258 259 struct mbuf * 260 mbuf_get_pkt(int af, int proto, const char *src, const char *dst, 261 int sport, int dport) 262 { 263 struct mbuf *m; 264 struct ip *ip; 265 struct ip6_hdr *ip6; 266 struct tcphdr *th; 267 struct udphdr *uh; 268 void *p, *ipsrc, *ipdst; 269 270 switch (af) { 271 case AF_INET6: 272 m = mbuf_construct6(proto); 273 p = mbuf_return_hdrs6(m, &ip6); 274 ipsrc = &ip6->ip6_src; 275 ipdst = &ip6->ip6_dst; 276 break; 277 case AF_INET: 278 default: 279 m = mbuf_construct(proto); 280 p = mbuf_return_hdrs(m, false, &ip); 281 ipsrc = &ip->ip_src.s_addr; 282 ipdst = &ip->ip_dst.s_addr; 283 } 284 285 npf_inet_pton(af, src, ipsrc); 286 npf_inet_pton(af, dst, ipdst); 287 288 switch (proto) { 289 case IPPROTO_TCP: 290 th = p; 291 th->th_sport = htons(sport); 292 th->th_dport = htons(dport); 293 break; 294 case IPPROTO_UDP: 295 uh = p; 296 uh->uh_sport = htons(sport); 297 uh->uh_dport = htons(dport); 298 break; 299 default: 300 KASSERT(false); 301 } 302 return m; 303 } 304 305 npf_cache_t * 306 get_cached_pkt(struct mbuf *m, const char *ifname) 307 { 308 ifnet_t *ifp = npf_test_getif(ifname ? ifname : IFNAME_DUMMY); 309 npf_cache_t *npc = kmem_zalloc(sizeof(npf_cache_t), KM_SLEEP); 310 nbuf_t *nbuf = kmem_zalloc(sizeof(nbuf_t), KM_SLEEP); 311 int ret; 312 313 npc->npc_info = 0; 314 npc->npc_ctx = npf_getkernctx(); 315 316 nbuf_init(npc->npc_ctx, nbuf, m, ifp); 317 npc->npc_nbuf = nbuf; 318 ret = npf_cache_all(npc); 319 assert(ret); (void)ret; 320 321 return npc; 322 } 323 324 void 325 put_cached_pkt(npf_cache_t *npc) 326 { 327 struct mbuf *m = nbuf_head_mbuf(npc->npc_nbuf); 328 kmem_free(npc->npc_nbuf, sizeof(nbuf_t)); 329 kmem_free(npc, sizeof(npf_cache_t)); 330 m_freem(m); 331 } 332 333 const npf_mbufops_t npftest_mbufops = { 334 .alloc = npfkern_m_get, 335 .free = npfkern_m_freem, 336 .getdata = npfkern_m_getdata, 337 .getnext = npfkern_m_next, 338 .getlen = npfkern_m_buflen, 339 .getchainlen = npfkern_m_length, 340 .ensure_contig = npfkern_m_ensure_contig, 341 .ensure_writable = NULL, 342 }; 343