Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 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 | pj_size_t 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 | |