1 /* $NetBSD: npf_mbuf_subr.c,v 1.6 2016/12/26 23:05:05 christos Exp $ */ 2 3 /* 4 * NPF testing - helper routines. 5 * 6 * Public Domain. 7 */ 8 9 #ifdef _KERNEL 10 #include <sys/types.h> 11 #include <sys/kmem.h> 12 #endif 13 14 #include "npf_impl.h" 15 #include "npf_test.h" 16 17 18 #if defined(_NPF_STANDALONE) 19 struct mbuf * 20 npfkern_m_get(int flags, int space) 21 { 22 unsigned mlen = offsetof(struct mbuf, m_data0[space]); 23 struct mbuf *m; 24 25 m = calloc(1, sizeof(struct mbuf)); 26 if (m) { 27 m->m_type = 1; 28 m->m_flags = flags; 29 m->m_data = m->m_data0; 30 } 31 return m; 32 } 33 #else 34 struct mbuf * 35 npfkern_m_get(int flags, int space) 36 { 37 return m_get(flags, space); 38 } 39 #endif 40 41 static void * 42 npfkern_m_getdata(const struct mbuf *m) 43 { 44 return m->m_data; 45 } 46 47 static struct mbuf * 48 npfkern_m_next(struct mbuf *m) 49 { 50 return m->m_next; 51 } 52 53 static size_t 54 npfkern_m_buflen(const struct mbuf *m) 55 { 56 return m->m_len; 57 } 58 59 size_t 60 npfkern_m_length(const struct mbuf *m) 61 { 62 const struct mbuf *m0; 63 unsigned pktlen = 0; 64 65 if ((m->m_flags & M_PKTHDR) != 0) 66 return m->m_pkthdr.len; 67 for (m0 = m; m0 != NULL; m0 = m0->m_next) 68 pktlen += m0->m_len; 69 return pktlen; 70 } 71 72 void 73 npfkern_m_freem(struct mbuf *m) 74 { 75 #ifdef _NPF_STANDALONE 76 struct mbuf *n; 77 78 do { 79 n = m->m_next; 80 m->m_type = MT_FREE; 81 free(m); 82 m = n; 83 } while (m); 84 #else 85 m_freem(m); 86 #endif 87 } 88 89 static bool 90 npfkern_m_ensure_contig(struct mbuf **m0, size_t len) 91 { 92 struct mbuf *m1; 93 unsigned tlen; 94 char *dptr; 95 96 tlen = npfkern_m_length(*m0); 97 if ((m1 = npfkern_m_get(M_PKTHDR, tlen)) == NULL) { 98 return false; 99 } 100 m1->m_pkthdr.len = m1->m_len = tlen; 101 dptr = m1->m_data; 102 for (struct mbuf *m = *m0; m != NULL; m = m->m_next) { 103 memcpy(dptr, m->m_data, m->m_len); 104 dptr += m->m_len; 105 } 106 *m0 = m1; 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 struct icmp *ic = (struct icmp *)((uint8_t *)iphdr + hlen); 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 const npf_mbufops_t npftest_mbufops = { 260 .alloc = npfkern_m_get, 261 .free = npfkern_m_freem, 262 .getdata = npfkern_m_getdata, 263 .getnext = npfkern_m_next, 264 .getlen = npfkern_m_buflen, 265 .getchainlen = npfkern_m_length, 266 .ensure_contig = npfkern_m_ensure_contig, 267 .ensure_writable = NULL, 268 }; 269