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