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 *
npfkern_m_get(npf_t * npf __unused,unsigned flags,size_t space)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 *
npfkern_m_get(npf_t * npf __unused,unsigned flags,size_t space)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 *
npfkern_m_getdata(const struct mbuf * m)40 npfkern_m_getdata(const struct mbuf *m)
41 {
42 return m->m_data;
43 }
44
45 static struct mbuf *
npfkern_m_next(struct mbuf * m)46 npfkern_m_next(struct mbuf *m)
47 {
48 return m->m_next;
49 }
50
51 static size_t
npfkern_m_buflen(const struct mbuf * m)52 npfkern_m_buflen(const struct mbuf *m)
53 {
54 return m->m_len;
55 }
56
57 size_t
npfkern_m_length(const struct mbuf * m)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
npfkern_m_freem(struct mbuf * m)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
npfkern_m_ensure_contig(struct mbuf ** m0,size_t len)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 *
mbuf_getwithdata(const void * data,size_t len)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 *
mbuf_construct_ether(int proto)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
mbuf_fill_proto(int proto,void * l4data)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 *
mbuf_construct(int proto)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 *
mbuf_construct6(int proto)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 *
mbuf_return_hdrs(struct mbuf * m,bool ether,struct ip ** ip)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 *
mbuf_return_hdrs6(struct mbuf * m,struct ip6_hdr ** ip6)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
mbuf_icmp_append(struct mbuf * m,struct mbuf * m_orig)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 *
mbuf_get_pkt(int af,int proto,const char * src,const char * dst,int sport,int dport)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 *
get_cached_pkt(struct mbuf * m,const char * ifname)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
put_cached_pkt(npf_cache_t * npc)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