修改4g拨号为QMI,需要在系统里后台执行quectel-CM

hc

2024-10-22 8ac6c7a54ed1b98d142dce24b11c6de6a1e239a5

 kernel/net/ipv6/esp6.c
..
..
@@ -1,18 +1,6 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
1
2
 /*
2
3
  * Copyright (C)2002 USAGI/WIDE Project
3
- *
4
- * This program is free software; you can redistribute it and/or modify
5
- * it under the terms of the GNU General Public License as published by
6
- * the Free Software Foundation; either version 2 of the License, or
7
- * (at your option) any later version.
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
4
  *
17
5
  * Authors
18
6
  *
..
..
@@ -38,11 +26,16 @@
38
26
 #include <linux/random.h>
39
27
 #include <linux/slab.h>
40
28
 #include <linux/spinlock.h>
29
+#include <net/ip6_checksum.h>
41
30
 #include <net/ip6_route.h>
42
31
 #include <net/icmp.h>
43
32
 #include <net/ipv6.h>
44
33
 #include <net/protocol.h>
34
+#include <net/udp.h>
45
35
 #include <linux/icmpv6.h>
36
+#include <net/tcp.h>
37
+#include <net/espintcp.h>
38
+#include <net/inet6_hashtables.h>
46
39
 
47
40
 #include <linux/highmem.h>
48
41
 
..
..
@@ -51,9 +44,12 @@
51
44
 	void *tmp;
52
45
 };
53
46
 
54
-#define ESP_SKB_CB(__skb) ((struct esp_skb_cb *)&((__skb)->cb[0]))
47
+struct esp_output_extra {
48
+	__be32 seqhi;
49
+	u32 esphoff;
50
+};
55
51
 
56
-static u32 esp6_get_mtu(struct xfrm_state *x, int mtu);
52
+#define ESP_SKB_CB(__skb) ((struct esp_skb_cb *)&((__skb)->cb[0]))
57
53
 
58
54
 /*
59
55
  * Allocate an AEAD request structure with extra space for SG and IV.
..
..
@@ -86,9 +82,9 @@
86
82
 	return kmalloc(len, GFP_ATOMIC);
87
83
 }
88
84
 
89
-static inline __be32 *esp_tmp_seqhi(void *tmp)
85
+static inline void *esp_tmp_extra(void *tmp)
90
86
 {
91
-	return PTR_ALIGN((__be32 *)tmp, __alignof__(__be32));
87
+	return PTR_ALIGN(tmp, __alignof__(struct esp_output_extra));
92
88
 }
93
89
 
94
90
 static inline u8 *esp_tmp_iv(struct crypto_aead *aead, void *tmp, int seqhilen)
..
..
@@ -118,16 +114,17 @@
118
114
 
119
115
 static void esp_ssg_unref(struct xfrm_state *x, void *tmp)
120
116
 {
117
+	struct esp_output_extra *extra = esp_tmp_extra(tmp);
121
118
 	struct crypto_aead *aead = x->data;
122
-	int seqhilen = 0;
119
+	int extralen = 0;
123
120
 	u8 *iv;
124
121
 	struct aead_request *req;
125
122
 	struct scatterlist *sg;
126
123
 
127
124
 	if (x->props.flags & XFRM_STATE_ESN)
128
-		seqhilen += sizeof(__be32);
125
+		extralen += sizeof(*extra);
129
126
 
130
-	iv = esp_tmp_iv(aead, tmp, seqhilen);
127
+	iv = esp_tmp_iv(aead, tmp, extralen);
131
128
 	req = esp_tmp_req(aead, iv);
132
129
 
133
130
 	/* Unref skb_frag_pages in the src scatterlist if necessary.
..
..
@@ -138,6 +135,149 @@
138
135
 			put_page(sg_page(sg));
139
136
 }
140
137
 
138
+#ifdef CONFIG_INET6_ESPINTCP
139
+struct esp_tcp_sk {
140
+	struct sock *sk;
141
+	struct rcu_head rcu;
142
+};
143
+
144
+static void esp_free_tcp_sk(struct rcu_head *head)
145
+{
146
+	struct esp_tcp_sk *esk = container_of(head, struct esp_tcp_sk, rcu);
147
+
148
+	sock_put(esk->sk);
149
+	kfree(esk);
150
+}
151
+
152
+static struct sock *esp6_find_tcp_sk(struct xfrm_state *x)
153
+{
154
+	struct xfrm_encap_tmpl *encap = x->encap;
155
+	struct esp_tcp_sk *esk;
156
+	__be16 sport, dport;
157
+	struct sock *nsk;
158
+	struct sock *sk;
159
+
160
+	sk = rcu_dereference(x->encap_sk);
161
+	if (sk && sk->sk_state == TCP_ESTABLISHED)
162
+		return sk;
163
+
164
+	spin_lock_bh(&x->lock);
165
+	sport = encap->encap_sport;
166
+	dport = encap->encap_dport;
167
+	nsk = rcu_dereference_protected(x->encap_sk,
168
+					lockdep_is_held(&x->lock));
169
+	if (sk && sk == nsk) {
170
+		esk = kmalloc(sizeof(*esk), GFP_ATOMIC);
171
+		if (!esk) {
172
+			spin_unlock_bh(&x->lock);
173
+			return ERR_PTR(-ENOMEM);
174
+		}
175
+		RCU_INIT_POINTER(x->encap_sk, NULL);
176
+		esk->sk = sk;
177
+		call_rcu(&esk->rcu, esp_free_tcp_sk);
178
+	}
179
+	spin_unlock_bh(&x->lock);
180
+
181
+	sk = __inet6_lookup_established(xs_net(x), &tcp_hashinfo, &x->id.daddr.in6,
182
+					dport, &x->props.saddr.in6, ntohs(sport), 0, 0);
183
+	if (!sk)
184
+		return ERR_PTR(-ENOENT);
185
+
186
+	if (!tcp_is_ulp_esp(sk)) {
187
+		sock_put(sk);
188
+		return ERR_PTR(-EINVAL);
189
+	}
190
+
191
+	spin_lock_bh(&x->lock);
192
+	nsk = rcu_dereference_protected(x->encap_sk,
193
+					lockdep_is_held(&x->lock));
194
+	if (encap->encap_sport != sport ||
195
+	    encap->encap_dport != dport) {
196
+		sock_put(sk);
197
+		sk = nsk ?: ERR_PTR(-EREMCHG);
198
+	} else if (sk == nsk) {
199
+		sock_put(sk);
200
+	} else {
201
+		rcu_assign_pointer(x->encap_sk, sk);
202
+	}
203
+	spin_unlock_bh(&x->lock);
204
+
205
+	return sk;
206
+}
207
+
208
+static int esp_output_tcp_finish(struct xfrm_state *x, struct sk_buff *skb)
209
+{
210
+	struct sock *sk;
211
+	int err;
212
+
213
+	rcu_read_lock();
214
+
215
+	sk = esp6_find_tcp_sk(x);
216
+	err = PTR_ERR_OR_ZERO(sk);
217
+	if (err)
218
+		goto out;
219
+
220
+	bh_lock_sock(sk);
221
+	if (sock_owned_by_user(sk))
222
+		err = espintcp_queue_out(sk, skb);
223
+	else
224
+		err = espintcp_push_skb(sk, skb);
225
+	bh_unlock_sock(sk);
226
+
227
+out:
228
+	rcu_read_unlock();
229
+	return err;
230
+}
231
+
232
+static int esp_output_tcp_encap_cb(struct net *net, struct sock *sk,
233
+				   struct sk_buff *skb)
234
+{
235
+	struct dst_entry *dst = skb_dst(skb);
236
+	struct xfrm_state *x = dst->xfrm;
237
+
238
+	return esp_output_tcp_finish(x, skb);
239
+}
240
+
241
+static int esp_output_tail_tcp(struct xfrm_state *x, struct sk_buff *skb)
242
+{
243
+	int err;
244
+
245
+	local_bh_disable();
246
+	err = xfrm_trans_queue_net(xs_net(x), skb, esp_output_tcp_encap_cb);
247
+	local_bh_enable();
248
+
249
+	/* EINPROGRESS just happens to do the right thing.  It
250
+	 * actually means that the skb has been consumed and
251
+	 * isn't coming back.
252
+	 */
253
+	return err ?: -EINPROGRESS;
254
+}
255
+#else
256
+static int esp_output_tail_tcp(struct xfrm_state *x, struct sk_buff *skb)
257
+{
258
+	kfree_skb(skb);
259
+
260
+	return -EOPNOTSUPP;
261
+}
262
+#endif
263
+
264
+static void esp_output_encap_csum(struct sk_buff *skb)
265
+{
266
+	/* UDP encap with IPv6 requires a valid checksum */
267
+	if (*skb_mac_header(skb) == IPPROTO_UDP) {
268
+		struct udphdr *uh = udp_hdr(skb);
269
+		struct ipv6hdr *ip6h = ipv6_hdr(skb);
270
+		int len = ntohs(uh->len);
271
+		unsigned int offset = skb_transport_offset(skb);
272
+		__wsum csum = skb_checksum(skb, offset, skb->len - offset, 0);
273
+
274
+		uh->check = csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
275
+					    len, IPPROTO_UDP, csum);
276
+		if (uh->check == 0)
277
+			uh->check = CSUM_MANGLED_0;
278
+	}
279
+}
280
+
141
281
 static void esp_output_done(struct crypto_async_request *base, int err)
142
282
 {
143
283
 	struct sk_buff *skb = base->data;
..
..
@@ -145,14 +285,19 @@
145
285
 	void *tmp;
146
286
 	struct xfrm_state *x;
147
287
 
148
-	if (xo && (xo->flags & XFRM_DEV_RESUME))
149
-		x = skb->sp->xvec[skb->sp->len - 1];
150
-	else
288
+	if (xo && (xo->flags & XFRM_DEV_RESUME)) {
289
+		struct sec_path *sp = skb_sec_path(skb);
290
+
291
+		x = sp->xvec[sp->len - 1];
292
+	} else {
151
293
 		x = skb_dst(skb)->xfrm;
294
+	}
152
295
 
153
296
 	tmp = ESP_SKB_CB(skb)->tmp;
154
297
 	esp_ssg_unref(x, tmp);
155
298
 	kfree(tmp);
299
+
300
+	esp_output_encap_csum(skb);
156
301
 
157
302
 	if (xo && (xo->flags & XFRM_DEV_RESUME)) {
158
303
 		if (err) {
..
..
@@ -165,7 +310,11 @@
165
310
 		secpath_reset(skb);
166
311
 		xfrm_dev_resume(skb);
167
312
 	} else {
168
-		xfrm_output_resume(skb, err);
313
+		if (!err &&
314
+		    x->encap && x->encap->encap_type == TCP_ENCAP_ESPINTCP)
315
+			esp_output_tail_tcp(x, skb);
316
+		else
317
+			xfrm_output_resume(skb, err);
169
318
 	}
170
319
 }
171
320
 
..
..
@@ -174,7 +323,7 @@
174
323
 {
175
324
 	struct ip_esp_hdr *esph = (void *)(skb->data + offset);
176
325
 	void *tmp = ESP_SKB_CB(skb)->tmp;
177
-	__be32 *seqhi = esp_tmp_seqhi(tmp);
326
+	__be32 *seqhi = esp_tmp_extra(tmp);
178
327
 
179
328
 	esph->seq_no = esph->spi;
180
329
 	esph->spi = *seqhi;
..
..
@@ -182,27 +331,36 @@
182
331
 
183
332
 static void esp_output_restore_header(struct sk_buff *skb)
184
333
 {
185
-	esp_restore_header(skb, skb_transport_offset(skb) - sizeof(__be32));
334
+	void *tmp = ESP_SKB_CB(skb)->tmp;
335
+	struct esp_output_extra *extra = esp_tmp_extra(tmp);
336
+
337
+	esp_restore_header(skb, skb_transport_offset(skb) + extra->esphoff -
338
+				sizeof(__be32));
186
339
 }
187
340
 
188
341
 static struct ip_esp_hdr *esp_output_set_esn(struct sk_buff *skb,
189
342
 					     struct xfrm_state *x,
190
343
 					     struct ip_esp_hdr *esph,
191
-					     __be32 *seqhi)
344
+					     struct esp_output_extra *extra)
192
345
 {
193
346
 	/* For ESN we move the header forward by 4 bytes to
194
347
 	 * accomodate the high bits.  We will move it back after
195
348
 	 * encryption.
196
349
 	 */
197
350
 	if ((x->props.flags & XFRM_STATE_ESN)) {
351
+		__u32 seqhi;
198
352
 		struct xfrm_offload *xo = xfrm_offload(skb);
199
353
 
200
-		esph = (void *)(skb_transport_header(skb) - sizeof(__be32));
201
-		*seqhi = esph->spi;
202
354
 		if (xo)
203
-			esph->seq_no = htonl(xo->seq.hi);
355
+			seqhi = xo->seq.hi;
204
356
 		else
205
-			esph->seq_no = htonl(XFRM_SKB_CB(skb)->seq.output.hi);
357
+			seqhi = XFRM_SKB_CB(skb)->seq.output.hi;
358
+
359
+		extra->esphoff = (unsigned char *)esph -
360
+				 skb_transport_header(skb);
361
+		esph = (struct ip_esp_hdr *)((unsigned char *)esph - 4);
362
+		extra->seqhi = esph->spi;
363
+		esph->seq_no = htonl(seqhi);
206
364
 	}
207
365
 
208
366
 	esph->spi = x->id.spi;
..
..
@@ -218,29 +376,124 @@
218
376
 	esp_output_done(base, err);
219
377
 }
220
378
 
221
-static void esp_output_fill_trailer(u8 *tail, int tfclen, int plen, __u8 proto)
379
+static struct ip_esp_hdr *esp6_output_udp_encap(struct sk_buff *skb,
380
+					       int encap_type,
381
+					       struct esp_info *esp,
382
+					       __be16 sport,
383
+					       __be16 dport)
222
384
 {
223
-	/* Fill padding... */
224
-	if (tfclen) {
225
-		memset(tail, 0, tfclen);
226
-		tail += tfclen;
385
+	struct udphdr *uh;
386
+	__be32 *udpdata32;
387
+	unsigned int len;
388
+
389
+	len = skb->len + esp->tailen - skb_transport_offset(skb);
390
+	if (len > U16_MAX)
391
+		return ERR_PTR(-EMSGSIZE);
392
+
393
+	uh = (struct udphdr *)esp->esph;
394
+	uh->source = sport;
395
+	uh->dest = dport;
396
+	uh->len = htons(len);
397
+	uh->check = 0;
398
+
399
+	*skb_mac_header(skb) = IPPROTO_UDP;
400
+
401
+	if (encap_type == UDP_ENCAP_ESPINUDP_NON_IKE) {
402
+		udpdata32 = (__be32 *)(uh + 1);
403
+		udpdata32[0] = udpdata32[1] = 0;
404
+		return (struct ip_esp_hdr *)(udpdata32 + 2);
227
405
 	}
228
-	do {
229
-		int i;
230
-		for (i = 0; i < plen - 2; i++)
231
-			tail[i] = i + 1;
232
-	} while (0);
233
-	tail[plen - 2] = plen - 2;
234
-	tail[plen - 1] = proto;
406
+
407
+	return (struct ip_esp_hdr *)(uh + 1);
408
+}
409
+
410
+#ifdef CONFIG_INET6_ESPINTCP
411
+static struct ip_esp_hdr *esp6_output_tcp_encap(struct xfrm_state *x,
412
+						struct sk_buff *skb,
413
+						struct esp_info *esp)
414
+{
415
+	__be16 *lenp = (void *)esp->esph;
416
+	struct ip_esp_hdr *esph;
417
+	unsigned int len;
418
+	struct sock *sk;
419
+
420
+	len = skb->len + esp->tailen - skb_transport_offset(skb);
421
+	if (len > IP_MAX_MTU)
422
+		return ERR_PTR(-EMSGSIZE);
423
+
424
+	rcu_read_lock();
425
+	sk = esp6_find_tcp_sk(x);
426
+	rcu_read_unlock();
427
+
428
+	if (IS_ERR(sk))
429
+		return ERR_CAST(sk);
430
+
431
+	*lenp = htons(len);
432
+	esph = (struct ip_esp_hdr *)(lenp + 1);
433
+
434
+	return esph;
435
+}
436
+#else
437
+static struct ip_esp_hdr *esp6_output_tcp_encap(struct xfrm_state *x,
438
+						struct sk_buff *skb,
439
+						struct esp_info *esp)
440
+{
441
+	return ERR_PTR(-EOPNOTSUPP);
442
+}
443
+#endif
444
+
445
+static int esp6_output_encap(struct xfrm_state *x, struct sk_buff *skb,
446
+			    struct esp_info *esp)
447
+{
448
+	struct xfrm_encap_tmpl *encap = x->encap;
449
+	struct ip_esp_hdr *esph;
450
+	__be16 sport, dport;
451
+	int encap_type;
452
+
453
+	spin_lock_bh(&x->lock);
454
+	sport = encap->encap_sport;
455
+	dport = encap->encap_dport;
456
+	encap_type = encap->encap_type;
457
+	spin_unlock_bh(&x->lock);
458
+
459
+	switch (encap_type) {
460
+	default:
461
+	case UDP_ENCAP_ESPINUDP:
462
+	case UDP_ENCAP_ESPINUDP_NON_IKE:
463
+		esph = esp6_output_udp_encap(skb, encap_type, esp, sport, dport);
464
+		break;
465
+	case TCP_ENCAP_ESPINTCP:
466
+		esph = esp6_output_tcp_encap(x, skb, esp);
467
+		break;
468
+	}
469
+
470
+	if (IS_ERR(esph))
471
+		return PTR_ERR(esph);
472
+
473
+	esp->esph = esph;
474
+
475
+	return 0;
235
476
 }
236
477
 
237
478
 int esp6_output_head(struct xfrm_state *x, struct sk_buff *skb, struct esp_info *esp)
238
479
 {
239
480
 	u8 *tail;
240
481
 	int nfrags;
482
+	int esph_offset;
241
483
 	struct page *page;
242
484
 	struct sk_buff *trailer;
243
485
 	int tailen = esp->tailen;
486
+
487
+	if (x->encap) {
488
+		int err = esp6_output_encap(x, skb, esp);
489
+
490
+		if (err < 0)
491
+			return err;
492
+	}
493
+
494
+	if (ALIGN(tailen, L1_CACHE_BYTES) > PAGE_SIZE ||
495
+	    ALIGN(skb->data_len, L1_CACHE_BYTES) > PAGE_SIZE)
496
+		goto cow;
244
497
 
245
498
 	if (!skb_cloned(skb)) {
246
499
 		if (tailen <= skb_tailroom(skb)) {
..
..
@@ -296,10 +549,13 @@
296
549
 	}
297
550
 
298
551
 cow:
552
+	esph_offset = (unsigned char *)esp->esph - skb_transport_header(skb);
553
+
299
554
 	nfrags = skb_cow_data(skb, tailen, &trailer);
300
555
 	if (nfrags < 0)
301
556
 		goto out;
302
557
 	tail = skb_tail_pointer(trailer);
558
+	esp->esph = (struct ip_esp_hdr *)(skb_transport_header(skb) + esph_offset);
303
559
 
304
560
 skip_cow:
305
561
 	esp_output_fill_trailer(tail, esp->tfclen, esp->plen, esp->proto);
..
..
@@ -317,20 +573,20 @@
317
573
 	void *tmp;
318
574
 	int ivlen;
319
575
 	int assoclen;
320
-	int seqhilen;
321
-	__be32 *seqhi;
576
+	int extralen;
322
577
 	struct page *page;
323
578
 	struct ip_esp_hdr *esph;
324
579
 	struct aead_request *req;
325
580
 	struct crypto_aead *aead;
326
581
 	struct scatterlist *sg, *dsg;
582
+	struct esp_output_extra *extra;
327
583
 	int err = -ENOMEM;
328
584
 
329
585
 	assoclen = sizeof(struct ip_esp_hdr);
330
-	seqhilen = 0;
586
+	extralen = 0;
331
587
 
332
588
 	if (x->props.flags & XFRM_STATE_ESN) {
333
-		seqhilen += sizeof(__be32);
589
+		extralen += sizeof(*extra);
334
590
 		assoclen += sizeof(__be32);
335
591
 	}
336
592
 
..
..
@@ -338,12 +594,12 @@
338
594
 	alen = crypto_aead_authsize(aead);
339
595
 	ivlen = crypto_aead_ivsize(aead);
340
596
 
341
-	tmp = esp_alloc_tmp(aead, esp->nfrags + 2, seqhilen);
597
+	tmp = esp_alloc_tmp(aead, esp->nfrags + 2, extralen);
342
598
 	if (!tmp)
343
599
 		goto error;
344
600
 
345
-	seqhi = esp_tmp_seqhi(tmp);
346
-	iv = esp_tmp_iv(aead, tmp, seqhilen);
601
+	extra = esp_tmp_extra(tmp);
602
+	iv = esp_tmp_iv(aead, tmp, extralen);
347
603
 	req = esp_tmp_req(aead, iv);
348
604
 	sg = esp_req_sg(aead, req);
349
605
 
..
..
@@ -352,7 +608,8 @@
352
608
 	else
353
609
 		dsg = &sg[esp->nfrags];
354
610
 
355
-	esph = esp_output_set_esn(skb, x, ip_esp_hdr(skb), seqhi);
611
+	esph = esp_output_set_esn(skb, x, esp->esph, extra);
612
+	esp->esph = esph;
356
613
 
357
614
 	sg_init_table(sg, esp->nfrags);
358
615
 	err = skb_to_sgvec(skb, sg,
..
..
@@ -416,10 +673,14 @@
416
673
 	case 0:
417
674
 		if ((x->props.flags & XFRM_STATE_ESN))
418
675
 			esp_output_restore_header(skb);
676
+		esp_output_encap_csum(skb);
419
677
 	}
420
678
 
421
679
 	if (sg != dsg)
422
680
 		esp_ssg_unref(x, tmp);
681
+
682
+	if (!err && x->encap && x->encap->encap_type == TCP_ENCAP_ESPINTCP)
683
+		err = esp_output_tail_tcp(x, skb);
423
684
 
424
685
 error_free:
425
686
 	kfree(tmp);
..
..
@@ -451,7 +712,7 @@
451
712
 		struct xfrm_dst *dst = (struct xfrm_dst *)skb_dst(skb);
452
713
 		u32 padto;
453
714
 
454
-		padto = min(x->tfcpad, esp6_get_mtu(x, dst->child_mtu_cached));
715
+		padto = min(x->tfcpad, xfrm_state_mtu(x, dst->child_mtu_cached));
455
716
 		if (skb->len < padto)
456
717
 			esp.tfclen = padto - skb->len;
457
718
 	}
..
..
@@ -460,11 +721,13 @@
460
721
 	esp.plen = esp.clen - skb->len - esp.tfclen;
461
722
 	esp.tailen = esp.tfclen + esp.plen + alen;
462
723
 
724
+	esp.esph = ip_esp_hdr(skb);
725
+
463
726
 	esp.nfrags = esp6_output_head(x, skb, &esp);
464
727
 	if (esp.nfrags < 0)
465
728
 		return esp.nfrags;
466
729
 
467
-	esph = ip_esp_hdr(skb);
730
+	esph = esp.esph;
468
731
 	esph->spi = x->id.spi;
469
732
 
470
733
 	esph->seq_no = htonl(XFRM_SKB_CB(skb)->seq.output.low);
..
..
@@ -539,6 +802,72 @@
539
802
 	if (unlikely(err < 0))
540
803
 		goto out;
541
804
 
805
+	if (x->encap) {
806
+		const struct ipv6hdr *ip6h = ipv6_hdr(skb);
807
+		int offset = skb_network_offset(skb) + sizeof(*ip6h);
808
+		struct xfrm_encap_tmpl *encap = x->encap;
809
+		u8 nexthdr = ip6h->nexthdr;
810
+		__be16 frag_off, source;
811
+		struct udphdr *uh;
812
+		struct tcphdr *th;
813
+
814
+		offset = ipv6_skip_exthdr(skb, offset, &nexthdr, &frag_off);
815
+		if (offset == -1) {
816
+			err = -EINVAL;
817
+			goto out;
818
+		}
819
+
820
+		uh = (void *)(skb->data + offset);
821
+		th = (void *)(skb->data + offset);
822
+		hdr_len += offset;
823
+
824
+		switch (x->encap->encap_type) {
825
+		case TCP_ENCAP_ESPINTCP:
826
+			source = th->source;
827
+			break;
828
+		case UDP_ENCAP_ESPINUDP:
829
+		case UDP_ENCAP_ESPINUDP_NON_IKE:
830
+			source = uh->source;
831
+			break;
832
+		default:
833
+			WARN_ON_ONCE(1);
834
+			err = -EINVAL;
835
+			goto out;
836
+		}
837
+
838
+		/*
839
+		 * 1) if the NAT-T peer's IP or port changed then
840
+		 *    advertize the change to the keying daemon.
841
+		 *    This is an inbound SA, so just compare
842
+		 *    SRC ports.
843
+		 */
844
+		if (!ipv6_addr_equal(&ip6h->saddr, &x->props.saddr.in6) ||
845
+		    source != encap->encap_sport) {
846
+			xfrm_address_t ipaddr;
847
+
848
+			memcpy(&ipaddr.a6, &ip6h->saddr.s6_addr, sizeof(ipaddr.a6));
849
+			km_new_mapping(x, &ipaddr, source);
850
+
851
+			/* XXX: perhaps add an extra
852
+			 * policy check here, to see
853
+			 * if we should allow or
854
+			 * reject a packet from a
855
+			 * different source
856
+			 * address/port.
857
+			 */
858
+		}
859
+
860
+		/*
861
+		 * 2) ignore UDP/TCP checksums in case
862
+		 *    of NAT-T in Transport Mode, or
863
+		 *    perform other post-processing fixes
864
+		 *    as per draft-ietf-ipsec-udp-encaps-06,
865
+		 *    section 3.1.2
866
+		 */
867
+		if (x->props.mode == XFRM_MODE_TRANSPORT)
868
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
869
+	}
870
+
542
871
 	skb_postpull_rcsum(skb, skb_network_header(skb),
543
872
 			   skb_network_header_len(skb));
544
873
 	skb_pull_rcsum(skb, hlen);
..
..
@@ -596,12 +925,11 @@
596
925
 
597
926
 static int esp6_input(struct xfrm_state *x, struct sk_buff *skb)
598
927
 {
599
-	struct ip_esp_hdr *esph;
600
928
 	struct crypto_aead *aead = x->data;
601
929
 	struct aead_request *req;
602
930
 	struct sk_buff *trailer;
603
931
 	int ivlen = crypto_aead_ivsize(aead);
604
-	int elen = skb->len - sizeof(*esph) - ivlen;
932
+	int elen = skb->len - sizeof(struct ip_esp_hdr) - ivlen;
605
933
 	int nfrags;
606
934
 	int assoclen;
607
935
 	int seqhilen;
..
..
@@ -611,7 +939,7 @@
611
939
 	u8 *iv;
612
940
 	struct scatterlist *sg;
613
941
 
614
-	if (!pskb_may_pull(skb, sizeof(*esph) + ivlen)) {
942
+	if (!pskb_may_pull(skb, sizeof(struct ip_esp_hdr) + ivlen)) {
615
943
 		ret = -EINVAL;
616
944
 		goto out;
617
945
 	}
..
..
@@ -621,7 +949,7 @@
621
949
 		goto out;
622
950
 	}
623
951
 
624
-	assoclen = sizeof(*esph);
952
+	assoclen = sizeof(struct ip_esp_hdr);
625
953
 	seqhilen = 0;
626
954
 
627
955
 	if (x->props.flags & XFRM_STATE_ESN) {
..
..
@@ -655,7 +983,7 @@
655
983
 		goto out;
656
984
 
657
985
 	ESP_SKB_CB(skb)->tmp = tmp;
658
-	seqhi = esp_tmp_seqhi(tmp);
986
+	seqhi = esp_tmp_extra(tmp);
659
987
 	iv = esp_tmp_iv(aead, tmp, seqhilen);
660
988
 	req = esp_tmp_req(aead, iv);
661
989
 	sg = esp_req_sg(aead, req);
..
..
@@ -690,21 +1018,6 @@
690
1018
 
691
1019
 out:
692
1020
 	return ret;
693
-}
694
-
695
-static u32 esp6_get_mtu(struct xfrm_state *x, int mtu)
696
-{
697
-	struct crypto_aead *aead = x->data;
698
-	u32 blksize = ALIGN(crypto_aead_blocksize(aead), 4);
699
-	unsigned int net_adj;
700
-
701
-	if (x->props.mode != XFRM_MODE_TUNNEL)
702
-		net_adj = sizeof(struct ipv6hdr);
703
-	else
704
-		net_adj = 0;
705
-
706
-	return ((mtu - x->props.header_len - crypto_aead_authsize(aead) -
707
-		 net_adj) & ~(blksize - 1)) + net_adj - 2;
708
1021
 }
709
1022
 
710
1023
 static int esp6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
..
..
@@ -874,9 +1187,6 @@
874
1187
 	u32 align;
875
1188
 	int err;
876
1189
 
877
-	if (x->encap)
878
-		return -EINVAL;
879
-
880
1190
 	x->data = NULL;
881
1191
 
882
1192
 	if (x->aead)
..
..
@@ -905,6 +1215,30 @@
905
1215
 		break;
906
1216
 	}
907
1217
 
1218
+	if (x->encap) {
1219
+		struct xfrm_encap_tmpl *encap = x->encap;
1220
+
1221
+		switch (encap->encap_type) {
1222
+		default:
1223
+			err = -EINVAL;
1224
+			goto error;
1225
+		case UDP_ENCAP_ESPINUDP:
1226
+			x->props.header_len += sizeof(struct udphdr);
1227
+			break;
1228
+		case UDP_ENCAP_ESPINUDP_NON_IKE:
1229
+			x->props.header_len += sizeof(struct udphdr) + 2 * sizeof(u32);
1230
+			break;
1231
+#ifdef CONFIG_INET6_ESPINTCP
1232
+		case TCP_ENCAP_ESPINTCP:
1233
+			/* only the length field, TCP encap is done by
1234
+			 * the socket
1235
+			 */
1236
+			x->props.header_len += 2;
1237
+			break;
1238
+#endif
1239
+		}
1240
+	}
1241
+
908
1242
 	align = ALIGN(crypto_aead_blocksize(aead), 4);
909
1243
 	x->props.trailer_len = align + 1 + crypto_aead_authsize(aead);
910
1244
 
..
..
@@ -924,7 +1258,6 @@
924
1258
 	.flags		= XFRM_TYPE_REPLAY_PROT,
925
1259
 	.init_state	= esp6_init_state,
926
1260
 	.destructor	= esp6_destroy,
927
-	.get_mtu	= esp6_get_mtu,
928
1261
 	.input		= esp6_input,
929
1262
 	.output		= esp6_output,
930
1263
 	.hdr_offset	= xfrm6_find_1stfragopt,
..
..
@@ -932,6 +1265,7 @@
932
1265
 
933
1266
 static struct xfrm6_protocol esp6_protocol = {
934
1267
 	.handler	=	xfrm6_rcv,
1268
+	.input_handler	=	xfrm_input,
935
1269
 	.cb_handler	=	esp6_rcv_cb,
936
1270
 	.err_handler	=	esp6_err,
937
1271
 	.priority	=	0,
..
..
@@ -956,8 +1290,7 @@
956
1290
 {
957
1291
 	if (xfrm6_protocol_deregister(&esp6_protocol, IPPROTO_ESP) < 0)
958
1292
 		pr_info("%s: can't remove protocol\n", __func__);
959
-	if (xfrm_unregister_type(&esp6_type, AF_INET6) < 0)
960
-		pr_info("%s: can't remove xfrm type\n", __func__);
1293
+	xfrm_unregister_type(&esp6_type, AF_INET6);
961
1294
 }
962
1295
 
963
1296
 module_init(esp6_init);