1 /* $Id$ */
2 /*
3 * Copyright (C) 2008-2011 Teluu Inc. (http://www.teluu.com)
4 * Copyright (C) 2003-2008 Benny Prijono <benny@prijono.org>
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 */
20 #include <pjlib-util/pcap.h>
21 #include <pj/assert.h>
22 #include <pj/errno.h>
23 #include <pj/file_io.h>
24 #include <pj/log.h>
25 #include <pj/pool.h>
26 #include <pj/sock.h>
27 #include <pj/string.h>
28
29 #if 0
30 # define TRACE_(x) PJ_LOG(5,x)
31 #else
32 # define TRACE_(x)
33 #endif
34
35
36 #pragma pack(1)
37
38 typedef struct pj_pcap_hdr
39 {
40 pj_uint32_t magic_number; /* magic number */
41 pj_uint16_t version_major; /* major version number */
42 pj_uint16_t version_minor; /* minor version number */
43 pj_int32_t thiszone; /* GMT to local correction */
44 pj_uint32_t sigfigs; /* accuracy of timestamps */
45 pj_uint32_t snaplen; /* max length of captured packets, in octets */
46 pj_uint32_t network; /* data link type */
47 } pj_pcap_hdr;
48
49 typedef struct pj_pcap_rec_hdr
50 {
51 pj_uint32_t ts_sec; /* timestamp seconds */
52 pj_uint32_t ts_usec; /* timestamp microseconds */
53 pj_uint32_t incl_len; /* number of octets of packet saved in file */
54 pj_uint32_t orig_len; /* actual length of packet */
55 } pj_pcap_rec_hdr;
56
57 #if 0
58 /* gcc insisted on aligning this struct to 32bit on ARM */
59 typedef struct pj_pcap_eth_hdr
60 {
61 pj_uint8_t dest[6];
62 pj_uint8_t src[6];
63 pj_uint8_t len[2];
64 } pj_pcap_eth_hdr;
65 #else
66 typedef pj_uint8_t pj_pcap_eth_hdr[14];
67 #endif
68
69 typedef struct pj_pcap_ip_hdr
70 {
71 pj_uint8_t v_ihl;
72 pj_uint8_t tos;
73 pj_uint16_t len;
74 pj_uint16_t id;
75 pj_uint16_t flags_fragment;
76 pj_uint8_t ttl;
77 pj_uint8_t proto;
78 pj_uint16_t csum;
79 pj_uint32_t ip_src;
80 pj_uint32_t ip_dst;
81 } pj_pcap_ip_hdr;
82
83 /* Implementation of pcap file */
84 struct pj_pcap_file
85 {
86 char obj_name[PJ_MAX_OBJ_NAME];
87 pj_oshandle_t fd;
88 pj_bool_t swap;
89 pj_pcap_hdr hdr;
90 pj_pcap_filter filter;
91 };
92
93 #pragma pack()
94
95 /* Init default filter */
pj_pcap_filter_default(pj_pcap_filter * filter)96 PJ_DEF(void) pj_pcap_filter_default(pj_pcap_filter *filter)
97 {
98 pj_bzero(filter, sizeof(*filter));
99 }
100
101 /* Open pcap file */
pj_pcap_open(pj_pool_t * pool,const char * path,pj_pcap_file ** p_file)102 PJ_DEF(pj_status_t) pj_pcap_open(pj_pool_t *pool,
103 const char *path,
104 pj_pcap_file **p_file)
105 {
106 pj_pcap_file *file;
107 pj_ssize_t sz;
108 pj_status_t status;
109
110 PJ_ASSERT_RETURN(pool && path && p_file, PJ_EINVAL);
111
112 /* More sanity checks */
113 TRACE_(("pcap", "sizeof(pj_pcap_eth_hdr)=%d",
114 sizeof(pj_pcap_eth_hdr)));
115 PJ_ASSERT_RETURN(sizeof(pj_pcap_eth_hdr)==14, PJ_EBUG);
116 TRACE_(("pcap", "sizeof(pj_pcap_ip_hdr)=%d",
117 sizeof(pj_pcap_ip_hdr)));
118 PJ_ASSERT_RETURN(sizeof(pj_pcap_ip_hdr)==20, PJ_EBUG);
119 TRACE_(("pcap", "sizeof(pj_pcap_udp_hdr)=%d",
120 sizeof(pj_pcap_udp_hdr)));
121 PJ_ASSERT_RETURN(sizeof(pj_pcap_udp_hdr)==8, PJ_EBUG);
122
123 file = PJ_POOL_ZALLOC_T(pool, pj_pcap_file);
124
125 pj_ansi_strcpy(file->obj_name, "pcap");
126
127 status = pj_file_open(pool, path, PJ_O_RDONLY, &file->fd);
128 if (status != PJ_SUCCESS)
129 return status;
130
131 /* Read file pcap header */
132 sz = sizeof(file->hdr);
133 status = pj_file_read(file->fd, &file->hdr, &sz);
134 if (status != PJ_SUCCESS) {
135 pj_file_close(file->fd);
136 return status;
137 }
138
139 /* Check magic number */
140 if (file->hdr.magic_number == 0xa1b2c3d4) {
141 file->swap = PJ_FALSE;
142 } else if (file->hdr.magic_number == 0xd4c3b2a1) {
143 file->swap = PJ_TRUE;
144 file->hdr.network = pj_ntohl(file->hdr.network);
145 } else {
146 /* Not PCAP file */
147 pj_file_close(file->fd);
148 return PJ_EINVALIDOP;
149 }
150
151 TRACE_((file->obj_name, "PCAP file %s opened", path));
152
153 *p_file = file;
154 return PJ_SUCCESS;
155 }
156
157 /* Close pcap file */
pj_pcap_close(pj_pcap_file * file)158 PJ_DEF(pj_status_t) pj_pcap_close(pj_pcap_file *file)
159 {
160 PJ_ASSERT_RETURN(file, PJ_EINVAL);
161 TRACE_((file->obj_name, "PCAP file closed"));
162 return pj_file_close(file->fd);
163 }
164
165 /* Setup filter */
pj_pcap_set_filter(pj_pcap_file * file,const pj_pcap_filter * fil)166 PJ_DEF(pj_status_t) pj_pcap_set_filter(pj_pcap_file *file,
167 const pj_pcap_filter *fil)
168 {
169 PJ_ASSERT_RETURN(file && fil, PJ_EINVAL);
170 pj_memcpy(&file->filter, fil, sizeof(pj_pcap_filter));
171 return PJ_SUCCESS;
172 }
173
174 /* Read file */
read_file(pj_pcap_file * file,void * buf,pj_ssize_t * sz)175 static pj_status_t read_file(pj_pcap_file *file,
176 void *buf,
177 pj_ssize_t *sz)
178 {
179 pj_status_t status;
180 status = pj_file_read(file->fd, buf, sz);
181 if (status != PJ_SUCCESS)
182 return status;
183 if (*sz == 0)
184 return PJ_EEOF;
185 return PJ_SUCCESS;
186 }
187
skip(pj_oshandle_t fd,pj_off_t bytes)188 static pj_status_t skip(pj_oshandle_t fd, pj_off_t bytes)
189 {
190 pj_status_t status;
191 status = pj_file_setpos(fd, bytes, PJ_SEEK_CUR);
192 if (status != PJ_SUCCESS)
193 return status;
194 return PJ_SUCCESS;
195 }
196
197
198 #define SKIP_PKT() \
199 if (rec_incl > sz_read) { \
200 status = skip(file->fd, rec_incl-sz_read);\
201 if (status != PJ_SUCCESS) \
202 return status; \
203 }
204
205 /* Read UDP packet */
pj_pcap_read_udp(pj_pcap_file * file,pj_pcap_udp_hdr * udp_hdr,pj_uint8_t * udp_payload,pj_size_t * udp_payload_size)206 PJ_DEF(pj_status_t) pj_pcap_read_udp(pj_pcap_file *file,
207 pj_pcap_udp_hdr *udp_hdr,
208 pj_uint8_t *udp_payload,
209 pj_size_t *udp_payload_size)
210 {
211 PJ_ASSERT_RETURN(file && udp_payload && udp_payload_size, PJ_EINVAL);
212 PJ_ASSERT_RETURN(*udp_payload_size, PJ_EINVAL);
213
214 /* Check data link type in PCAP file header */
215 if ((file->filter.link &&
216 file->hdr.network != (pj_uint32_t)file->filter.link) ||
217 file->hdr.network != PJ_PCAP_LINK_TYPE_ETH)
218 {
219 /* Link header other than Ethernet is not supported for now */
220 return PJ_ENOTSUP;
221 }
222
223 /* Loop until we have the packet */
224 for (;;) {
225 union {
226 pj_pcap_rec_hdr rec;
227 pj_pcap_eth_hdr eth;
228 pj_pcap_ip_hdr ip;
229 pj_pcap_udp_hdr udp;
230 } tmp;
231 unsigned rec_incl;
232 pj_ssize_t sz;
233 pj_size_t sz_read = 0;
234 char addr[PJ_INET_ADDRSTRLEN];
235 pj_status_t status;
236
237 TRACE_((file->obj_name, "Reading packet.."));
238 pj_bzero(&addr, sizeof(addr));
239
240 /* Read PCAP packet header */
241 sz = sizeof(tmp.rec);
242 status = read_file(file, &tmp.rec, &sz);
243 if (status != PJ_SUCCESS) {
244 TRACE_((file->obj_name, "read_file() error: %d", status));
245 return status;
246 }
247
248 rec_incl = tmp.rec.incl_len;
249
250 /* Swap byte ordering */
251 if (file->swap) {
252 tmp.rec.incl_len = pj_ntohl(tmp.rec.incl_len);
253 tmp.rec.orig_len = pj_ntohl(tmp.rec.orig_len);
254 tmp.rec.ts_sec = pj_ntohl(tmp.rec.ts_sec);
255 tmp.rec.ts_usec = pj_ntohl(tmp.rec.ts_usec);
256 }
257
258 /* Read link layer header */
259 switch (file->hdr.network) {
260 case PJ_PCAP_LINK_TYPE_ETH:
261 sz = sizeof(tmp.eth);
262 status = read_file(file, &tmp.eth, &sz);
263 break;
264 default:
265 TRACE_((file->obj_name, "Error: link layer not Ethernet"));
266 return PJ_ENOTSUP;
267 }
268
269 if (status != PJ_SUCCESS) {
270 TRACE_((file->obj_name, "Error reading Eth header: %d", status));
271 return status;
272 }
273
274 sz_read += sz;
275
276 /* Read IP header */
277 sz = sizeof(tmp.ip);
278 status = read_file(file, &tmp.ip, &sz);
279 if (status != PJ_SUCCESS) {
280 TRACE_((file->obj_name, "Error reading IP header: %d", status));
281 return status;
282 }
283
284 sz_read += sz;
285
286 /* Skip if IP source mismatch */
287 if (file->filter.ip_src && tmp.ip.ip_src != file->filter.ip_src) {
288 TRACE_((file->obj_name, "IP source %s mismatch, skipping",
289 pj_inet_ntop2(pj_AF_INET(), (pj_in_addr*)&tmp.ip.ip_src,
290 addr, sizeof(addr))));
291 SKIP_PKT();
292 continue;
293 }
294
295 /* Skip if IP destination mismatch */
296 if (file->filter.ip_dst && tmp.ip.ip_dst != file->filter.ip_dst) {
297 TRACE_((file->obj_name, "IP detination %s mismatch, skipping",
298 pj_inet_ntop2(pj_AF_INET(), (pj_in_addr*)&tmp.ip.ip_dst,
299 addr, sizeof(addr))));
300 SKIP_PKT();
301 continue;
302 }
303
304 /* Skip if proto mismatch */
305 if (file->filter.proto && tmp.ip.proto != file->filter.proto) {
306 TRACE_((file->obj_name, "IP proto %d mismatch, skipping",
307 tmp.ip.proto));
308 SKIP_PKT();
309 continue;
310 }
311
312 /* Read transport layer header */
313 switch (tmp.ip.proto) {
314 case PJ_PCAP_PROTO_TYPE_UDP:
315 sz = sizeof(tmp.udp);
316 status = read_file(file, &tmp.udp, &sz);
317 if (status != PJ_SUCCESS) {
318 TRACE_((file->obj_name, "Error reading UDP header: %d",status));
319 return status;
320 }
321
322 sz_read += sz;
323
324 /* Skip if source port mismatch */
325 if (file->filter.src_port &&
326 tmp.udp.src_port != file->filter.src_port)
327 {
328 TRACE_((file->obj_name, "UDP src port %d mismatch, skipping",
329 pj_ntohs(tmp.udp.src_port)));
330 SKIP_PKT();
331 continue;
332 }
333
334 /* Skip if destination port mismatch */
335 if (file->filter.dst_port &&
336 tmp.udp.dst_port != file->filter.dst_port)
337 {
338 TRACE_((file->obj_name, "UDP dst port %d mismatch, skipping",
339 pj_ntohs(tmp.udp.dst_port)));
340 SKIP_PKT();
341 continue;
342 }
343
344 /* Copy UDP header if caller wants it */
345 if (udp_hdr) {
346 pj_memcpy(udp_hdr, &tmp.udp, sizeof(*udp_hdr));
347 }
348
349 /* Calculate payload size */
350 sz = pj_ntohs(tmp.udp.len) - sizeof(tmp.udp);
351 break;
352 default:
353 TRACE_((file->obj_name, "Not UDP, skipping"));
354 SKIP_PKT();
355 continue;
356 }
357
358 /* Check if payload fits the buffer */
359 if (sz > (pj_ssize_t)*udp_payload_size) {
360 TRACE_((file->obj_name,
361 "Error: packet too large (%d bytes required)", sz));
362 SKIP_PKT();
363 return PJ_ETOOSMALL;
364 }
365
366 /* Read the payload */
367 status = read_file(file, udp_payload, &sz);
368 if (status != PJ_SUCCESS) {
369 TRACE_((file->obj_name, "Error reading payload: %d", status));
370 return status;
371 }
372
373 sz_read += sz;
374
375 *udp_payload_size = sz;
376
377 // Some layers may have trailer, e.g: link eth2.
378 /* Check that we've read all the packets */
379 //PJ_ASSERT_RETURN(sz_read == rec_incl, PJ_EBUG);
380
381 /* Skip trailer */
382 while (sz_read < rec_incl) {
383 sz = rec_incl - sz_read;
384 status = read_file(file, &tmp.eth, &sz);
385 if (status != PJ_SUCCESS) {
386 TRACE_((file->obj_name, "Error reading trailer: %d", status));
387 return status;
388 }
389 sz_read += sz;
390 }
391
392 return PJ_SUCCESS;
393 }
394
395 /* Does not reach here */
396 }
397
398
399