1 /* $OpenBSD: packet.c,v 1.1 2017/03/17 14:45:16 rzalamena Exp $ */
2
3 /* Packet assembly code, originally contributed by Archie Cobbs. */
4
5 /*
6 * Copyright (c) 1995, 1996, 1999 The Internet Software Consortium.
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * 1. Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * 2. Redistributions in binary form must reproduce the above copyright
16 * notice, this list of conditions and the following disclaimer in the
17 * documentation and/or other materials provided with the distribution.
18 * 3. Neither the name of The Internet Software Consortium nor the names
19 * of its contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE INTERNET SOFTWARE CONSORTIUM AND
23 * CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
24 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
25 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 * DISCLAIMED. IN NO EVENT SHALL THE INTERNET SOFTWARE CONSORTIUM OR
27 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
28 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
29 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
30 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
33 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
34 * SUCH DAMAGE.
35 *
36 * This software has been written for the Internet Software Consortium
37 * by Ted Lemon <mellon@fugue.com> in cooperation with Vixie
38 * Enterprises. To learn more about the Internet Software Consortium,
39 * see ``http://www.vix.com/isc''. To learn more about Vixie
40 * Enterprises, see ``http://www.vix.com''.
41 */
42
43 #include <sys/types.h>
44 #include <sys/socket.h>
45
46 #include <arpa/inet.h>
47
48 #include <net/if.h>
49
50 #include <netinet/in.h>
51 #include <netinet/ip.h>
52 #include <netinet/ip6.h>
53 #include <netinet/udp.h>
54 #include <netinet/if_ether.h>
55
56 #include <string.h>
57
58 #include "dhcp.h"
59 #include "dhcpd.h"
60 #include "log.h"
61
62
63 u_int32_t checksum(unsigned char *, unsigned, u_int32_t);
64 u_int32_t wrapsum(u_int32_t);
65
66 u_int32_t
checksum(unsigned char * buf,unsigned nbytes,u_int32_t sum)67 checksum(unsigned char *buf, unsigned nbytes, u_int32_t sum)
68 {
69 unsigned int i;
70
71 /* Checksum all the pairs of bytes first... */
72 for (i = 0; i < (nbytes & ~1U); i += 2) {
73 sum += (u_int16_t)ntohs(*((u_int16_t *)(buf + i)));
74 if (sum > 0xFFFF)
75 sum -= 0xFFFF;
76 }
77
78 /*
79 * If there's a single byte left over, checksum it, too.
80 * Network byte order is big-endian, so the remaining byte is
81 * the high byte.
82 */
83 if (i < nbytes) {
84 sum += buf[i] << 8;
85 if (sum > 0xFFFF)
86 sum -= 0xFFFF;
87 }
88
89 return (sum);
90 }
91
92 u_int32_t
wrapsum(u_int32_t sum)93 wrapsum(u_int32_t sum)
94 {
95 sum = ~sum & 0xFFFF;
96 return (htons(sum));
97 }
98
99 void
assemble_hw_header(unsigned char * buf,int * bufix,struct packet_ctx * pc)100 assemble_hw_header(unsigned char *buf, int *bufix, struct packet_ctx *pc)
101 {
102 struct ether_header eh;
103
104 memcpy(eh.ether_shost, pc->pc_smac, ETHER_ADDR_LEN);
105 memcpy(eh.ether_dhost, pc->pc_dmac, ETHER_ADDR_LEN);
106 eh.ether_type = htons(pc->pc_ethertype);
107
108 memcpy(&buf[*bufix], &eh, ETHER_HDR_LEN);
109 *bufix += ETHER_HDR_LEN;
110 }
111
112 void
assemble_udp_ip6_header(unsigned char * p,int * off,struct packet_ctx * pc,unsigned char * payload,int plen)113 assemble_udp_ip6_header(unsigned char *p, int *off, struct packet_ctx *pc,
114 unsigned char *payload, int plen)
115 {
116 struct ip6_hdr ip6;
117 struct udphdr uh;
118
119 memset(&ip6, 0, sizeof(ip6));
120 ip6.ip6_vfc = IPV6_VERSION;
121 ip6.ip6_nxt = IPPROTO_UDP;
122 ip6.ip6_src = ss2sin6(&pc->pc_src)->sin6_addr;
123 ip6.ip6_dst = ss2sin6(&pc->pc_dst)->sin6_addr;
124 ip6.ip6_plen = htons(sizeof(uh) + plen);
125 ip6.ip6_hlim = 64;
126 memcpy(&p[*off], &ip6, sizeof(ip6));
127 *off += sizeof(ip6);
128
129 memset(&uh, 0, sizeof(uh));
130 uh.uh_ulen = ip6.ip6_plen;
131 uh.uh_sport = ss2sin6(&pc->pc_src)->sin6_port;
132 uh.uh_dport = ss2sin6(&pc->pc_dst)->sin6_port;
133 uh.uh_sum = wrapsum(
134 checksum((unsigned char *)&uh, sizeof(uh),
135 checksum(payload, plen,
136 checksum((unsigned char *)&ip6.ip6_src, sizeof(ip6.ip6_src),
137 checksum((unsigned char *)&ip6.ip6_dst, sizeof(ip6.ip6_dst),
138 IPPROTO_UDP + ntohs(ip6.ip6_plen)
139 ))))
140 );
141 memcpy(&p[*off], &uh, sizeof(uh));
142 *off += sizeof(uh);
143 }
144
145 ssize_t
decode_hw_header(unsigned char * buf,int bufix,struct packet_ctx * pc)146 decode_hw_header(unsigned char *buf, int bufix, struct packet_ctx *pc)
147 {
148 struct ether_header *ether;
149
150 ether = (struct ether_header *)(buf + bufix);
151 memcpy(pc->pc_dmac, ether->ether_dhost, ETHER_ADDR_LEN);
152 memcpy(pc->pc_smac, ether->ether_shost, ETHER_ADDR_LEN);
153 pc->pc_ethertype = ntohs(ether->ether_type);
154
155 pc->pc_htype = ARPHRD_ETHER;
156 pc->pc_hlen = ETHER_ADDR_LEN;
157
158 return sizeof(struct ether_header);
159 }
160
161 ssize_t
decode_udp_ip6_header(unsigned char * p,int off,struct packet_ctx * pc,size_t plen)162 decode_udp_ip6_header(unsigned char *p, int off, struct packet_ctx *pc,
163 size_t plen)
164 {
165 struct ip6_hdr *ip6;
166 struct udphdr *uh;
167 struct in6_addr *asrc, *adst;
168 size_t ptotal, poff = 0;
169 uint16_t ocksum, cksum;
170
171 /* Check the IPv6 header. */
172 if (plen < sizeof(*ip6)) {
173 log_debug("package too small (%ld)", plen);
174 return -1;
175 }
176
177 ip6 = (struct ip6_hdr *)(p + off);
178 if ((ip6->ip6_vfc & IPV6_VERSION_MASK) != IPV6_VERSION) {
179 log_debug("invalid IPv6 version");
180 return -1;
181 }
182
183 poff += sizeof(*ip6);
184
185 ptotal = ntohs(ip6->ip6_plen);
186 if (ptotal > plen) {
187 log_debug("expected %ld bytes, but got %ld", ptotal, plen);
188 return (-1);
189 }
190
191 pc->pc_src.ss_len = sizeof(struct sockaddr_in6);
192 pc->pc_src.ss_family = AF_INET6;
193 asrc = &ss2sin6(&pc->pc_src)->sin6_addr;
194 memcpy(asrc, &ip6->ip6_src, sizeof(*asrc));
195
196 pc->pc_dst.ss_len = sizeof(struct sockaddr_in6);
197 pc->pc_dst.ss_family = AF_INET6;
198 adst = &ss2sin6(&pc->pc_dst)->sin6_addr;
199 memcpy(adst, &ip6->ip6_dst, sizeof(*adst));
200
201 /* Deal with the UDP header. */
202 if (ip6->ip6_nxt != IPPROTO_UDP) {
203 /* We don't support skipping extensions yet. */
204 log_debug("expected UDP header, got %#02X", ip6->ip6_nxt);
205 return -1;
206 }
207
208 uh = (struct udphdr *)((uint8_t *)ip6 + sizeof(*ip6));
209 ss2sin6(&pc->pc_src)->sin6_port = uh->uh_sport;
210 ss2sin6(&pc->pc_dst)->sin6_port = uh->uh_dport;
211 ocksum = uh->uh_sum;
212 uh->uh_sum = 0;
213 poff += sizeof(*uh);
214
215 /* Validate the packet. */
216 cksum = wrapsum(
217 checksum((unsigned char *)asrc, sizeof(*asrc),
218 checksum((unsigned char *)adst, sizeof(*adst),
219 checksum((unsigned char *)uh, sizeof(*uh),
220 checksum(p + off + poff, ptotal - sizeof(*uh),
221 IPPROTO_UDP + ntohs(uh->uh_ulen)))))
222 );
223
224 if (ocksum != cksum) {
225 log_debug("checksum invalid (%#04x != %#04x)",
226 ocksum, cksum);
227 return -1;
228 }
229
230 return poff;
231 }
232