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 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 93 wrapsum(u_int32_t sum) 94 { 95 sum = ~sum & 0xFFFF; 96 return (htons(sum)); 97 } 98 99 void 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 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 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 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