1 /*
2 pmacct (Promiscuous mode IP Accounting package)
3 pmacct is Copyright (C) 2003-2019 by Paolo Lucente
4 */
5
6 /*
7 This program is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 This program is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with this program; if not, write to the Free Software
19 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
20 */
21
22 /* includes */
23 #include "pmacct.h"
24 #include "pmacct-data.h"
25
26 /* eth_handler() picks a whole packet, reads
27 informtions contained in the link layer
28 protocol header and fills a pointer structure */
eth_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)29 void eth_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
30 {
31 u_int16_t e8021Q, ppp, cfp, cvnt;
32 struct eth_header *eth_pk;
33 u_int16_t etype, caplen = h->caplen, nl;
34 u_int8_t cursor = 0;
35
36 if (caplen < ETHER_HDRLEN) {
37 pptrs->iph_ptr = NULL;
38 return;
39 }
40
41 eth_pk = (struct eth_header *) pptrs->packet_ptr;
42 etype = ntohs(eth_pk->ether_type);
43 pptrs->mac_ptr = (u_char *) eth_pk->ether_dhost;
44 pptrs->vlan_ptr = NULL; /* avoid stale vlan pointers */
45 pptrs->mpls_ptr = NULL; /* avoid stale MPLS pointers */
46 nl = ETHER_HDRLEN;
47 caplen -= ETHER_HDRLEN;
48
49 recurse:
50 if (etype == ETHERTYPE_IP) {
51 pptrs->l3_proto = ETHERTYPE_IP;
52 pptrs->l3_handler = ip_handler;
53 pptrs->iph_ptr = pptrs->packet_ptr + nl;
54 return;
55 }
56 if (etype == ETHERTYPE_IPV6) {
57 pptrs->l3_proto = ETHERTYPE_IPV6;
58 pptrs->l3_handler = ip6_handler;
59 pptrs->iph_ptr = pptrs->packet_ptr + nl;
60 return;
61 }
62
63 /* originally contributed by Rich Gade */
64 if (etype == ETHERTYPE_8021Q) {
65 if (caplen < IEEE8021Q_TAGLEN) {
66 pptrs->iph_ptr = NULL;
67 return;
68 }
69 memcpy(&e8021Q, pptrs->packet_ptr+nl+2, 2);
70 if (!cursor) pptrs->vlan_ptr = pptrs->packet_ptr + nl;
71 etype = ntohs(e8021Q);
72 nl += IEEE8021Q_TAGLEN;
73 caplen -= IEEE8021Q_TAGLEN;
74 cursor++;
75 goto recurse;
76 }
77
78 /* Process Cisco Fabric Path Header */
79 if (etype == ETHERTYPE_CFP) {
80 if (caplen < CFP_TAGLEN) {
81 pptrs->iph_ptr = NULL;
82 return;
83 }
84
85 memcpy(&cfp, pptrs->packet_ptr+nl+CFP_TAGLEN-2, 2);
86 etype = ntohs(cfp);
87 nl += CFP_TAGLEN;
88 caplen -= CFP_TAGLEN;
89 cursor++;
90 goto recurse;
91 }
92
93 /* Process Cisco Virtual Network TAG Header */
94 if (etype == ETHERTYPE_CVNT) {
95 if (caplen < CVNT_TAGLEN) {
96 pptrs->iph_ptr = NULL;
97 return;
98 }
99
100 memcpy(&cvnt, pptrs->packet_ptr+nl+CVNT_TAGLEN-2, 2);
101 etype = ntohs(cvnt);
102 nl += CVNT_TAGLEN;
103 caplen -= CVNT_TAGLEN;
104 cursor++;
105 goto recurse;
106 }
107
108 /* originally contributed by Vasiliy Ponomarev */
109 if (etype == ETHERTYPE_PPPOE) {
110 if (caplen < PPPOE_HDRLEN+PPP_TAGLEN) {
111 pptrs->iph_ptr = NULL;
112 return;
113 }
114 memcpy(&ppp, pptrs->packet_ptr+nl+PPPOE_HDRLEN, 2);
115 etype = ntohs(ppp);
116
117 if (etype == PPP_IP) etype = ETHERTYPE_IP;
118 if (etype == PPP_IPV6) etype = ETHERTYPE_IPV6;
119
120 nl += PPPOE_HDRLEN+PPP_TAGLEN;
121 caplen -= PPPOE_HDRLEN+PPP_TAGLEN;
122 cursor = 1;
123 goto recurse;
124 }
125
126 if (etype == ETHERTYPE_MPLS || etype == ETHERTYPE_MPLS_MULTI) {
127 etype = mpls_handler(pptrs->packet_ptr + nl, &caplen, &nl, pptrs);
128 cursor = 1;
129 goto recurse;
130 }
131
132 pptrs->l3_proto = 0;
133 pptrs->l3_handler = NULL;
134 pptrs->iph_ptr = NULL;
135 }
136
mpls_handler(u_char * bp,u_int16_t * caplen,u_int16_t * nl,register struct packet_ptrs * pptrs)137 u_int16_t mpls_handler(u_char *bp, u_int16_t *caplen, u_int16_t *nl, register struct packet_ptrs *pptrs)
138 {
139 u_int32_t *p = (u_int32_t *) bp;
140 u_char *next = bp;
141 u_int32_t label=0;
142
143 pptrs->mpls_ptr = bp;
144
145 if (*caplen < 4) {
146 pptrs->iph_ptr = NULL;
147 return 0;
148 }
149
150 do {
151 label = ntohl(*p);
152 p += 4; *nl += 4; next += 4; *caplen -= 4;
153 } while (!MPLS_STACK(label) && *caplen >= 4);
154
155 switch (MPLS_LABEL(label)) {
156 case 0: /* IPv4 explicit NULL label */
157 case 3: /* IPv4 implicit NULL label */
158 return ETHERTYPE_IP;
159 case 2: /* IPv6 explicit NULL label */
160 return ETHERTYPE_IPV6;
161 default:
162 /*
163 support for what is sometimes referred as null-encapsulation:
164 by looking at the first payload byte (but only if the Bottom
165 of Stack bit is set) we try to determine the network layer
166 protocol:
167 0x45-0x4f is IPv4
168 0x60-0x6f is IPv6
169 */
170 if (MPLS_STACK(label)) {
171 switch (*next) {
172 case 0x45:
173 case 0x46:
174 case 0x47:
175 case 0x48:
176 case 0x49:
177 case 0x4a:
178 case 0x4b:
179 case 0x4c:
180 case 0x4d:
181 case 0x4e:
182 case 0x4f:
183 return ETHERTYPE_IP;
184 case 0x60:
185 case 0x61:
186 case 0x62:
187 case 0x63:
188 case 0x64:
189 case 0x65:
190 case 0x66:
191 case 0x67:
192 case 0x68:
193 case 0x69:
194 case 0x6a:
195 case 0x6b:
196 case 0x6c:
197 case 0x6d:
198 case 0x6e:
199 case 0x6f:
200 return ETHERTYPE_IPV6;
201 default:
202 break;
203 }
204 }
205 break;
206 }
207
208 return FALSE;
209 }
210
ppp_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)211 void ppp_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
212 {
213 u_char *p = pptrs->packet_ptr;
214 u_int16_t caplen = h->caplen, nl = 0;
215 unsigned int proto = 0;
216
217 if (caplen < PPP_HDRLEN) {
218 pptrs->iph_ptr = NULL;
219 return;
220 }
221
222 if (*p == PPP_ADDRESS && *(p + 1) == PPP_CONTROL) {
223 p += 2;
224 caplen -= 2;
225 if (caplen < 2) {
226 pptrs->iph_ptr = NULL;
227 return;
228 }
229 }
230
231 if (*p % 2) {
232 proto = *p;
233 p++;
234 }
235 else {
236 proto = EXTRACT_16BITS(p);
237 p += 2;
238 }
239
240 recurse:
241 if ((proto == PPP_IP) || (proto == ETHERTYPE_IP)) {
242 pptrs->l3_proto = ETHERTYPE_IP;
243 pptrs->l3_handler = ip_handler;
244 pptrs->iph_ptr = p;
245 return;
246 }
247
248 if ((proto == PPP_IPV6) || (proto == ETHERTYPE_IPV6)) {
249 pptrs->l3_proto = ETHERTYPE_IPV6;
250 pptrs->l3_handler = ip6_handler;
251 pptrs->iph_ptr = p;
252 return;
253 }
254
255 if (proto == PPP_MPLS_UCAST || proto == PPP_MPLS_MCAST) {
256 proto = mpls_handler(p, &caplen, &nl, pptrs);
257 goto recurse;
258 }
259
260 pptrs->l3_proto = 0;
261 pptrs->l3_handler = NULL;
262 pptrs->iph_ptr = NULL;
263 }
264
265 /*
266 support for 802.11 Wireless LAN protocol. I'm writing
267 it during a sad morning spent at Fiumicino's Airport
268 because of Alitalia strikes. It's currently working
269 well for me at FCO WiFi zone. Let me know.
270
271 28-11-2003, Paolo.
272 */
ieee_802_11_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)273 void ieee_802_11_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
274 {
275 u_int16_t fc;
276 u_int caplen = h->caplen;
277 short int hdrlen;
278 u_char *p;
279
280 if (caplen < IEEE802_11_FC_LEN) {
281 pptrs->iph_ptr = NULL;
282 return;
283 }
284
285 p = pptrs->packet_ptr;
286
287 fc = EXTRACT_LE_16BITS(p);
288 if (FC_TYPE(fc) == T_DATA) {
289 if (FC_TO_DS(fc) && FC_FROM_DS(fc)) hdrlen = 30;
290 else hdrlen = 24;
291 if (caplen < hdrlen) {
292 pptrs->iph_ptr = NULL;
293 return;
294 }
295 caplen -= hdrlen;
296 p += hdrlen;
297 if (!FC_WEP(fc)) {
298 if ((p = llc_handler(h, caplen, p, pptrs)) != NULL) {
299 pptrs->iph_ptr = p;
300 return;
301 }
302 }
303 }
304
305 pptrs->l3_proto = 0;
306 pptrs->l3_handler = NULL;
307 pptrs->iph_ptr = NULL;
308 }
309
raw_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)310 void raw_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
311 {
312 register u_int16_t caplen = h->caplen;
313 struct pm_iphdr *hdr;
314
315 if (caplen < 4) {
316 pptrs->iph_ptr = NULL;
317 return;
318 }
319
320 hdr = (struct pm_iphdr *) pptrs->packet_ptr;
321 switch (IP_V(hdr)) {
322 case 4:
323 pptrs->iph_ptr = pptrs->packet_ptr;
324 pptrs->l3_proto = ETHERTYPE_IP;
325 pptrs->l3_handler = ip_handler;
326 return;
327 case 6:
328 pptrs->iph_ptr = pptrs->packet_ptr;
329 pptrs->l3_proto = ETHERTYPE_IPV6;
330 pptrs->l3_handler = ip6_handler;
331 return;
332 default:
333 pptrs->iph_ptr = NULL;
334 pptrs->l3_proto = 0;
335 pptrs->l3_handler = NULL;
336 return;
337 }
338 }
339
null_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)340 void null_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
341 {
342 register u_int32_t *family;
343 u_int caplen = h->caplen;
344
345 if (caplen < 4) {
346 pptrs->iph_ptr = NULL;
347 return;
348 }
349
350 family = (u_int32_t *) pptrs->packet_ptr;
351
352 if (*family == AF_INET || ntohl(*family) == AF_INET ) {
353 pptrs->l3_proto = ETHERTYPE_IP;
354 pptrs->l3_handler = ip_handler;
355 pptrs->iph_ptr = (u_char *)(pptrs->packet_ptr + 4);
356 return;
357 }
358
359 if (*family == AF_INET6 || ntohl(*family) == AF_INET6 ) {
360 pptrs->l3_proto = ETHERTYPE_IPV6;
361 pptrs->l3_handler = ip6_handler;
362 pptrs->iph_ptr = (u_char *)(pptrs->packet_ptr + 4);
363 return;
364 }
365
366 pptrs->l3_proto = 0;
367 pptrs->l3_handler = NULL;
368 pptrs->iph_ptr = NULL;
369 }
370
sll_handler(const struct pcap_pkthdr * h,register struct packet_ptrs * pptrs)371 void sll_handler(const struct pcap_pkthdr *h, register struct packet_ptrs *pptrs)
372 {
373 register const struct sll_header *sllp;
374 register u_short etype;
375 u_char *p;
376 u_int16_t caplen = h->caplen;
377 u_int16_t e8021Q, nl;
378 int cursor;
379
380 if (caplen < SLL_HDR_LEN) {
381 pptrs->iph_ptr = NULL;
382 return;
383 }
384
385 pptrs->mac_ptr = NULL;
386 pptrs->vlan_ptr = NULL;
387 pptrs->mpls_ptr = NULL;
388
389 p = pptrs->packet_ptr;
390
391 sllp = (const struct sll_header *) pptrs->packet_ptr;
392 etype = ntohs(sllp->sll_protocol);
393 nl = SLL_HDR_LEN;
394
395 if (EXTRACT_16BITS(&sllp->sll_halen) == ETH_ADDR_LEN) {
396 memcpy(sll_mac[1], sllp->sll_addr, ETH_ADDR_LEN);
397 pptrs->mac_ptr = (u_char *) sll_mac;
398 }
399
400 recurse:
401 if (etype == ETHERTYPE_IP) {
402 pptrs->l3_proto = ETHERTYPE_IP;
403 pptrs->l3_handler = ip_handler;
404 pptrs->iph_ptr = (u_char *)(pptrs->packet_ptr + nl);
405 return;
406 }
407
408 if (etype == ETHERTYPE_IPV6) {
409 pptrs->l3_proto = ETHERTYPE_IPV6;
410 pptrs->l3_handler = ip6_handler;
411 pptrs->iph_ptr = pptrs->packet_ptr + nl;
412 return;
413 }
414
415 if (etype == LINUX_SLL_P_802_2) {
416 /* going up to LLC/SNAP layer header */
417 p += SLL_HDR_LEN;
418 caplen -= SLL_HDR_LEN;
419 if ((p = llc_handler(h, caplen, p, pptrs)) != NULL) {
420 pptrs->iph_ptr = p;
421 return;
422 }
423 }
424
425 /* originally contributed by Rich Gade for eth_handler() */
426 if (etype == ETHERTYPE_8021Q) {
427 if (caplen < IEEE8021Q_TAGLEN) {
428 pptrs->iph_ptr = NULL;
429 return;
430 }
431 memcpy(&e8021Q, pptrs->packet_ptr+nl+2, 2);
432 if (!cursor) pptrs->vlan_ptr = pptrs->packet_ptr + nl;
433 etype = ntohs(e8021Q);
434 nl += IEEE8021Q_TAGLEN;
435 caplen -= IEEE8021Q_TAGLEN;
436 cursor++;
437 goto recurse;
438 }
439
440 if (etype == ETHERTYPE_MPLS || etype == ETHERTYPE_MPLS_MULTI) {
441 etype = mpls_handler(pptrs->packet_ptr + nl, &caplen, &nl, pptrs);
442 cursor = 1;
443 goto recurse;
444 }
445
446 pptrs->l3_proto = 0;
447 pptrs->l3_handler = NULL;
448 pptrs->iph_ptr = NULL;
449 }
450
llc_handler(const struct pcap_pkthdr * h,u_int caplen,register u_char * buf,register struct packet_ptrs * pptrs)451 u_char *llc_handler(const struct pcap_pkthdr *h, u_int caplen, register u_char *buf, register struct packet_ptrs *pptrs)
452 {
453 struct llc llc;
454 register u_short etype;
455
456 if (caplen < 3) return NULL;
457
458 memcpy((char *)&llc, (char *) buf, MIN(caplen, sizeof(llc)));
459 if (llc.ssap == LLCSAP_SNAP && llc.dsap == LLCSAP_SNAP
460 && llc.ctl.snap.snap_ui == LLC_UI) {
461 etype = EXTRACT_16BITS(&llc.ctl.snap_ether.snap_ethertype[0]);
462
463 if (etype == ETHERTYPE_IP) {
464 pptrs->l3_proto = ETHERTYPE_IP;
465 pptrs->l3_handler = ip_handler;
466 return (u_char *)(buf + MIN(caplen, sizeof(llc)));
467 }
468
469 if (etype == ETHERTYPE_IPV6) {
470 pptrs->l3_proto = ETHERTYPE_IPV6;
471 pptrs->l3_handler = ip6_handler;
472 return (u_char *)(buf + MIN(caplen, sizeof(llc)));
473 }
474
475 return NULL;
476 }
477 else return NULL;
478 }
479