Home | History | Annotate | Line # | Download | only in libnpftest
      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