1 /*
2 * ip-util.c
3 *
4 * Copyright (c) 2002 Dug Song <dugsong@monkey.org>
5 *
6 * $Id$
7 */
8
9 #include "config.h"
10
11 #include <errno.h>
12 #include <stdlib.h>
13 #include <string.h>
14
15 #include "dnet.h"
16 #include "crc32ct.h"
17
18 /* CRC-32C (Castagnoli). Public domain. */
19 static unsigned long
_crc32c(unsigned char * buf,int len)20 _crc32c(unsigned char *buf, int len)
21 {
22 int i;
23 unsigned long crc32 = ~0L;
24 unsigned long result;
25 unsigned char byte0, byte1, byte2, byte3;
26
27 for (i = 0; i < len; i++) {
28 CRC32C(crc32, buf[i]);
29 }
30
31 result = ~crc32;
32
33 byte0 = result & 0xff;
34 byte1 = (result >> 8) & 0xff;
35 byte2 = (result >> 16) & 0xff;
36 byte3 = (result >> 24) & 0xff;
37 crc32 = ((byte0 << 24) | (byte1 << 16) | (byte2 << 8) | byte3);
38 return crc32;
39 }
40
41 ssize_t
ip_add_option(void * buf,size_t len,int proto,const void * optbuf,size_t optlen)42 ip_add_option(void *buf, size_t len, int proto,
43 const void *optbuf, size_t optlen)
44 {
45 struct ip_hdr *ip;
46 struct tcp_hdr *tcp = NULL;
47 u_char *p;
48 int hl, datalen, padlen;
49
50 if (proto != IP_PROTO_IP && proto != IP_PROTO_TCP) {
51 errno = EINVAL;
52 return (-1);
53 }
54 ip = (struct ip_hdr *)buf;
55 hl = ip->ip_hl << 2;
56 p = (u_char *)buf + hl;
57
58 if (proto == IP_PROTO_TCP) {
59 tcp = (struct tcp_hdr *)p;
60 hl = tcp->th_off << 2;
61 p = (u_char *)tcp + hl;
62 }
63 datalen = ntohs(ip->ip_len) - (p - (u_char *)buf);
64
65 /* Compute padding to next word boundary. */
66 if ((padlen = 4 - (optlen % 4)) == 4)
67 padlen = 0;
68
69 /* XXX - IP_HDR_LEN_MAX == TCP_HDR_LEN_MAX */
70 if (hl + optlen + padlen > IP_HDR_LEN_MAX ||
71 ntohs(ip->ip_len) + optlen + padlen > len) {
72 errno = EINVAL;
73 return (-1);
74 }
75 /* XXX - IP_OPT_TYPEONLY() == TCP_OPT_TYPEONLY */
76 if (IP_OPT_TYPEONLY(((struct ip_opt *)optbuf)->opt_type))
77 optlen = 1;
78
79 /* Shift any existing data. */
80 if (datalen) {
81 memmove(p + optlen + padlen, p, datalen);
82 }
83 /* XXX - IP_OPT_NOP == TCP_OPT_NOP */
84 if (padlen) {
85 memset(p, IP_OPT_NOP, padlen);
86 p += padlen;
87 }
88 memmove(p, optbuf, optlen);
89 p += optlen;
90 optlen += padlen;
91
92 if (proto == IP_PROTO_IP)
93 ip->ip_hl = (p - (u_char *)ip) >> 2;
94 else if (proto == IP_PROTO_TCP)
95 tcp->th_off = (p - (u_char *)tcp) >> 2;
96
97 ip->ip_len = htons(ntohs(ip->ip_len) + optlen);
98
99 return (optlen);
100 }
101
102 void
ip_checksum(void * buf,size_t len)103 ip_checksum(void *buf, size_t len)
104 {
105 struct ip_hdr *ip;
106 int hl, off, sum;
107
108 if (len < IP_HDR_LEN)
109 return;
110
111 ip = (struct ip_hdr *)buf;
112 hl = ip->ip_hl << 2;
113 ip->ip_sum = 0;
114 sum = ip_cksum_add(ip, hl, 0);
115 ip->ip_sum = ip_cksum_carry(sum);
116
117 off = htons(ip->ip_off);
118
119 if ((off & IP_OFFMASK) != 0 || (off & IP_MF) != 0)
120 return;
121
122 len -= hl;
123
124 if (ip->ip_p == IP_PROTO_TCP) {
125 struct tcp_hdr *tcp = (struct tcp_hdr *)((u_char *)ip + hl);
126
127 if (len >= TCP_HDR_LEN) {
128 tcp->th_sum = 0;
129 sum = ip_cksum_add(tcp, len, 0) +
130 htons(ip->ip_p + len);
131 sum = ip_cksum_add(&ip->ip_src, 8, sum);
132 tcp->th_sum = ip_cksum_carry(sum);
133 }
134 } else if (ip->ip_p == IP_PROTO_UDP) {
135 struct udp_hdr *udp = (struct udp_hdr *)((u_char *)ip + hl);
136
137 if (len >= UDP_HDR_LEN) {
138 udp->uh_sum = 0;
139 sum = ip_cksum_add(udp, len, 0) +
140 htons(ip->ip_p + len);
141 sum = ip_cksum_add(&ip->ip_src, 8, sum);
142 udp->uh_sum = ip_cksum_carry(sum);
143 if (!udp->uh_sum)
144 udp->uh_sum = 0xffff; /* RFC 768 */
145 }
146 } else if (ip->ip_p == IP_PROTO_SCTP) {
147 struct sctp_hdr *sctp = (struct sctp_hdr *)((u_char *)ip + hl);
148
149 if (len >= SCTP_HDR_LEN) {
150 sctp->sh_sum = 0;
151 sctp->sh_sum = htonl(_crc32c((u_char *)sctp, len));
152 }
153 } else if (ip->ip_p == IP_PROTO_ICMP || ip->ip_p == IP_PROTO_IGMP) {
154 struct icmp_hdr *icmp = (struct icmp_hdr *)((u_char *)ip + hl);
155
156 if (len >= ICMP_HDR_LEN) {
157 icmp->icmp_cksum = 0;
158 sum = ip_cksum_add(icmp, len, 0);
159 icmp->icmp_cksum = ip_cksum_carry(sum);
160 }
161 }
162 }
163
164 int
ip_cksum_add(const void * buf,size_t len,int cksum)165 ip_cksum_add(const void *buf, size_t len, int cksum)
166 {
167 uint16_t *sp = (uint16_t *)buf;
168 int n, sn;
169
170 sn = len / 2;
171 n = (sn + 15) / 16;
172
173 /* XXX - unroll loop using Duff's device. */
174 switch (sn % 16) {
175 case 0: do {
176 cksum += *sp++;
177 case 15:
178 cksum += *sp++;
179 case 14:
180 cksum += *sp++;
181 case 13:
182 cksum += *sp++;
183 case 12:
184 cksum += *sp++;
185 case 11:
186 cksum += *sp++;
187 case 10:
188 cksum += *sp++;
189 case 9:
190 cksum += *sp++;
191 case 8:
192 cksum += *sp++;
193 case 7:
194 cksum += *sp++;
195 case 6:
196 cksum += *sp++;
197 case 5:
198 cksum += *sp++;
199 case 4:
200 cksum += *sp++;
201 case 3:
202 cksum += *sp++;
203 case 2:
204 cksum += *sp++;
205 case 1:
206 cksum += *sp++;
207 } while (--n > 0);
208 }
209 if (len & 1)
210 cksum += htons(*(u_char *)sp << 8);
211
212 return (cksum);
213 }
214