1 /*
2  * pkt.c
3  *
4  * Copyright (c) 2001 Dug Song <dugsong@monkey.org>
5  *
6  * $Id: pkt.c,v 1.10 2002/04/07 22:55:20 dugsong Exp $
7  */
8 
9 #include "config.h"
10 
11 #include <sys/types.h>
12 
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <string.h>
16 
17 #include "bget.h"
18 #include "pkt.h"
19 
20 void
pkt_init(int size)21 pkt_init(int size)
22 {
23 	bectl(NULL, malloc, free, sizeof(struct pkt) * size);
24 }
25 
26 struct pkt *
pkt_new(void)27 pkt_new(void)
28 {
29 	struct pkt *pkt;
30 
31 	if ((pkt = bget(sizeof(*pkt))) == NULL)
32 		return (NULL);
33 
34 	timerclear(&pkt->pkt_ts);
35 	memset(&pkt->pkt_ev, 0, sizeof(pkt->pkt_ev));
36 
37 	pkt->pkt_data = pkt->pkt_buf + PKT_BUF_ALIGN;
38 	pkt->pkt_eth = (struct eth_hdr *)pkt->pkt_data;
39 	pkt->pkt_eth_data = pkt->pkt_data + ETH_HDR_LEN;
40 	pkt->pkt_ip_data = pkt->pkt_data + ETH_HDR_LEN + IP_HDR_LEN;
41 	pkt->pkt_tcp_data = NULL;
42 	pkt->pkt_end = pkt->pkt_ip_data;
43 
44 	return (pkt);
45 }
46 
47 struct pkt *
pkt_dup(struct pkt * pkt)48 pkt_dup(struct pkt *pkt)
49 {
50 	struct pkt *new;
51 	off_t off;
52 
53 	if ((new = bget(sizeof(*new))) == NULL)
54 		return (NULL);
55 
56 	off = new->pkt_buf - pkt->pkt_buf;
57 
58 	new->pkt_ts = pkt->pkt_ts;
59 	memset(&new->pkt_ev, 0, sizeof(new->pkt_ev));
60 
61 	new->pkt_data = pkt->pkt_data + off;
62 
63 	new->pkt_eth = (pkt->pkt_eth != NULL) ?
64 	    (struct eth_hdr *)new->pkt_data : NULL;
65 
66 	new->pkt_eth_data = (pkt->pkt_eth_data != NULL) ?
67 	    pkt->pkt_eth_data + off : NULL;
68 
69 	new->pkt_ip_data = (pkt->pkt_ip_data != NULL) ?
70 	    pkt->pkt_ip_data + off : NULL;
71 
72 	new->pkt_tcp_data = (pkt->pkt_tcp_data != NULL) ?
73 	    pkt->pkt_tcp_data + off : NULL;
74 
75 	memcpy(new->pkt_data, pkt->pkt_data, pkt->pkt_end - pkt->pkt_data);
76 
77 	new->pkt_end = pkt->pkt_end + off;
78 
79 	return (new);
80 }
81 
82 void
pkt_decorate(struct pkt * pkt)83 pkt_decorate(struct pkt *pkt)
84 {
85 	u_char *p;
86 	int hl, len, off;
87 
88 	pkt->pkt_data = pkt->pkt_buf + PKT_BUF_ALIGN;
89 	pkt->pkt_eth = NULL;
90 	pkt->pkt_ip = NULL;
91 	pkt->pkt_ip_data = NULL;
92 	pkt->pkt_tcp_data = NULL;
93 
94 	p = pkt->pkt_data;
95 
96 	if (p + ETH_HDR_LEN > pkt->pkt_end)
97 		return;
98 
99 	pkt->pkt_eth = (struct eth_hdr *)p;
100 	p += ETH_HDR_LEN;
101 
102 	if (p + IP_HDR_LEN > pkt->pkt_end)
103 		return;
104 
105 	pkt->pkt_eth_data = p;
106 
107 	/* If IP header length is longer than packet length, stop. */
108 	hl = pkt->pkt_ip->ip_hl << 2;
109 	if (p + hl > pkt->pkt_end) {
110 		pkt->pkt_ip = NULL;
111 		return;
112 	}
113 	/* If IP length is longer than packet length, stop. */
114 	len = ntohs(pkt->pkt_ip->ip_len);
115 	if (p + len > pkt->pkt_end)
116 		return;
117 
118 	/* If IP fragment, stop. */
119 	off = ntohs(pkt->pkt_ip->ip_off);
120 	if ((off & IP_OFFMASK) != 0 || (off & IP_MF) != 0)
121 		return;
122 
123 	pkt->pkt_end = p + len;
124 	p += hl;
125 
126 	/* If transport layer header is longer than packet length, stop. */
127 	switch (pkt->pkt_ip->ip_p) {
128 	case IP_PROTO_ICMP:
129 		hl = ICMP_HDR_LEN;
130 		break;
131 	case IP_PROTO_TCP:
132 		if (p + TCP_HDR_LEN > pkt->pkt_end)
133 			return;
134 		hl = ((struct tcp_hdr *)p)->th_off << 2;
135 		break;
136 	case IP_PROTO_UDP:
137 		hl = UDP_HDR_LEN;
138 		break;
139 	default:
140 		return;
141 	}
142 	if (p + hl > pkt->pkt_end)
143 		return;
144 
145 	pkt->pkt_ip_data = p;
146 	p += hl;
147 
148 	/* Check for transport layer data. */
149 	switch (pkt->pkt_ip->ip_p) {
150 	case IP_PROTO_ICMP:
151 		pkt->pkt_icmp_msg = (union icmp_msg *)p;
152 
153 		switch (pkt->pkt_icmp->icmp_type) {
154 		case ICMP_ECHO:
155 		case ICMP_ECHOREPLY:
156 			hl = sizeof(pkt->pkt_icmp_msg->echo);
157 			break;
158 		case ICMP_UNREACH:
159 			if (pkt->pkt_icmp->icmp_code == ICMP_UNREACH_NEEDFRAG)
160 				hl = sizeof(pkt->pkt_icmp_msg->needfrag);
161 			else
162 				hl = sizeof(pkt->pkt_icmp_msg->unreach);
163 			break;
164 		case ICMP_SRCQUENCH:
165 		case ICMP_REDIRECT:
166 		case ICMP_TIMEXCEED:
167 		case ICMP_PARAMPROB:
168 			hl = sizeof(pkt->pkt_icmp_msg->srcquench);
169 			break;
170 		case ICMP_RTRADVERT:
171 			hl = sizeof(pkt->pkt_icmp_msg->rtradvert);
172 			break;
173 		case ICMP_RTRSOLICIT:
174 			hl = sizeof(pkt->pkt_icmp_msg->rtrsolicit);
175 			break;
176 		case ICMP_TSTAMP:
177 		case ICMP_TSTAMPREPLY:
178 			hl = sizeof(pkt->pkt_icmp_msg->tstamp);
179 			break;
180 		case ICMP_INFO:
181 		case ICMP_INFOREPLY:
182 		case ICMP_DNS:
183 			hl = sizeof(pkt->pkt_icmp_msg->info);
184 			break;
185 		case ICMP_MASK:
186 		case ICMP_MASKREPLY:
187 			hl = sizeof(pkt->pkt_icmp_msg->mask);
188 			break;
189 		case ICMP_DNSREPLY:
190 			hl = sizeof(pkt->pkt_icmp_msg->dnsreply);
191 			break;
192 		default:
193 			hl = pkt->pkt_end - p + 1;
194 			break;
195 		}
196 		if (p + hl > pkt->pkt_end)
197 			pkt->pkt_icmp_msg = NULL;
198 		break;
199 	case IP_PROTO_TCP:
200 		if (p < pkt->pkt_end)
201 			pkt->pkt_tcp_data = p;
202 		break;
203 	case IP_PROTO_UDP:
204 		if (pkt->pkt_ip_data + ntohs(pkt->pkt_udp->uh_ulen) <=
205 		    pkt->pkt_end)
206 			pkt->pkt_udp_data = p;
207 		break;
208 	}
209 }
210 
211 void
pkt_free(struct pkt * pkt)212 pkt_free(struct pkt *pkt)
213 {
214 	brel(pkt);
215 }
216 
217 void
pktq_reverse(struct pktq * pktq)218 pktq_reverse(struct pktq *pktq)
219 {
220 	struct pktq tmpq;
221 	struct pkt *pkt, *next;
222 
223 	TAILQ_INIT(&tmpq);
224 	for (pkt = TAILQ_FIRST(pktq); pkt != TAILQ_END(pktq); pkt = next) {
225 		next = TAILQ_NEXT(pkt, pkt_next);
226 		TAILQ_INSERT_HEAD(&tmpq, pkt, pkt_next);
227 	}
228 	*pktq = tmpq;
229 }
230 
231 void
pktq_shuffle(rand_t * r,struct pktq * pktq)232 pktq_shuffle(rand_t *r, struct pktq *pktq)
233 {
234 	static struct pkt **pvbase;
235 	static int pvlen;
236 	struct pkt *pkt;
237 	int i;
238 
239 	i = 0;
240 	TAILQ_FOREACH(pkt, pktq, pkt_next) {
241 		i++;
242 	}
243 	if (i > pvlen) {
244 		pvlen = i;
245 		if (pvbase == NULL)
246 			pvbase = malloc(sizeof(pkt) * pvlen);
247 		else
248 			pvbase = realloc(pvbase, sizeof(pkt) * pvlen);
249 	}
250 	i = 0;
251 	TAILQ_FOREACH(pkt, pktq, pkt_next) {
252 		pvbase[i++] = pkt;
253 	}
254 	TAILQ_INIT(pktq);
255 
256 	rand_shuffle(r, pvbase, i, sizeof(pkt));
257 
258 	while (--i >= 0) {
259 		TAILQ_INSERT_TAIL(pktq, pvbase[i], pkt_next);
260 	}
261 }
262 
263 struct pkt *
pktq_random(rand_t * r,struct pktq * pktq)264 pktq_random(rand_t *r, struct pktq *pktq)
265 {
266 	struct pkt *pkt;
267 	int i;
268 
269 	i = 0;
270 	TAILQ_FOREACH(pkt, pktq, pkt_next) {
271 		i++;
272 	}
273 	i = rand_uint32(r) % (i - 1);
274 	pkt = TAILQ_FIRST(pktq);
275 
276 	while (--i >= 0) {
277 		pkt = TAILQ_NEXT(pkt, pkt_next);
278 	}
279 	return (pkt);
280 }
281