1 | /* |
2 | * IP checksumming functions. |
3 | * (c) 2008 Gerd Hoffmann <kraxel@redhat.com> |
4 | * |
5 | * This program is free software; you can redistribute it and/or modify |
6 | * it under the terms of the GNU General Public License as published by |
7 | * the Free Software Foundation; under version 2 or later of the License. |
8 | * |
9 | * This program is distributed in the hope that it will be useful, |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
12 | * GNU General Public License for more details. |
13 | * |
14 | * You should have received a copy of the GNU General Public License |
15 | * along with this program; if not, see <http://www.gnu.org/licenses/>. |
16 | */ |
17 | |
18 | #include "qemu/osdep.h" |
19 | #include "net/checksum.h" |
20 | #include "net/eth.h" |
21 | |
22 | uint32_t net_checksum_add_cont(int len, uint8_t *buf, int seq) |
23 | { |
24 | uint32_t sum1 = 0, sum2 = 0; |
25 | int i; |
26 | |
27 | for (i = 0; i < len - 1; i += 2) { |
28 | sum1 += (uint32_t)buf[i]; |
29 | sum2 += (uint32_t)buf[i + 1]; |
30 | } |
31 | if (i < len) { |
32 | sum1 += (uint32_t)buf[i]; |
33 | } |
34 | |
35 | if (seq & 1) { |
36 | return sum1 + (sum2 << 8); |
37 | } else { |
38 | return sum2 + (sum1 << 8); |
39 | } |
40 | } |
41 | |
42 | uint16_t net_checksum_finish(uint32_t sum) |
43 | { |
44 | while (sum>>16) |
45 | sum = (sum & 0xFFFF)+(sum >> 16); |
46 | return ~sum; |
47 | } |
48 | |
49 | uint16_t net_checksum_tcpudp(uint16_t length, uint16_t proto, |
50 | uint8_t *addrs, uint8_t *buf) |
51 | { |
52 | uint32_t sum = 0; |
53 | |
54 | sum += net_checksum_add(length, buf); // payload |
55 | sum += net_checksum_add(8, addrs); // src + dst address |
56 | sum += proto + length; // protocol & length |
57 | return net_checksum_finish(sum); |
58 | } |
59 | |
60 | void net_checksum_calculate(uint8_t *data, int length) |
61 | { |
62 | int mac_hdr_len, ip_len; |
63 | struct ip_header *ip; |
64 | |
65 | /* |
66 | * Note: We cannot assume "data" is aligned, so the all code uses |
67 | * some macros that take care of possible unaligned access for |
68 | * struct members (just in case). |
69 | */ |
70 | |
71 | /* Ensure we have at least an Eth header */ |
72 | if (length < sizeof(struct eth_header)) { |
73 | return; |
74 | } |
75 | |
76 | /* Handle the optionnal VLAN headers */ |
77 | switch (lduw_be_p(&PKT_GET_ETH_HDR(data)->h_proto)) { |
78 | case ETH_P_VLAN: |
79 | mac_hdr_len = sizeof(struct eth_header) + |
80 | sizeof(struct vlan_header); |
81 | break; |
82 | case ETH_P_DVLAN: |
83 | if (lduw_be_p(&PKT_GET_VLAN_HDR(data)->h_proto) == ETH_P_VLAN) { |
84 | mac_hdr_len = sizeof(struct eth_header) + |
85 | 2 * sizeof(struct vlan_header); |
86 | } else { |
87 | mac_hdr_len = sizeof(struct eth_header) + |
88 | sizeof(struct vlan_header); |
89 | } |
90 | break; |
91 | default: |
92 | mac_hdr_len = sizeof(struct eth_header); |
93 | break; |
94 | } |
95 | |
96 | length -= mac_hdr_len; |
97 | |
98 | /* Now check we have an IP header (with an optionnal VLAN header) */ |
99 | if (length < sizeof(struct ip_header)) { |
100 | return; |
101 | } |
102 | |
103 | ip = (struct ip_header *)(data + mac_hdr_len); |
104 | |
105 | if (IP_HEADER_VERSION(ip) != IP_HEADER_VERSION_4) { |
106 | return; /* not IPv4 */ |
107 | } |
108 | |
109 | ip_len = lduw_be_p(&ip->ip_len); |
110 | |
111 | /* Last, check that we have enough data for the all IP frame */ |
112 | if (length < ip_len) { |
113 | return; |
114 | } |
115 | |
116 | ip_len -= IP_HDR_GET_LEN(ip); |
117 | |
118 | switch (ip->ip_p) { |
119 | case IP_PROTO_TCP: |
120 | { |
121 | uint16_t csum; |
122 | tcp_header *tcp = (tcp_header *)(ip + 1); |
123 | |
124 | if (ip_len < sizeof(tcp_header)) { |
125 | return; |
126 | } |
127 | |
128 | /* Set csum to 0 */ |
129 | stw_he_p(&tcp->th_sum, 0); |
130 | |
131 | csum = net_checksum_tcpudp(ip_len, ip->ip_p, |
132 | (uint8_t *)&ip->ip_src, |
133 | (uint8_t *)tcp); |
134 | |
135 | /* Store computed csum */ |
136 | stw_be_p(&tcp->th_sum, csum); |
137 | |
138 | break; |
139 | } |
140 | case IP_PROTO_UDP: |
141 | { |
142 | uint16_t csum; |
143 | udp_header *udp = (udp_header *)(ip + 1); |
144 | |
145 | if (ip_len < sizeof(udp_header)) { |
146 | return; |
147 | } |
148 | |
149 | /* Set csum to 0 */ |
150 | stw_he_p(&udp->uh_sum, 0); |
151 | |
152 | csum = net_checksum_tcpudp(ip_len, ip->ip_p, |
153 | (uint8_t *)&ip->ip_src, |
154 | (uint8_t *)udp); |
155 | |
156 | /* Store computed csum */ |
157 | stw_be_p(&udp->uh_sum, csum); |
158 | |
159 | break; |
160 | } |
161 | default: |
162 | /* Can't handle any other protocol */ |
163 | break; |
164 | } |
165 | } |
166 | |
167 | uint32_t |
168 | net_checksum_add_iov(const struct iovec *iov, const unsigned int iov_cnt, |
169 | uint32_t iov_off, uint32_t size, uint32_t csum_offset) |
170 | { |
171 | size_t iovec_off, buf_off; |
172 | unsigned int i; |
173 | uint32_t res = 0; |
174 | |
175 | iovec_off = 0; |
176 | buf_off = 0; |
177 | for (i = 0; i < iov_cnt && size; i++) { |
178 | if (iov_off < (iovec_off + iov[i].iov_len)) { |
179 | size_t len = MIN((iovec_off + iov[i].iov_len) - iov_off , size); |
180 | void *chunk_buf = iov[i].iov_base + (iov_off - iovec_off); |
181 | |
182 | res += net_checksum_add_cont(len, chunk_buf, csum_offset); |
183 | csum_offset += len; |
184 | |
185 | buf_off += len; |
186 | iov_off += len; |
187 | size -= len; |
188 | } |
189 | iovec_off += iov[i].iov_len; |
190 | } |
191 | return res; |
192 | } |
193 | |