1*b636d99dSDavid van Moolenbroek /*-
2*b636d99dSDavid van Moolenbroek * Copyright (c) 2003, 2004 David Young. All rights reserved.
3*b636d99dSDavid van Moolenbroek *
4*b636d99dSDavid van Moolenbroek * Redistribution and use in source and binary forms, with or without
5*b636d99dSDavid van Moolenbroek * modification, are permitted provided that the following conditions
6*b636d99dSDavid van Moolenbroek * are met:
7*b636d99dSDavid van Moolenbroek * 1. Redistributions of source code must retain the above copyright
8*b636d99dSDavid van Moolenbroek * notice, this list of conditions and the following disclaimer.
9*b636d99dSDavid van Moolenbroek * 2. Redistributions in binary form must reproduce the above copyright
10*b636d99dSDavid van Moolenbroek * notice, this list of conditions and the following disclaimer in the
11*b636d99dSDavid van Moolenbroek * documentation and/or other materials provided with the distribution.
12*b636d99dSDavid van Moolenbroek *
13*b636d99dSDavid van Moolenbroek * THIS SOFTWARE IS PROVIDED BY DAVID YOUNG ``AS IS'' AND ANY
14*b636d99dSDavid van Moolenbroek * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
15*b636d99dSDavid van Moolenbroek * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
16*b636d99dSDavid van Moolenbroek * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL DAVID
17*b636d99dSDavid van Moolenbroek * YOUNG BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
18*b636d99dSDavid van Moolenbroek * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
19*b636d99dSDavid van Moolenbroek * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
20*b636d99dSDavid van Moolenbroek * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
21*b636d99dSDavid van Moolenbroek * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
22*b636d99dSDavid van Moolenbroek * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23*b636d99dSDavid van Moolenbroek * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
24*b636d99dSDavid van Moolenbroek * OF SUCH DAMAGE.
25*b636d99dSDavid van Moolenbroek */
26*b636d99dSDavid van Moolenbroek
27*b636d99dSDavid van Moolenbroek #define NETDISSECT_REWORKED
28*b636d99dSDavid van Moolenbroek #ifdef HAVE_CONFIG_H
29*b636d99dSDavid van Moolenbroek #include "config.h"
30*b636d99dSDavid van Moolenbroek #endif
31*b636d99dSDavid van Moolenbroek
32*b636d99dSDavid van Moolenbroek #include <stdlib.h>
33*b636d99dSDavid van Moolenbroek #include <string.h>
34*b636d99dSDavid van Moolenbroek #include <tcpdump-stdinc.h>
35*b636d99dSDavid van Moolenbroek
36*b636d99dSDavid van Moolenbroek #include "cpack.h"
37*b636d99dSDavid van Moolenbroek #include "extract.h"
38*b636d99dSDavid van Moolenbroek
39*b636d99dSDavid van Moolenbroek uint8_t *
cpack_next_boundary(uint8_t * buf,uint8_t * p,size_t alignment)40*b636d99dSDavid van Moolenbroek cpack_next_boundary(uint8_t *buf, uint8_t *p, size_t alignment)
41*b636d99dSDavid van Moolenbroek {
42*b636d99dSDavid van Moolenbroek size_t misalignment = (size_t)(p - buf) % alignment;
43*b636d99dSDavid van Moolenbroek
44*b636d99dSDavid van Moolenbroek if (misalignment == 0)
45*b636d99dSDavid van Moolenbroek return p;
46*b636d99dSDavid van Moolenbroek
47*b636d99dSDavid van Moolenbroek return p + (alignment - misalignment);
48*b636d99dSDavid van Moolenbroek }
49*b636d99dSDavid van Moolenbroek
50*b636d99dSDavid van Moolenbroek /* Advance to the next wordsize boundary. Return NULL if fewer than
51*b636d99dSDavid van Moolenbroek * wordsize bytes remain in the buffer after the boundary. Otherwise,
52*b636d99dSDavid van Moolenbroek * return a pointer to the boundary.
53*b636d99dSDavid van Moolenbroek */
54*b636d99dSDavid van Moolenbroek uint8_t *
cpack_align_and_reserve(struct cpack_state * cs,size_t wordsize)55*b636d99dSDavid van Moolenbroek cpack_align_and_reserve(struct cpack_state *cs, size_t wordsize)
56*b636d99dSDavid van Moolenbroek {
57*b636d99dSDavid van Moolenbroek uint8_t *next;
58*b636d99dSDavid van Moolenbroek
59*b636d99dSDavid van Moolenbroek /* Ensure alignment. */
60*b636d99dSDavid van Moolenbroek next = cpack_next_boundary(cs->c_buf, cs->c_next, wordsize);
61*b636d99dSDavid van Moolenbroek
62*b636d99dSDavid van Moolenbroek /* Too little space for wordsize bytes? */
63*b636d99dSDavid van Moolenbroek if (next - cs->c_buf + wordsize > cs->c_len)
64*b636d99dSDavid van Moolenbroek return NULL;
65*b636d99dSDavid van Moolenbroek
66*b636d99dSDavid van Moolenbroek return next;
67*b636d99dSDavid van Moolenbroek }
68*b636d99dSDavid van Moolenbroek
69*b636d99dSDavid van Moolenbroek /* Advance by N bytes without returning them. */
70*b636d99dSDavid van Moolenbroek int
cpack_advance(struct cpack_state * cs,const size_t toskip)71*b636d99dSDavid van Moolenbroek cpack_advance(struct cpack_state *cs, const size_t toskip)
72*b636d99dSDavid van Moolenbroek {
73*b636d99dSDavid van Moolenbroek /* No space left? */
74*b636d99dSDavid van Moolenbroek if (cs->c_next - cs->c_buf + toskip > cs->c_len)
75*b636d99dSDavid van Moolenbroek return -1;
76*b636d99dSDavid van Moolenbroek cs->c_next += toskip;
77*b636d99dSDavid van Moolenbroek return 0;
78*b636d99dSDavid van Moolenbroek }
79*b636d99dSDavid van Moolenbroek
80*b636d99dSDavid van Moolenbroek int
cpack_init(struct cpack_state * cs,uint8_t * buf,size_t buflen)81*b636d99dSDavid van Moolenbroek cpack_init(struct cpack_state *cs, uint8_t *buf, size_t buflen)
82*b636d99dSDavid van Moolenbroek {
83*b636d99dSDavid van Moolenbroek memset(cs, 0, sizeof(*cs));
84*b636d99dSDavid van Moolenbroek
85*b636d99dSDavid van Moolenbroek cs->c_buf = buf;
86*b636d99dSDavid van Moolenbroek cs->c_len = buflen;
87*b636d99dSDavid van Moolenbroek cs->c_next = cs->c_buf;
88*b636d99dSDavid van Moolenbroek
89*b636d99dSDavid van Moolenbroek return 0;
90*b636d99dSDavid van Moolenbroek }
91*b636d99dSDavid van Moolenbroek
92*b636d99dSDavid van Moolenbroek /* Unpack a 64-bit unsigned integer. */
93*b636d99dSDavid van Moolenbroek int
cpack_uint64(struct cpack_state * cs,uint64_t * u)94*b636d99dSDavid van Moolenbroek cpack_uint64(struct cpack_state *cs, uint64_t *u)
95*b636d99dSDavid van Moolenbroek {
96*b636d99dSDavid van Moolenbroek uint8_t *next;
97*b636d99dSDavid van Moolenbroek
98*b636d99dSDavid van Moolenbroek if ((next = cpack_align_and_reserve(cs, sizeof(*u))) == NULL)
99*b636d99dSDavid van Moolenbroek return -1;
100*b636d99dSDavid van Moolenbroek
101*b636d99dSDavid van Moolenbroek *u = EXTRACT_LE_64BITS(next);
102*b636d99dSDavid van Moolenbroek
103*b636d99dSDavid van Moolenbroek /* Move pointer past the uint64_t. */
104*b636d99dSDavid van Moolenbroek cs->c_next = next + sizeof(*u);
105*b636d99dSDavid van Moolenbroek return 0;
106*b636d99dSDavid van Moolenbroek }
107*b636d99dSDavid van Moolenbroek
108*b636d99dSDavid van Moolenbroek /* Unpack a 32-bit unsigned integer. */
109*b636d99dSDavid van Moolenbroek int
cpack_uint32(struct cpack_state * cs,uint32_t * u)110*b636d99dSDavid van Moolenbroek cpack_uint32(struct cpack_state *cs, uint32_t *u)
111*b636d99dSDavid van Moolenbroek {
112*b636d99dSDavid van Moolenbroek uint8_t *next;
113*b636d99dSDavid van Moolenbroek
114*b636d99dSDavid van Moolenbroek if ((next = cpack_align_and_reserve(cs, sizeof(*u))) == NULL)
115*b636d99dSDavid van Moolenbroek return -1;
116*b636d99dSDavid van Moolenbroek
117*b636d99dSDavid van Moolenbroek *u = EXTRACT_LE_32BITS(next);
118*b636d99dSDavid van Moolenbroek
119*b636d99dSDavid van Moolenbroek /* Move pointer past the uint32_t. */
120*b636d99dSDavid van Moolenbroek cs->c_next = next + sizeof(*u);
121*b636d99dSDavid van Moolenbroek return 0;
122*b636d99dSDavid van Moolenbroek }
123*b636d99dSDavid van Moolenbroek
124*b636d99dSDavid van Moolenbroek /* Unpack a 16-bit unsigned integer. */
125*b636d99dSDavid van Moolenbroek int
cpack_uint16(struct cpack_state * cs,uint16_t * u)126*b636d99dSDavid van Moolenbroek cpack_uint16(struct cpack_state *cs, uint16_t *u)
127*b636d99dSDavid van Moolenbroek {
128*b636d99dSDavid van Moolenbroek uint8_t *next;
129*b636d99dSDavid van Moolenbroek
130*b636d99dSDavid van Moolenbroek if ((next = cpack_align_and_reserve(cs, sizeof(*u))) == NULL)
131*b636d99dSDavid van Moolenbroek return -1;
132*b636d99dSDavid van Moolenbroek
133*b636d99dSDavid van Moolenbroek *u = EXTRACT_LE_16BITS(next);
134*b636d99dSDavid van Moolenbroek
135*b636d99dSDavid van Moolenbroek /* Move pointer past the uint16_t. */
136*b636d99dSDavid van Moolenbroek cs->c_next = next + sizeof(*u);
137*b636d99dSDavid van Moolenbroek return 0;
138*b636d99dSDavid van Moolenbroek }
139*b636d99dSDavid van Moolenbroek
140*b636d99dSDavid van Moolenbroek /* Unpack an 8-bit unsigned integer. */
141*b636d99dSDavid van Moolenbroek int
cpack_uint8(struct cpack_state * cs,uint8_t * u)142*b636d99dSDavid van Moolenbroek cpack_uint8(struct cpack_state *cs, uint8_t *u)
143*b636d99dSDavid van Moolenbroek {
144*b636d99dSDavid van Moolenbroek /* No space left? */
145*b636d99dSDavid van Moolenbroek if ((size_t)(cs->c_next - cs->c_buf) >= cs->c_len)
146*b636d99dSDavid van Moolenbroek return -1;
147*b636d99dSDavid van Moolenbroek
148*b636d99dSDavid van Moolenbroek *u = *cs->c_next;
149*b636d99dSDavid van Moolenbroek
150*b636d99dSDavid van Moolenbroek /* Move pointer past the uint8_t. */
151*b636d99dSDavid van Moolenbroek cs->c_next++;
152*b636d99dSDavid van Moolenbroek return 0;
153*b636d99dSDavid van Moolenbroek }
154