Add support for ICE/STUN/TURN in res_rtp_asterisk and chan_sip.
[asterisk/asterisk.git] / res / pjproject / pjlib-util / src / pjlib-util / pcap.c
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 /* Init default filter */
94 PJ_DEF(void) pj_pcap_filter_default(pj_pcap_filter *filter)
95 {
96     pj_bzero(filter, sizeof(*filter));
97 }
98
99 /* Open pcap file */
100 PJ_DEF(pj_status_t) pj_pcap_open(pj_pool_t *pool,
101                                  const char *path,
102                                  pj_pcap_file **p_file)
103 {
104     pj_pcap_file *file;
105     pj_ssize_t sz;
106     pj_status_t status;
107
108     PJ_ASSERT_RETURN(pool && path && p_file, PJ_EINVAL);
109
110     /* More sanity checks */
111     TRACE_(("pcap", "sizeof(pj_pcap_eth_hdr)=%d",
112             sizeof(pj_pcap_eth_hdr)));
113     PJ_ASSERT_RETURN(sizeof(pj_pcap_eth_hdr)==14, PJ_EBUG);
114     TRACE_(("pcap", "sizeof(pj_pcap_ip_hdr)=%d",
115             sizeof(pj_pcap_ip_hdr)));
116     PJ_ASSERT_RETURN(sizeof(pj_pcap_ip_hdr)==20, PJ_EBUG);
117     TRACE_(("pcap", "sizeof(pj_pcap_udp_hdr)=%d",
118             sizeof(pj_pcap_udp_hdr)));
119     PJ_ASSERT_RETURN(sizeof(pj_pcap_udp_hdr)==8, PJ_EBUG);
120     
121     file = PJ_POOL_ZALLOC_T(pool, pj_pcap_file);
122
123     pj_ansi_strcpy(file->obj_name, "pcap");
124
125     status = pj_file_open(pool, path, PJ_O_RDONLY, &file->fd);
126     if (status != PJ_SUCCESS)
127         return status;
128
129     /* Read file pcap header */
130     sz = sizeof(file->hdr);
131     status = pj_file_read(file->fd, &file->hdr, &sz);
132     if (status != PJ_SUCCESS) {
133         pj_file_close(file->fd);
134         return status;
135     }
136
137     /* Check magic number */
138     if (file->hdr.magic_number == 0xa1b2c3d4) {
139         file->swap = PJ_FALSE;
140     } else if (file->hdr.magic_number == 0xd4c3b2a1) {
141         file->swap = PJ_TRUE;
142         file->hdr.network = pj_ntohl(file->hdr.network);
143     } else {
144         /* Not PCAP file */
145         pj_file_close(file->fd);
146         return PJ_EINVALIDOP;
147     }
148
149     TRACE_((file->obj_name, "PCAP file %s opened", path));
150     
151     *p_file = file;
152     return PJ_SUCCESS;
153 }
154
155 /* Close pcap file */
156 PJ_DEF(pj_status_t) pj_pcap_close(pj_pcap_file *file)
157 {
158     PJ_ASSERT_RETURN(file, PJ_EINVAL);
159     TRACE_((file->obj_name, "PCAP file closed"));
160     return pj_file_close(file->fd);
161 }
162
163 /* Setup filter */
164 PJ_DEF(pj_status_t) pj_pcap_set_filter(pj_pcap_file *file,
165                                        const pj_pcap_filter *fil)
166 {
167     PJ_ASSERT_RETURN(file && fil, PJ_EINVAL);
168     pj_memcpy(&file->filter, fil, sizeof(pj_pcap_filter));
169     return PJ_SUCCESS;
170 }
171
172 /* Read file */
173 static pj_status_t read_file(pj_pcap_file *file,
174                              void *buf,
175                              pj_ssize_t *sz)
176 {
177     pj_status_t status;
178     status = pj_file_read(file->fd, buf, sz);
179     if (status != PJ_SUCCESS)
180         return status;
181     if (*sz == 0)
182         return PJ_EEOF;
183     return PJ_SUCCESS;
184 }
185
186 static pj_status_t skip(pj_oshandle_t fd, pj_off_t bytes)
187 {
188     pj_status_t status;
189     status = pj_file_setpos(fd, bytes, PJ_SEEK_CUR);
190     if (status != PJ_SUCCESS)
191         return status; 
192     return PJ_SUCCESS;
193 }
194
195
196 #define SKIP_PKT()  \
197         if (rec_incl > sz_read) { \
198             status = skip(file->fd, rec_incl-sz_read);\
199             if (status != PJ_SUCCESS) \
200                 return status; \
201         }
202
203 /* Read UDP packet */
204 PJ_DEF(pj_status_t) pj_pcap_read_udp(pj_pcap_file *file,
205                                      pj_pcap_udp_hdr *udp_hdr,
206                                      pj_uint8_t *udp_payload,
207                                      pj_size_t *udp_payload_size)
208 {
209     PJ_ASSERT_RETURN(file && udp_payload && udp_payload_size, PJ_EINVAL);
210     PJ_ASSERT_RETURN(*udp_payload_size, PJ_EINVAL);
211
212     /* Check data link type in PCAP file header */
213     if ((file->filter.link && 
214             file->hdr.network != (pj_uint32_t)file->filter.link) ||
215         file->hdr.network != PJ_PCAP_LINK_TYPE_ETH)
216     {
217         /* Link header other than Ethernet is not supported for now */
218         return PJ_ENOTSUP;
219     }
220
221     /* Loop until we have the packet */
222     for (;;) {
223         union {
224             pj_pcap_rec_hdr rec;
225             pj_pcap_eth_hdr eth;
226             pj_pcap_ip_hdr ip;
227             pj_pcap_udp_hdr udp;
228         } tmp;
229         unsigned rec_incl;
230         pj_ssize_t sz;
231         unsigned sz_read = 0;
232         pj_status_t status;
233
234         TRACE_((file->obj_name, "Reading packet.."));
235
236         /* Read PCAP packet header */
237         sz = sizeof(tmp.rec);
238         status = read_file(file, &tmp.rec, &sz);
239         if (status != PJ_SUCCESS) {
240             TRACE_((file->obj_name, "read_file() error: %d", status));
241             return status;
242         }
243
244         rec_incl = tmp.rec.incl_len;
245
246         /* Swap byte ordering */
247         if (file->swap) {
248             tmp.rec.incl_len = pj_ntohl(tmp.rec.incl_len);
249             tmp.rec.orig_len = pj_ntohl(tmp.rec.orig_len);
250             tmp.rec.ts_sec = pj_ntohl(tmp.rec.ts_sec);
251             tmp.rec.ts_usec = pj_ntohl(tmp.rec.ts_usec);
252         }
253
254         /* Read link layer header */
255         switch (file->hdr.network) {
256         case PJ_PCAP_LINK_TYPE_ETH:
257             sz = sizeof(tmp.eth);
258             status = read_file(file, &tmp.eth, &sz);
259             break;
260         default:
261             TRACE_((file->obj_name, "Error: link layer not Ethernet"));
262             return PJ_ENOTSUP;
263         }
264
265         if (status != PJ_SUCCESS) {
266             TRACE_((file->obj_name, "Error reading Eth header: %d", status));
267             return status;
268         }
269
270         sz_read += sz;
271             
272         /* Read IP header */
273         sz = sizeof(tmp.ip);
274         status = read_file(file, &tmp.ip, &sz);
275         if (status != PJ_SUCCESS) {
276             TRACE_((file->obj_name, "Error reading IP header: %d", status));
277             return status;
278         }
279
280         sz_read += sz;
281
282         /* Skip if IP source mismatch */
283         if (file->filter.ip_src && tmp.ip.ip_src != file->filter.ip_src) {
284             TRACE_((file->obj_name, "IP source %s mismatch, skipping", 
285                     pj_inet_ntoa(*(pj_in_addr*)&tmp.ip.ip_src)));
286             SKIP_PKT();
287             continue;
288         }
289
290         /* Skip if IP destination mismatch */
291         if (file->filter.ip_dst && tmp.ip.ip_dst != file->filter.ip_dst) {
292             TRACE_((file->obj_name, "IP detination %s mismatch, skipping", 
293                     pj_inet_ntoa(*(pj_in_addr*)&tmp.ip.ip_dst)));
294             SKIP_PKT();
295             continue;
296         }
297
298         /* Skip if proto mismatch */
299         if (file->filter.proto && tmp.ip.proto != file->filter.proto) {
300             TRACE_((file->obj_name, "IP proto %d mismatch, skipping", 
301                     tmp.ip.proto));
302             SKIP_PKT();
303             continue;
304         }
305
306         /* Read transport layer header */
307         switch (tmp.ip.proto) {
308         case PJ_PCAP_PROTO_TYPE_UDP:
309             sz = sizeof(tmp.udp);
310             status = read_file(file, &tmp.udp, &sz);
311             if (status != PJ_SUCCESS) {
312                 TRACE_((file->obj_name, "Error reading UDP header: %d",status));
313                 return status;
314             }
315
316             sz_read += sz;
317
318             /* Skip if source port mismatch */
319             if (file->filter.src_port && 
320                 tmp.udp.src_port != file->filter.src_port) 
321             {
322                 TRACE_((file->obj_name, "UDP src port %d mismatch, skipping", 
323                         pj_ntohs(tmp.udp.src_port)));
324                 SKIP_PKT();
325                 continue;
326             }
327
328             /* Skip if destination port mismatch */
329             if (file->filter.dst_port && 
330                 tmp.udp.dst_port != file->filter.dst_port) 
331             {
332                 TRACE_((file->obj_name, "UDP dst port %d mismatch, skipping", 
333                         pj_ntohs(tmp.udp.dst_port)));
334                 SKIP_PKT();
335                 continue;
336             }
337
338             /* Copy UDP header if caller wants it */
339             if (udp_hdr) {
340                 pj_memcpy(udp_hdr, &tmp.udp, sizeof(*udp_hdr));
341             }
342
343             /* Calculate payload size */
344             sz = pj_ntohs(tmp.udp.len) - sizeof(tmp.udp);
345             break;
346         default:
347             TRACE_((file->obj_name, "Not UDP, skipping"));
348             SKIP_PKT();
349             continue;
350         }
351
352         /* Check if payload fits the buffer */
353         if (sz > (pj_ssize_t)*udp_payload_size) {
354             TRACE_((file->obj_name, 
355                     "Error: packet too large (%d bytes required)", sz));
356             SKIP_PKT();
357             return PJ_ETOOSMALL;
358         }
359
360         /* Read the payload */
361         status = read_file(file, udp_payload, &sz);
362         if (status != PJ_SUCCESS) {
363             TRACE_((file->obj_name, "Error reading payload: %d", status));
364             return status;
365         }
366
367         sz_read += sz;
368
369         *udp_payload_size = sz;
370
371         // Some layers may have trailer, e.g: link eth2.
372         /* Check that we've read all the packets */
373         //PJ_ASSERT_RETURN(sz_read == rec_incl, PJ_EBUG);
374
375         /* Skip trailer */
376         while (sz_read < rec_incl) {
377             sz = rec_incl - sz_read;
378             status = read_file(file, &tmp.eth, &sz);
379             if (status != PJ_SUCCESS) {
380                 TRACE_((file->obj_name, "Error reading trailer: %d", status));
381                 return status;
382             }
383             sz_read += sz;
384         }
385
386         return PJ_SUCCESS;
387     }
388
389     /* Does not reach here */
390 }
391
392