5113f6f70
 김현기
kernel add
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
|
/*
* Copyright (C) 2012-2016 Texas Instruments Incorporated
* Authors: Sandeep Nair <sandeep_n@ti.com>
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License as
* published by the Free Software Foundation version 2.
*
* This program is distributed "as is" WITHOUT ANY WARRANTY of any
* kind, whether express or implied; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/io.h>
#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/types.h>
#include <linux/module.h>
#include <linux/delay.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/of_address.h>
#include <linux/interrupt.h>
#include <linux/byteorder/generic.h>
#include <linux/platform_device.h>
#include <linux/errqueue.h>
#include <net/ipv6.h>
#include <net/xfrm.h>
#include "netcp.h"
#define NET_SA_CHAN_PRFX "satx"
#define PASAHO_SA_SHORT_INFO 1
/* Format the entire short info command */
#define PASAHO_SINFO_FORMAT_CMD(offset, len) \
(((offset) << 16) | (len) | (PASAHO_SA_SHORT_INFO << 29))
#define SA_RXHOOK_ORDER 30
#define ETH_TYPE_OFFSET 12
#define ETH_HDR_LEN 14
/* Version 0 of sa_rx_hook definitions (for NETCP 1.0 on K2HK) */
#define BITS(x) (BIT(x) - 1)
#define BITMASK(n, s) (BITS(n) << (s))
#define SA_IS_IPSEC_ESP_MASK BIT(25)
#define SA_IS_FLAG_FRAG_MASK BIT(3)
#define SA_NUM_IP_HEADERS_SHIFT 8
#define SA_NUM_IP_HEADERS_MASK BITMASK(3, SA_NUM_IP_HEADERS_SHIFT)
#define SA_NUM_VLAN_HEADERS_SHIFT 14
#define SA_NUM_VLAN_HEADERS_MASK BITMASK(2, SA_NUM_VLAN_HEADERS_SHIFT)
#define SA_TXHOOK_ORDER 30
#define SA_IPSECMAN_MAJIC 0xdeadfeed
/* Version 1 of sa_rx_hook definitions (for NSS 1.0 on K2L and K2E) */
#define SA_IS_IPSEC_ESP_MASK_VER1 BIT(8)
#define SA_IS_FLAG_FRAG_MASK_VER1 BIT(19)
#define SA_NUM_IP_HEADERS_SHIFT_VER1 0
#define SA_NUM_IP_HEADERS_MASK_VER1 BITMASK(3, SA_NUM_IP_HEADERS_SHIFT_VER1)
#define SA_NUM_VLAN_HEADERS_SHIFT_VER1 6
#define SA_NUM_VLAN_HEADERS_MASK_VER1 \
BITMASK(2, SA_NUM_VLAN_HEADERS_SHIFT_VER1)
#define FLOW_ID_SHIFT 16
#define SWINFO2_MASK 0xff000000
struct sa_device {
struct device *dev;
struct netcp_device *netcp_device;
struct device_node *node;
u32 tx_queue_id;
u32 multi_if;
};
struct sa_intf {
struct net_device *net_device;
const char *tx_chan_name;
struct netcp_tx_pipe tx_pipe;
u32 natt_port;
u32 netcp_ver; /* 0: NETCP 1.0, 1: NSS 1.0 */
};
struct ipsecmgr_mod_sa_ctx_info {
atomic_t refcnt;
int len;
int majic;
u32 word0;
u32 word1;
u16 flow_id;
};
static inline void calc_ipv6_esp_info(const struct sk_buff *skb,
u16 *p_offset, u16 *p_len)
{
const struct ipv6hdr *ipv6h = ipv6_hdr(skb);
/* type of the 1st exthdr */
WARN_ON(ipv6_hdr(skb)->nexthdr != NEXTHDR_ESP);
*p_offset = (u16)(skb_network_header(skb) -
skb->data) + skb_network_header_len(skb);
/* payload length */
*p_len = ntohs(ipv6h->payload_len);
}
static int sa_tx_hook(int order, void *data, struct netcp_packet *p_info)
{
struct sa_intf *sa_intf = data;
u16 offset, len, ihl;
u32 *psdata, *swinfo;
const struct iphdr *iph;
struct ipsecmgr_mod_sa_ctx_info *ctx_info =
(struct ipsecmgr_mod_sa_ctx_info *)p_info->skb->sp;
if ((!ctx_info) || (ctx_info->majic != SA_IPSECMAN_MAJIC))
return 0;
iph = ip_hdr(p_info->skb);
if (iph->version == IPVERSION) {
ihl = iph->ihl * 4;
if (iph->protocol == IPPROTO_UDP) {
/* UDP encapsulation for IPSec NAT-T */
offset = (ulong)(skb_network_header(p_info->skb) -
p_info->skb->data) + ihl +
sizeof(struct udphdr);
len = ntohs(iph->tot_len) - ihl - sizeof(struct udphdr);
} else if (iph->protocol == IPPROTO_ESP) {
offset = (ulong)(skb_network_header(p_info->skb) -
p_info->skb->data) + ihl;
len = ntohs(iph->tot_len) - ihl;
} else {
return 0;
}
} else if (iph->version == 6) {
if ((ipv6_hdr(p_info->skb)->nexthdr) == IPPROTO_ESP)
/* need to calculate offset to where ESP header
* starts and total length of the payload
*/
calc_ipv6_esp_info(p_info->skb, &offset, &len);
else
return 0;
} else {
return 0;
}
psdata = netcp_push_psdata(p_info, (2 * sizeof(u32)));
if (!psdata)
return -ENOMEM;
psdata[0] = PASAHO_SINFO_FORMAT_CMD(offset, len);
psdata[1] = 0;
swinfo = &p_info->epib[1];
swinfo[0] = ctx_info->word0 | BIT(30);
swinfo[1] = ctx_info->word1;
swinfo[2] &= SWINFO2_MASK;
swinfo[2] |= (ctx_info->flow_id) << FLOW_ID_SHIFT |
p_info->tx_pipe->dma_queue_id;
p_info->tx_pipe = &sa_intf->tx_pipe;
ctx_info->len = 0;
secpath_reset(p_info->skb);
return 0;
}
static int sa_rx_hook(int order, void *data, struct netcp_packet *p_info)
{
struct sk_buff *skb = p_info->skb;
struct sa_intf *sa = data;
/* See if packet is a fragment, if it is not then return out.
* This is word 3, bit 3 of metadata psdata
*/
if (!(p_info->psdata[3] & SA_IS_FLAG_FRAG_MASK))
return 0;
/* Make sure there is just one IP header. If 2 ipheaders then
* we passed IPSEC; so return out.
*/
if (((p_info->psdata[3] & SA_NUM_IP_HEADERS_MASK) >>
SA_NUM_IP_HEADERS_SHIFT) != 1)
return 0;
/* Check to see if this is an ESP packet.
* ESP packet indication is in word 3, bit 25 of psdata
*/
if (p_info->psdata[3] & SA_IS_IPSEC_ESP_MASK) {
/* Packet is ESP, a fragment and does not have 2
* ipheaders, so set ignore_df flag
*/
skb->ignore_df = 1;
return 0;
}
/* If we have a NAT-T port set, check for NAT-T packet */
if (sa->natt_port) {
struct iphdr *ip_hdr;
u16 e_type;
int vlan;
/* Count how many VLAN headers we have */
vlan = ((p_info->psdata[3] & SA_NUM_VLAN_HEADERS_MASK) >>
SA_NUM_VLAN_HEADERS_SHIFT);
/* Return out for IPv6, nothing to be done.
* Ethertype follows the MAC and optional VLAN tags.
*/
e_type = *((u16 *)skb->data +
((ETH_TYPE_OFFSET / 2) + (vlan << 1)));
if (ntohs(e_type) == ETH_P_IPV6)
return 0;
/* Skip over Eth header to IP header.
* Eth header is 14 bytes + (4 bytes per VLAN header).
*/
ip_hdr = (struct iphdr *)(skb->data +
(ETH_HDR_LEN + (vlan << 2)));
/* Check if the protocol is UDP */
if (ip_hdr->protocol == IPPROTO_UDP) {
/* Get the UDP header.
* It follows the IP header + optional words;
* calculate offset with IHL
*/
struct udphdr *udp_hdr;
udp_hdr = (struct udphdr *)
((u_char *)ip_hdr + (ip_hdr->ihl << 2));
/* Check if the source or destination ports ==
* NAT-T port. If so, it is a NAT-T packet;
* set ignore_df flag
*/
if (ntohs(udp_hdr->dest) == sa->natt_port ||
ntohs(udp_hdr->source) == sa->natt_port)
skb->ignore_df = 1;
}
}
return 0;
}
static int sa_rx_hook_ver1(int order, void *data, struct netcp_packet *p_info)
{
struct sk_buff *skb = p_info->skb;
struct sa_intf *sa = data;
/* See if packet is a fragment, if it is not then return out.
* This is word 0, bit 19 of metadata psdata
*/
if (!(p_info->psdata[0] & SA_IS_FLAG_FRAG_MASK_VER1))
return 0;
/* Make sure there is just one IP header. If 2 ipheaders then
* we passed IPSEC; so return out.
*/
if (((p_info->psdata[4] & SA_NUM_IP_HEADERS_MASK_VER1) >>
SA_NUM_IP_HEADERS_SHIFT_VER1) != 1)
return 0;
/* Check to see if this is an ESP packet.
* ESP packet indication is in word 3, bit 8 of psdata
*/
if (p_info->psdata[3] & SA_IS_IPSEC_ESP_MASK_VER1) {
/* Packet is ESP, a fragment and does not have 2
* ipheaders, so set ignore_df flag
*/
skb->ignore_df = 1;
return 0;
}
/* If we have a NAT-T port set, check for NAT-T packet */
if (sa->natt_port) {
struct iphdr *ip_hdr;
u16 e_type;
int vlan;
/* Count how many VLAN headers we have */
vlan = ((p_info->psdata[4] & SA_NUM_VLAN_HEADERS_MASK_VER1) >>
SA_NUM_VLAN_HEADERS_SHIFT_VER1);
/* Return out for IPv6, nothing to be done.
* Ethertype follows the MAC and optional VLAN tags.
*/
e_type = *((u16 *)skb->data +
((ETH_TYPE_OFFSET / 2) + (vlan << 1)));
if (ntohs(e_type) == ETH_P_IPV6)
return 0;
/* Skip over Eth header to IP header.
* Eth header is 14 bytes + (4 bytes per VLAN header).
*/
ip_hdr = (struct iphdr *)(skb->data +
(ETH_HDR_LEN + (vlan << 2)));
/* Check if the protocol is UDP */
if (ip_hdr->protocol == IPPROTO_UDP) {
/* Get the UDP header.
* It follows the IP header; calculate with IHL
*/
struct udphdr *udp_hdr;
udp_hdr = (struct udphdr *)
((u_char *)ip_hdr + (ip_hdr->ihl << 2));
/* check if the source or destination ports ==
* NAT-T port. If so, it is a NAT-T packet;
* set ignore_df flag
*/
if (ntohs(udp_hdr->dest) == sa->natt_port ||
ntohs(udp_hdr->source) == sa->natt_port)
skb->ignore_df = 1;
}
}
return 0;
}
static int sa_close(void *intf_priv, struct net_device *ndev)
{
struct sa_intf *sa_intf = (struct sa_intf *)intf_priv;
struct netcp_intf *netcp_intf = netdev_priv(ndev);
netcp_unregister_txhook(netcp_intf, SA_TXHOOK_ORDER, sa_tx_hook,
sa_intf);
if (sa_intf->netcp_ver == 1)
netcp_unregister_rxhook(netcp_intf, SA_RXHOOK_ORDER,
sa_rx_hook_ver1, sa_intf);
else
netcp_unregister_rxhook(netcp_intf, SA_RXHOOK_ORDER,
sa_rx_hook, sa_intf);
netcp_txpipe_close(&sa_intf->tx_pipe);
return 0;
}
static int sa_open(void *intf_priv, struct net_device *ndev)
{
struct sa_intf *sa_intf = (struct sa_intf *)intf_priv;
struct netcp_intf *netcp_intf = netdev_priv(ndev);
int ret;
/* Open the SA IPSec data transmit channel */
ret = netcp_txpipe_open(&sa_intf->tx_pipe);
if (ret)
return ret;
netcp_register_txhook(netcp_intf, SA_TXHOOK_ORDER, sa_tx_hook, sa_intf);
if (sa_intf->netcp_ver == 1)
netcp_register_rxhook(netcp_intf, SA_RXHOOK_ORDER,
sa_rx_hook_ver1, sa_intf);
else
netcp_register_rxhook(netcp_intf, SA_RXHOOK_ORDER,
sa_rx_hook, sa_intf);
return 0;
}
static int sa_attach(void *inst_priv, struct net_device *ndev,
struct device_node *node, void **intf_priv)
{
struct sa_device *sa_dev = inst_priv;
struct sa_intf *sa_intf;
int ret = 0;
sa_intf = devm_kzalloc(sa_dev->dev, sizeof(struct sa_intf), GFP_KERNEL);
if (!sa_intf)
return -ENOMEM;
ret = of_property_read_string(node, "ti,tx-channel",
&sa_intf->tx_chan_name);
if (ret < 0) {
dev_err(sa_dev->dev, "missing \"ti,tx-channel\" parameter
");
return -EINVAL;
}
sa_intf->net_device = ndev;
*intf_priv = sa_intf;
netcp_txpipe_init(&sa_intf->tx_pipe, sa_dev->netcp_device,
sa_intf->tx_chan_name, sa_dev->tx_queue_id);
dev_dbg(sa_dev->dev, "keystone-sa attached for %s
", node->name);
ret = of_property_read_u32(sa_dev->node, "natt-port",
&sa_intf->natt_port);
if (ret < 0)
sa_intf->natt_port = 0;
ret = of_property_read_u32(sa_dev->node, "netcp_ver",
&sa_intf->netcp_ver);
if (ret < 0)
sa_intf->netcp_ver = 0;
return 0;
}
static int sa_release(void *intf_priv)
{
struct sa_intf *sa_intf = intf_priv;
kfree(sa_intf);
return 0;
}
static int sa_remove(struct netcp_device *netcp_device, void *inst_priv)
{
struct sa_device *sa_dev = inst_priv;
kfree(sa_dev);
return 0;
}
static int sa_probe(struct netcp_device *netcp_device,
struct device *dev,
struct device_node *node,
void **inst_priv)
{
struct sa_device *sa_dev;
int ret = 0;
if (!node) {
dev_err(dev, "device tree info unavailable
");
return -ENODEV;
}
sa_dev = devm_kzalloc(dev, sizeof(struct sa_device), GFP_KERNEL);
if (!sa_dev)
return -ENOMEM;
*inst_priv = sa_dev;
sa_dev->dev = dev;
sa_dev->netcp_device = netcp_device;
sa_dev->node = node;
if (of_find_property(node, "multi-interface", NULL))
sa_dev->multi_if = 1;
ret = of_property_read_u32(node, "tx-submit-queue",
&sa_dev->tx_queue_id);
if (ret < 0) {
dev_err(dev, "missing tx-submit-queue parameter, err %d
",
ret);
return -EINVAL;
}
dev_dbg(dev, "tx-submit-queue %u
", sa_dev->tx_queue_id);
return 0;
}
static struct netcp_module sa_module = {
.name = "netcp-sa",
.owner = THIS_MODULE,
.probe = sa_probe,
.open = sa_open,
.close = sa_close,
.remove = sa_remove,
.attach = sa_attach,
.release = sa_release,
};
static int __init keystone_sa_init(void)
{
return netcp_register_module(&sa_module);
}
module_init(keystone_sa_init);
static void __exit keystone_sa_exit(void)
{
netcp_unregister_module(&sa_module);
}
module_exit(keystone_sa_exit);
MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("IPSec driver for Keystone devices");
|