]>
Commit | Line | Data |
---|---|---|
61e12104 WK |
1 | /* |
2 | * Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. | |
3 | * | |
4 | * This software is licensed under the terms of the GNU General Public | |
5 | * License version 2, as published by the Free Software Foundation, and | |
6 | * may be copied, distributed, and modified under those terms. | |
7 | * | |
8 | * This program is distributed in the hope that it will be useful, | |
9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | * GNU General Public License for more details. | |
12 | */ | |
13 | ||
0ec473b5 JP |
14 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
15 | ||
61e12104 WK |
16 | #include <linux/etherdevice.h> |
17 | #include <linux/ip.h> | |
18 | #include <linux/ipv6.h> | |
19 | #include <linux/udp.h> | |
20 | #include <linux/in.h> | |
21 | #include <linux/if_arp.h> | |
22 | #include <linux/if_ether.h> | |
23 | #include <linux/if_vlan.h> | |
61e12104 | 24 | #include <linux/in6.h> |
61e12104 WK |
25 | #include <linux/tcp.h> |
26 | #include <linux/icmp.h> | |
27 | #include <linux/icmpv6.h> | |
28 | #include <linux/uaccess.h> | |
29 | #include <net/ndisc.h> | |
30 | ||
31 | #include "gdm_lte.h" | |
32 | #include "netlink_k.h" | |
33 | #include "hci.h" | |
34 | #include "hci_packet.h" | |
35 | #include "gdm_endian.h" | |
61e12104 WK |
36 | |
37 | /* | |
38 | * Netlink protocol number | |
39 | */ | |
40 | #define NETLINK_LTE 30 | |
41 | ||
42 | /* | |
43 | * Default MTU Size | |
44 | */ | |
45 | #define DEFAULT_MTU_SIZE 1500 | |
46 | ||
61e12104 WK |
47 | #define IP_VERSION_4 4 |
48 | #define IP_VERSION_6 6 | |
49 | ||
50 | static struct { | |
51 | int ref_cnt; | |
52 | struct sock *sock; | |
53 | } lte_event; | |
54 | ||
55 | static struct device_type wwan_type = { | |
56 | .name = "wwan", | |
57 | }; | |
58 | ||
59 | static int gdm_lte_open(struct net_device *dev) | |
60 | { | |
61 | netif_start_queue(dev); | |
62 | return 0; | |
63 | } | |
64 | ||
65 | static int gdm_lte_close(struct net_device *dev) | |
66 | { | |
67 | netif_stop_queue(dev); | |
68 | return 0; | |
69 | } | |
70 | ||
71 | static int gdm_lte_set_config(struct net_device *dev, struct ifmap *map) | |
72 | { | |
73 | if (dev->flags & IFF_UP) | |
74 | return -EBUSY; | |
75 | return 0; | |
76 | } | |
77 | ||
78 | static void tx_complete(void *arg) | |
79 | { | |
80 | struct nic *nic = arg; | |
81 | ||
82 | if (netif_queue_stopped(nic->netdev)) | |
83 | netif_wake_queue(nic->netdev); | |
84 | } | |
85 | ||
86 | static int gdm_lte_rx(struct sk_buff *skb, struct nic *nic, int nic_type) | |
87 | { | |
88 | int ret; | |
89 | ||
90 | ret = netif_rx_ni(skb); | |
91 | if (ret == NET_RX_DROP) { | |
61e12104 WK |
92 | nic->stats.rx_dropped++; |
93 | } else { | |
94 | nic->stats.rx_packets++; | |
95 | nic->stats.rx_bytes += skb->len + ETH_HLEN; | |
96 | } | |
97 | ||
98 | return 0; | |
99 | } | |
100 | ||
ff52b8fe | 101 | static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type) |
61e12104 WK |
102 | { |
103 | struct nic *nic = netdev_priv(skb_in->dev); | |
104 | struct sk_buff *skb_out; | |
105 | struct ethhdr eth; | |
106 | struct vlan_ethhdr vlan_eth; | |
107 | struct arphdr *arp_in; | |
108 | struct arphdr *arp_out; | |
109 | struct arpdata { | |
110 | u8 ar_sha[ETH_ALEN]; | |
111 | u8 ar_sip[4]; | |
112 | u8 ar_tha[ETH_ALEN]; | |
113 | u8 ar_tip[4]; | |
114 | }; | |
115 | struct arpdata *arp_data_in; | |
116 | struct arpdata *arp_data_out; | |
117 | u8 arp_temp[60]; | |
118 | void *mac_header_data; | |
119 | u32 mac_header_len; | |
120 | ||
121 | /* Format the mac header so that it can be put to skb */ | |
122 | if (ntohs(((struct ethhdr *)skb_in->data)->h_proto) == ETH_P_8021Q) { | |
123 | memcpy(&vlan_eth, skb_in->data, sizeof(struct vlan_ethhdr)); | |
124 | mac_header_data = &vlan_eth; | |
125 | mac_header_len = VLAN_ETH_HLEN; | |
126 | } else { | |
127 | memcpy(ð, skb_in->data, sizeof(struct ethhdr)); | |
128 | mac_header_data = ð | |
129 | mac_header_len = ETH_HLEN; | |
130 | } | |
131 | ||
132 | /* Get the pointer of the original request */ | |
133 | arp_in = (struct arphdr *)(skb_in->data + mac_header_len); | |
097b4d8c GK |
134 | arp_data_in = (struct arpdata *)(skb_in->data + mac_header_len + |
135 | sizeof(struct arphdr)); | |
61e12104 WK |
136 | |
137 | /* Get the pointer of the outgoing response */ | |
138 | arp_out = (struct arphdr *)arp_temp; | |
139 | arp_data_out = (struct arpdata *)(arp_temp + sizeof(struct arphdr)); | |
140 | ||
141 | /* Copy the arp header */ | |
142 | memcpy(arp_out, arp_in, sizeof(struct arphdr)); | |
143 | arp_out->ar_op = htons(ARPOP_REPLY); | |
144 | ||
145 | /* Copy the arp payload: based on 2 bytes of mac and fill the IP */ | |
146 | arp_data_out->ar_sha[0] = arp_data_in->ar_sha[0]; | |
147 | arp_data_out->ar_sha[1] = arp_data_in->ar_sha[1]; | |
148 | memcpy(&arp_data_out->ar_sha[2], &arp_data_in->ar_tip[0], 4); | |
149 | memcpy(&arp_data_out->ar_sip[0], &arp_data_in->ar_tip[0], 4); | |
150 | memcpy(&arp_data_out->ar_tha[0], &arp_data_in->ar_sha[0], 6); | |
151 | memcpy(&arp_data_out->ar_tip[0], &arp_data_in->ar_sip[0], 4); | |
152 | ||
153 | /* Fill the destination mac with source mac of the received packet */ | |
154 | memcpy(mac_header_data, mac_header_data + ETH_ALEN, ETH_ALEN); | |
155 | /* Fill the source mac with nic's source mac */ | |
156 | memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN); | |
157 | ||
158 | /* Alloc skb and reserve align */ | |
159 | skb_out = dev_alloc_skb(skb_in->len); | |
160 | if (!skb_out) | |
161 | return -ENOMEM; | |
162 | skb_reserve(skb_out, NET_IP_ALIGN); | |
163 | ||
59ae1d12 JB |
164 | skb_put_data(skb_out, mac_header_data, mac_header_len); |
165 | skb_put_data(skb_out, arp_out, sizeof(struct arphdr)); | |
166 | skb_put_data(skb_out, arp_data_out, sizeof(struct arpdata)); | |
61e12104 WK |
167 | |
168 | skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto; | |
169 | skb_out->dev = skb_in->dev; | |
170 | skb_reset_mac_header(skb_out); | |
171 | skb_pull(skb_out, ETH_HLEN); | |
172 | ||
173 | gdm_lte_rx(skb_out, nic, nic_type); | |
174 | ||
175 | return 0; | |
176 | } | |
177 | ||
efe96779 | 178 | static __sum16 icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len) |
61e12104 WK |
179 | { |
180 | unsigned short *w = ptr; | |
efe96779 | 181 | __wsum sum = 0; |
61e12104 WK |
182 | int i; |
183 | ||
184 | union { | |
185 | struct { | |
186 | u8 ph_src[16]; | |
187 | u8 ph_dst[16]; | |
188 | u32 ph_len; | |
189 | u8 ph_zero[3]; | |
190 | u8 ph_nxt; | |
191 | } ph __packed; | |
192 | u16 pa[20]; | |
193 | } pseudo_header; | |
194 | ||
195 | memset(&pseudo_header, 0, sizeof(pseudo_header)); | |
196 | memcpy(&pseudo_header.ph.ph_src, &ipv6->saddr.in6_u.u6_addr8, 16); | |
197 | memcpy(&pseudo_header.ph.ph_dst, &ipv6->daddr.in6_u.u6_addr8, 16); | |
d3ea53c7 | 198 | pseudo_header.ph.ph_len = be16_to_cpu(ipv6->payload_len); |
61e12104 WK |
199 | pseudo_header.ph.ph_nxt = ipv6->nexthdr; |
200 | ||
201 | w = (u16 *)&pseudo_header; | |
097b4d8c | 202 | for (i = 0; i < ARRAY_SIZE(pseudo_header.pa); i++) |
efe96779 JR |
203 | sum = csum_add(sum, csum_unfold( |
204 | (__force __sum16)pseudo_header.pa[i])); | |
61e12104 WK |
205 | |
206 | w = ptr; | |
207 | while (len > 1) { | |
efe96779 | 208 | sum = csum_add(sum, csum_unfold((__force __sum16)*w++)); |
61e12104 WK |
209 | len -= 2; |
210 | } | |
211 | ||
efe96779 | 212 | return csum_fold(sum); |
61e12104 WK |
213 | } |
214 | ||
ff52b8fe | 215 | static int gdm_lte_emulate_ndp(struct sk_buff *skb_in, u32 nic_type) |
61e12104 WK |
216 | { |
217 | struct nic *nic = netdev_priv(skb_in->dev); | |
218 | struct sk_buff *skb_out; | |
219 | struct ethhdr eth; | |
220 | struct vlan_ethhdr vlan_eth; | |
221 | struct neighbour_advertisement { | |
222 | u8 target_address[16]; | |
223 | u8 type; | |
224 | u8 length; | |
225 | u8 link_layer_address[6]; | |
226 | }; | |
227 | struct neighbour_advertisement na; | |
228 | struct neighbour_solicitation { | |
229 | u8 target_address[16]; | |
230 | }; | |
231 | struct neighbour_solicitation *ns; | |
232 | struct ipv6hdr *ipv6_in; | |
233 | struct ipv6hdr ipv6_out; | |
234 | struct icmp6hdr *icmp6_in; | |
235 | struct icmp6hdr icmp6_out; | |
236 | ||
237 | void *mac_header_data; | |
238 | u32 mac_header_len; | |
239 | ||
240 | /* Format the mac header so that it can be put to skb */ | |
241 | if (ntohs(((struct ethhdr *)skb_in->data)->h_proto) == ETH_P_8021Q) { | |
242 | memcpy(&vlan_eth, skb_in->data, sizeof(struct vlan_ethhdr)); | |
243 | if (ntohs(vlan_eth.h_vlan_encapsulated_proto) != ETH_P_IPV6) | |
244 | return -1; | |
245 | mac_header_data = &vlan_eth; | |
246 | mac_header_len = VLAN_ETH_HLEN; | |
247 | } else { | |
248 | memcpy(ð, skb_in->data, sizeof(struct ethhdr)); | |
249 | if (ntohs(eth.h_proto) != ETH_P_IPV6) | |
250 | return -1; | |
251 | mac_header_data = ð | |
252 | mac_header_len = ETH_HLEN; | |
253 | } | |
254 | ||
255 | /* Check if this is IPv6 ICMP packet */ | |
256 | ipv6_in = (struct ipv6hdr *)(skb_in->data + mac_header_len); | |
257 | if (ipv6_in->version != 6 || ipv6_in->nexthdr != IPPROTO_ICMPV6) | |
258 | return -1; | |
259 | ||
260 | /* Check if this is NDP packet */ | |
097b4d8c GK |
261 | icmp6_in = (struct icmp6hdr *)(skb_in->data + mac_header_len + |
262 | sizeof(struct ipv6hdr)); | |
61e12104 WK |
263 | if (icmp6_in->icmp6_type == NDISC_ROUTER_SOLICITATION) { /* Check RS */ |
264 | return -1; | |
097b4d8c GK |
265 | } else if (icmp6_in->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) { |
266 | /* Check NS */ | |
267 | u8 icmp_na[sizeof(struct icmp6hdr) + | |
268 | sizeof(struct neighbour_advertisement)]; | |
61e12104 WK |
269 | u8 zero_addr8[16] = {0,}; |
270 | ||
271 | if (memcmp(ipv6_in->saddr.in6_u.u6_addr8, zero_addr8, 16) == 0) | |
272 | /* Duplicate Address Detection: Source IP is all zero */ | |
273 | return 0; | |
274 | ||
275 | icmp6_out.icmp6_type = NDISC_NEIGHBOUR_ADVERTISEMENT; | |
276 | icmp6_out.icmp6_code = 0; | |
277 | icmp6_out.icmp6_cksum = 0; | |
59215e69 NZ |
278 | /* R=0, S=1, O=1 */ |
279 | icmp6_out.icmp6_dataun.un_data32[0] = htonl(0x60000000); | |
61e12104 | 280 | |
097b4d8c GK |
281 | ns = (struct neighbour_solicitation *) |
282 | (skb_in->data + mac_header_len + | |
283 | sizeof(struct ipv6hdr) + sizeof(struct icmp6hdr)); | |
61e12104 WK |
284 | memcpy(&na.target_address, ns->target_address, 16); |
285 | na.type = 0x02; | |
286 | na.length = 1; | |
287 | na.link_layer_address[0] = 0x00; | |
288 | na.link_layer_address[1] = 0x0a; | |
289 | na.link_layer_address[2] = 0x3b; | |
290 | na.link_layer_address[3] = 0xaf; | |
291 | na.link_layer_address[4] = 0x63; | |
292 | na.link_layer_address[5] = 0xc7; | |
293 | ||
294 | memcpy(&ipv6_out, ipv6_in, sizeof(struct ipv6hdr)); | |
295 | memcpy(ipv6_out.saddr.in6_u.u6_addr8, &na.target_address, 16); | |
097b4d8c | 296 | memcpy(ipv6_out.daddr.in6_u.u6_addr8, |
a4785ef8 | 297 | ipv6_in->saddr.in6_u.u6_addr8, 16); |
097b4d8c GK |
298 | ipv6_out.payload_len = htons(sizeof(struct icmp6hdr) + |
299 | sizeof(struct neighbour_advertisement)); | |
61e12104 WK |
300 | |
301 | memcpy(icmp_na, &icmp6_out, sizeof(struct icmp6hdr)); | |
097b4d8c | 302 | memcpy(icmp_na + sizeof(struct icmp6hdr), &na, |
a4785ef8 | 303 | sizeof(struct neighbour_advertisement)); |
61e12104 | 304 | |
097b4d8c GK |
305 | icmp6_out.icmp6_cksum = icmp6_checksum(&ipv6_out, |
306 | (u16 *)icmp_na, sizeof(icmp_na)); | |
61e12104 WK |
307 | } else { |
308 | return -1; | |
309 | } | |
310 | ||
311 | /* Fill the destination mac with source mac of the received packet */ | |
312 | memcpy(mac_header_data, mac_header_data + ETH_ALEN, ETH_ALEN); | |
313 | /* Fill the source mac with nic's source mac */ | |
314 | memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN); | |
315 | ||
316 | /* Alloc skb and reserve align */ | |
317 | skb_out = dev_alloc_skb(skb_in->len); | |
318 | if (!skb_out) | |
319 | return -ENOMEM; | |
320 | skb_reserve(skb_out, NET_IP_ALIGN); | |
321 | ||
59ae1d12 JB |
322 | skb_put_data(skb_out, mac_header_data, mac_header_len); |
323 | skb_put_data(skb_out, &ipv6_out, sizeof(struct ipv6hdr)); | |
324 | skb_put_data(skb_out, &icmp6_out, sizeof(struct icmp6hdr)); | |
325 | skb_put_data(skb_out, &na, sizeof(struct neighbour_advertisement)); | |
61e12104 WK |
326 | |
327 | skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto; | |
328 | skb_out->dev = skb_in->dev; | |
329 | skb_reset_mac_header(skb_out); | |
330 | skb_pull(skb_out, ETH_HLEN); | |
331 | ||
332 | gdm_lte_rx(skb_out, nic, nic_type); | |
333 | ||
334 | return 0; | |
335 | } | |
336 | ||
337 | static s32 gdm_lte_tx_nic_type(struct net_device *dev, struct sk_buff *skb) | |
338 | { | |
339 | struct nic *nic = netdev_priv(dev); | |
340 | struct ethhdr *eth; | |
341 | struct vlan_ethhdr *vlan_eth; | |
342 | struct iphdr *ip; | |
343 | struct ipv6hdr *ipv6; | |
344 | int mac_proto; | |
345 | void *network_data; | |
ca5af1f3 | 346 | u32 nic_type; |
61e12104 WK |
347 | |
348 | /* NIC TYPE is based on the nic_id of this net_device */ | |
349 | nic_type = 0x00000010 | nic->nic_id; | |
350 | ||
351 | /* Get ethernet protocol */ | |
352 | eth = (struct ethhdr *)skb->data; | |
353 | if (ntohs(eth->h_proto) == ETH_P_8021Q) { | |
354 | vlan_eth = (struct vlan_ethhdr *)skb->data; | |
355 | mac_proto = ntohs(vlan_eth->h_vlan_encapsulated_proto); | |
356 | network_data = skb->data + VLAN_ETH_HLEN; | |
357 | nic_type |= NIC_TYPE_F_VLAN; | |
358 | } else { | |
359 | mac_proto = ntohs(eth->h_proto); | |
360 | network_data = skb->data + ETH_HLEN; | |
361 | } | |
362 | ||
363 | /* Process packet for nic type */ | |
364 | switch (mac_proto) { | |
365 | case ETH_P_ARP: | |
366 | nic_type |= NIC_TYPE_ARP; | |
367 | break; | |
368 | case ETH_P_IP: | |
369 | nic_type |= NIC_TYPE_F_IPV4; | |
2594ca30 | 370 | ip = network_data; |
61e12104 WK |
371 | |
372 | /* Check DHCPv4 */ | |
373 | if (ip->protocol == IPPROTO_UDP) { | |
2594ca30 | 374 | struct udphdr *udp = |
e3b07865 | 375 | network_data + sizeof(struct iphdr); |
61e12104 WK |
376 | if (ntohs(udp->dest) == 67 || ntohs(udp->dest) == 68) |
377 | nic_type |= NIC_TYPE_F_DHCP; | |
378 | } | |
379 | break; | |
380 | case ETH_P_IPV6: | |
381 | nic_type |= NIC_TYPE_F_IPV6; | |
2594ca30 | 382 | ipv6 = network_data; |
61e12104 WK |
383 | |
384 | if (ipv6->nexthdr == IPPROTO_ICMPV6) /* Check NDP request */ { | |
2594ca30 | 385 | struct icmp6hdr *icmp6 = |
e3b07865 | 386 | network_data + sizeof(struct ipv6hdr); |
097b4d8c | 387 | if (icmp6->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) |
61e12104 WK |
388 | nic_type |= NIC_TYPE_ICMPV6; |
389 | } else if (ipv6->nexthdr == IPPROTO_UDP) /* Check DHCPv6 */ { | |
2594ca30 | 390 | struct udphdr *udp = |
e3b07865 | 391 | network_data + sizeof(struct ipv6hdr); |
61e12104 WK |
392 | if (ntohs(udp->dest) == 546 || ntohs(udp->dest) == 547) |
393 | nic_type |= NIC_TYPE_F_DHCP; | |
394 | } | |
395 | break; | |
396 | default: | |
397 | break; | |
398 | } | |
399 | ||
400 | return nic_type; | |
401 | } | |
402 | ||
403 | static int gdm_lte_tx(struct sk_buff *skb, struct net_device *dev) | |
404 | { | |
405 | struct nic *nic = netdev_priv(dev); | |
406 | u32 nic_type; | |
407 | void *data_buf; | |
408 | int data_len; | |
409 | int idx; | |
410 | int ret = 0; | |
411 | ||
412 | nic_type = gdm_lte_tx_nic_type(dev, skb); | |
413 | if (nic_type == 0) { | |
0ec473b5 | 414 | netdev_err(dev, "tx - invalid nic_type\n"); |
61e12104 WK |
415 | return -1; |
416 | } | |
417 | ||
418 | if (nic_type & NIC_TYPE_ARP) { | |
419 | if (gdm_lte_emulate_arp(skb, nic_type) == 0) { | |
420 | dev_kfree_skb(skb); | |
421 | return 0; | |
422 | } | |
423 | } | |
424 | ||
425 | if (nic_type & NIC_TYPE_ICMPV6) { | |
426 | if (gdm_lte_emulate_ndp(skb, nic_type) == 0) { | |
427 | dev_kfree_skb(skb); | |
428 | return 0; | |
429 | } | |
430 | } | |
431 | ||
432 | /* | |
097b4d8c GK |
433 | * Need byte shift (that is, remove VLAN tag) if there is one |
434 | * For the case of ARP, this breaks the offset as vlan_ethhdr+4 | |
435 | * is treated as ethhdr However, it shouldn't be a problem as | |
436 | * the response starts from arp_hdr and ethhdr is created by this | |
437 | * driver based on the NIC mac | |
438 | */ | |
61e12104 WK |
439 | if (nic_type & NIC_TYPE_F_VLAN) { |
440 | struct vlan_ethhdr *vlan_eth = (struct vlan_ethhdr *)skb->data; | |
4e13d410 | 441 | |
61e12104 WK |
442 | nic->vlan_id = ntohs(vlan_eth->h_vlan_TCI) & VLAN_VID_MASK; |
443 | data_buf = skb->data + (VLAN_ETH_HLEN - ETH_HLEN); | |
444 | data_len = skb->len - (VLAN_ETH_HLEN - ETH_HLEN); | |
445 | } else { | |
446 | nic->vlan_id = 0; | |
447 | data_buf = skb->data; | |
448 | data_len = skb->len; | |
449 | } | |
450 | ||
097b4d8c GK |
451 | /* If it is a ICMPV6 packet, clear all the other bits : |
452 | * for backward compatibility with the firmware | |
453 | */ | |
61e12104 WK |
454 | if (nic_type & NIC_TYPE_ICMPV6) |
455 | nic_type = NIC_TYPE_ICMPV6; | |
456 | ||
097b4d8c GK |
457 | /* If it is not a dhcp packet, clear all the flag bits : |
458 | * original NIC, otherwise the special flag (IPVX | DHCP) | |
459 | */ | |
61e12104 WK |
460 | if (!(nic_type & NIC_TYPE_F_DHCP)) |
461 | nic_type &= NIC_TYPE_MASK; | |
462 | ||
a28bfd11 MA |
463 | ret = sscanf(dev->name, "lte%d", &idx); |
464 | if (ret != 1) { | |
465 | dev_kfree_skb(skb); | |
466 | return -EINVAL; | |
467 | } | |
61e12104 | 468 | |
a23bb460 RK |
469 | ret = nic->phy_dev->send_sdu_func(nic->phy_dev->priv_dev, |
470 | data_buf, data_len, | |
471 | nic->pdn_table.dft_eps_id, 0, | |
472 | tx_complete, nic, idx, | |
473 | nic_type); | |
61e12104 WK |
474 | |
475 | if (ret == TX_NO_BUFFER || ret == TX_NO_SPC) { | |
476 | netif_stop_queue(dev); | |
477 | if (ret == TX_NO_BUFFER) | |
478 | ret = 0; | |
479 | else | |
480 | ret = -ENOSPC; | |
481 | } else if (ret == TX_NO_DEV) { | |
482 | ret = -ENODEV; | |
483 | } | |
484 | ||
485 | /* Updates tx stats */ | |
486 | if (ret) { | |
487 | nic->stats.tx_dropped++; | |
488 | } else { | |
489 | nic->stats.tx_packets++; | |
490 | nic->stats.tx_bytes += data_len; | |
491 | } | |
492 | dev_kfree_skb(skb); | |
493 | ||
494 | return 0; | |
495 | } | |
496 | ||
497 | static struct net_device_stats *gdm_lte_stats(struct net_device *dev) | |
498 | { | |
499 | struct nic *nic = netdev_priv(dev); | |
4e13d410 | 500 | |
61e12104 WK |
501 | return &nic->stats; |
502 | } | |
503 | ||
61e12104 WK |
504 | static int gdm_lte_event_send(struct net_device *dev, char *buf, int len) |
505 | { | |
506 | struct nic *nic = netdev_priv(dev); | |
507 | struct hci_packet *hci = (struct hci_packet *)buf; | |
508 | int idx; | |
a28bfd11 | 509 | int ret; |
61e12104 | 510 | |
a28bfd11 MA |
511 | ret = sscanf(dev->name, "lte%d", &idx); |
512 | if (ret != 1) | |
513 | return -EINVAL; | |
61e12104 WK |
514 | |
515 | return netlink_send(lte_event.sock, idx, 0, buf, | |
04db9c6a RK |
516 | gdm_dev16_to_cpu( |
517 | nic->phy_dev->get_endian( | |
518 | nic->phy_dev->priv_dev), hci->len) | |
519 | + HCI_HEADER_SIZE); | |
61e12104 WK |
520 | } |
521 | ||
097b4d8c | 522 | static void gdm_lte_event_rcv(struct net_device *dev, u16 type, |
a4785ef8 | 523 | void *msg, int len) |
61e12104 WK |
524 | { |
525 | struct nic *nic = netdev_priv(dev); | |
526 | ||
abb40c11 RK |
527 | nic->phy_dev->send_hci_func(nic->phy_dev->priv_dev, msg, len, NULL, |
528 | NULL); | |
61e12104 WK |
529 | } |
530 | ||
531 | int gdm_lte_event_init(void) | |
532 | { | |
533 | if (lte_event.ref_cnt == 0) | |
534 | lte_event.sock = netlink_init(NETLINK_LTE, gdm_lte_event_rcv); | |
535 | ||
536 | if (lte_event.sock) { | |
537 | lte_event.ref_cnt++; | |
538 | return 0; | |
539 | } | |
540 | ||
0ec473b5 | 541 | pr_err("event init failed\n"); |
61e12104 WK |
542 | return -1; |
543 | } | |
544 | ||
545 | void gdm_lte_event_exit(void) | |
546 | { | |
547 | if (lte_event.sock && --lte_event.ref_cnt == 0) { | |
ca3fde19 | 548 | sock_release(lte_event.sock->sk_socket); |
61e12104 WK |
549 | lte_event.sock = NULL; |
550 | } | |
551 | } | |
552 | ||
cd47e4d6 | 553 | static int find_dev_index(u32 nic_type) |
61e12104 WK |
554 | { |
555 | u8 index; | |
556 | ||
557 | index = (u8)(nic_type & 0x0000000f); | |
cd47e4d6 DC |
558 | if (index >= MAX_NIC_TYPE) |
559 | return -EINVAL; | |
61e12104 WK |
560 | |
561 | return index; | |
562 | } | |
563 | ||
097b4d8c | 564 | static void gdm_lte_netif_rx(struct net_device *dev, char *buf, |
a4785ef8 | 565 | int len, int flagged_nic_type) |
61e12104 WK |
566 | { |
567 | u32 nic_type; | |
568 | struct nic *nic; | |
569 | struct sk_buff *skb; | |
570 | struct ethhdr eth; | |
571 | struct vlan_ethhdr vlan_eth; | |
572 | void *mac_header_data; | |
573 | u32 mac_header_len; | |
574 | char ip_version = 0; | |
575 | ||
576 | nic_type = flagged_nic_type & NIC_TYPE_MASK; | |
577 | nic = netdev_priv(dev); | |
578 | ||
579 | if (flagged_nic_type & NIC_TYPE_F_DHCP) { | |
097b4d8c GK |
580 | /* Change the destination mac address |
581 | * with the one requested the IP | |
582 | */ | |
61e12104 WK |
583 | if (flagged_nic_type & NIC_TYPE_F_IPV4) { |
584 | struct dhcp_packet { | |
585 | u8 op; /* BOOTREQUEST or BOOTREPLY */ | |
097b4d8c GK |
586 | u8 htype; /* hardware address type. |
587 | * 1 = 10mb ethernet | |
588 | */ | |
61e12104 WK |
589 | u8 hlen; /* hardware address length */ |
590 | u8 hops; /* used by relay agents only */ | |
591 | u32 xid; /* unique id */ | |
097b4d8c GK |
592 | u16 secs; /* elapsed since client began |
593 | * acquisition/renewal | |
594 | */ | |
61e12104 | 595 | u16 flags; /* only one flag so far: */ |
097b4d8c GK |
596 | #define BROADCAST_FLAG 0x8000 |
597 | /* "I need broadcast replies" */ | |
598 | u32 ciaddr; /* client IP (if client is in | |
599 | * BOUND, RENEW or REBINDING state) | |
600 | */ | |
61e12104 | 601 | u32 yiaddr; /* 'your' (client) IP address */ |
097b4d8c GK |
602 | /* IP address of next server to use in |
603 | * bootstrap, returned in DHCPOFFER, | |
604 | * DHCPACK by server | |
605 | */ | |
61e12104 WK |
606 | u32 siaddr_nip; |
607 | u32 gateway_nip; /* relay agent IP address */ | |
097b4d8c GK |
608 | u8 chaddr[16]; /* link-layer client hardware |
609 | * address (MAC) | |
610 | */ | |
61e12104 WK |
611 | u8 sname[64]; /* server host name (ASCIZ) */ |
612 | u8 file[128]; /* boot file name (ASCIZ) */ | |
097b4d8c GK |
613 | u32 cookie; /* fixed first four option |
614 | * bytes (99,130,83,99 dec) | |
615 | */ | |
61e12104 | 616 | } __packed; |
097b4d8c GK |
617 | void *addr = buf + sizeof(struct iphdr) + |
618 | sizeof(struct udphdr) + | |
619 | offsetof(struct dhcp_packet, chaddr); | |
39952134 | 620 | ether_addr_copy(nic->dest_mac_addr, addr); |
61e12104 WK |
621 | } |
622 | } | |
623 | ||
624 | if (nic->vlan_id > 0) { | |
625 | mac_header_data = (void *)&vlan_eth; | |
626 | mac_header_len = VLAN_ETH_HLEN; | |
627 | } else { | |
628 | mac_header_data = (void *)ð | |
629 | mac_header_len = ETH_HLEN; | |
630 | } | |
631 | ||
632 | /* Format the data so that it can be put to skb */ | |
39952134 | 633 | ether_addr_copy(mac_header_data, nic->dest_mac_addr); |
61e12104 WK |
634 | memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN); |
635 | ||
636 | vlan_eth.h_vlan_TCI = htons(nic->vlan_id); | |
637 | vlan_eth.h_vlan_proto = htons(ETH_P_8021Q); | |
638 | ||
639 | if (nic_type == NIC_TYPE_ARP) { | |
097b4d8c GK |
640 | /* Should be response: Only happens because |
641 | * there was a request from the host | |
642 | */ | |
61e12104 WK |
643 | eth.h_proto = htons(ETH_P_ARP); |
644 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_ARP); | |
645 | } else { | |
646 | ip_version = buf[0] >> 4; | |
647 | if (ip_version == IP_VERSION_4) { | |
648 | eth.h_proto = htons(ETH_P_IP); | |
649 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IP); | |
650 | } else if (ip_version == IP_VERSION_6) { | |
651 | eth.h_proto = htons(ETH_P_IPV6); | |
652 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IPV6); | |
653 | } else { | |
0ec473b5 | 654 | netdev_err(dev, "Unknown IP version %d\n", ip_version); |
61e12104 WK |
655 | return; |
656 | } | |
657 | } | |
658 | ||
659 | /* Alloc skb and reserve align */ | |
660 | skb = dev_alloc_skb(len + mac_header_len + NET_IP_ALIGN); | |
661 | if (!skb) | |
662 | return; | |
663 | skb_reserve(skb, NET_IP_ALIGN); | |
664 | ||
59ae1d12 JB |
665 | skb_put_data(skb, mac_header_data, mac_header_len); |
666 | skb_put_data(skb, buf, len); | |
61e12104 WK |
667 | |
668 | skb->protocol = ((struct ethhdr *)mac_header_data)->h_proto; | |
669 | skb->dev = dev; | |
670 | skb_reset_mac_header(skb); | |
671 | skb_pull(skb, ETH_HLEN); | |
672 | ||
673 | gdm_lte_rx(skb, nic, nic_type); | |
674 | } | |
675 | ||
676 | static void gdm_lte_multi_sdu_pkt(struct phy_dev *phy_dev, char *buf, int len) | |
677 | { | |
678 | struct net_device *dev; | |
679 | struct multi_sdu *multi_sdu = (struct multi_sdu *)buf; | |
680 | struct sdu *sdu = NULL; | |
9b9cefd0 | 681 | struct gdm_endian *endian = phy_dev->get_endian(phy_dev->priv_dev); |
61e12104 WK |
682 | u8 *data = (u8 *)multi_sdu->data; |
683 | u16 i = 0; | |
684 | u16 num_packet; | |
685 | u16 hci_len; | |
686 | u16 cmd_evt; | |
687 | u32 nic_type; | |
cd47e4d6 | 688 | int index; |
61e12104 | 689 | |
9b9cefd0 EG |
690 | hci_len = gdm_dev16_to_cpu(endian, multi_sdu->len); |
691 | num_packet = gdm_dev16_to_cpu(endian, multi_sdu->num_packet); | |
61e12104 WK |
692 | |
693 | for (i = 0; i < num_packet; i++) { | |
694 | sdu = (struct sdu *)data; | |
695 | ||
9b9cefd0 EG |
696 | cmd_evt = gdm_dev16_to_cpu(endian, sdu->cmd_evt); |
697 | hci_len = gdm_dev16_to_cpu(endian, sdu->len); | |
698 | nic_type = gdm_dev32_to_cpu(endian, sdu->nic_type); | |
61e12104 WK |
699 | |
700 | if (cmd_evt != LTE_RX_SDU) { | |
0ec473b5 | 701 | pr_err("rx sdu wrong hci %04x\n", cmd_evt); |
61e12104 WK |
702 | return; |
703 | } | |
704 | if (hci_len < 12) { | |
0ec473b5 | 705 | pr_err("rx sdu invalid len %d\n", hci_len); |
61e12104 WK |
706 | return; |
707 | } | |
708 | ||
709 | index = find_dev_index(nic_type); | |
cd47e4d6 | 710 | if (index < 0) { |
0ec473b5 | 711 | pr_err("rx sdu invalid nic_type :%x\n", nic_type); |
cd47e4d6 | 712 | return; |
61e12104 | 713 | } |
cd47e4d6 DC |
714 | dev = phy_dev->dev[index]; |
715 | gdm_lte_netif_rx(dev, (char *)sdu->data, | |
716 | (int)(hci_len - 12), nic_type); | |
61e12104 | 717 | |
ba7f55b7 | 718 | data += ((hci_len + 3) & 0xfffc) + HCI_HEADER_SIZE; |
61e12104 WK |
719 | } |
720 | } | |
721 | ||
722 | static void gdm_lte_pdn_table(struct net_device *dev, char *buf, int len) | |
723 | { | |
724 | struct nic *nic = netdev_priv(dev); | |
725 | struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf; | |
726 | ||
727 | if (pdn_table->activate) { | |
728 | nic->pdn_table.activate = pdn_table->activate; | |
04db9c6a RK |
729 | nic->pdn_table.dft_eps_id = gdm_dev32_to_cpu( |
730 | nic->phy_dev->get_endian( | |
731 | nic->phy_dev->priv_dev), | |
732 | pdn_table->dft_eps_id); | |
733 | nic->pdn_table.nic_type = gdm_dev32_to_cpu( | |
734 | nic->phy_dev->get_endian( | |
735 | nic->phy_dev->priv_dev), | |
736 | pdn_table->nic_type); | |
61e12104 | 737 | |
0ec473b5 JP |
738 | netdev_info(dev, "pdn activated, nic_type=0x%x\n", |
739 | nic->pdn_table.nic_type); | |
61e12104 WK |
740 | } else { |
741 | memset(&nic->pdn_table, 0x00, sizeof(struct pdn_table)); | |
0ec473b5 | 742 | netdev_info(dev, "pdn deactivated\n"); |
61e12104 WK |
743 | } |
744 | } | |
745 | ||
746 | static int gdm_lte_receive_pkt(struct phy_dev *phy_dev, char *buf, int len) | |
747 | { | |
748 | struct hci_packet *hci = (struct hci_packet *)buf; | |
749 | struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf; | |
9b9cefd0 | 750 | struct gdm_endian *endian = phy_dev->get_endian(phy_dev->priv_dev); |
61e12104 WK |
751 | struct sdu *sdu; |
752 | struct net_device *dev; | |
753 | int ret = 0; | |
754 | u16 cmd_evt; | |
755 | u32 nic_type; | |
cd47e4d6 | 756 | int index; |
61e12104 WK |
757 | |
758 | if (!len) | |
759 | return ret; | |
760 | ||
9b9cefd0 | 761 | cmd_evt = gdm_dev16_to_cpu(endian, hci->cmd_evt); |
61e12104 WK |
762 | |
763 | dev = phy_dev->dev[0]; | |
b6f6fd8a | 764 | if (!dev) |
61e12104 WK |
765 | return 0; |
766 | ||
767 | switch (cmd_evt) { | |
768 | case LTE_RX_SDU: | |
769 | sdu = (struct sdu *)hci->data; | |
9b9cefd0 | 770 | nic_type = gdm_dev32_to_cpu(endian, sdu->nic_type); |
61e12104 | 771 | index = find_dev_index(nic_type); |
cd47e4d6 DC |
772 | if (index < 0) |
773 | return index; | |
61e12104 WK |
774 | dev = phy_dev->dev[index]; |
775 | gdm_lte_netif_rx(dev, hci->data, len, nic_type); | |
776 | break; | |
777 | case LTE_RX_MULTI_SDU: | |
778 | gdm_lte_multi_sdu_pkt(phy_dev, buf, len); | |
779 | break; | |
780 | case LTE_LINK_ON_OFF_INDICATION: | |
0ec473b5 JP |
781 | netdev_info(dev, "link %s\n", |
782 | ((struct hci_connect_ind *)buf)->connect | |
783 | ? "on" : "off"); | |
61e12104 WK |
784 | break; |
785 | case LTE_PDN_TABLE_IND: | |
786 | pdn_table = (struct hci_pdn_table_ind *)buf; | |
9b9cefd0 | 787 | nic_type = gdm_dev32_to_cpu(endian, pdn_table->nic_type); |
61e12104 | 788 | index = find_dev_index(nic_type); |
cd47e4d6 DC |
789 | if (index < 0) |
790 | return index; | |
61e12104 WK |
791 | dev = phy_dev->dev[index]; |
792 | gdm_lte_pdn_table(dev, buf, len); | |
793 | /* Fall through */ | |
794 | default: | |
795 | ret = gdm_lte_event_send(dev, buf, len); | |
796 | break; | |
797 | } | |
798 | ||
799 | return ret; | |
800 | } | |
801 | ||
802 | static int rx_complete(void *arg, void *data, int len, int context) | |
803 | { | |
2594ca30 | 804 | struct phy_dev *phy_dev = arg; |
61e12104 | 805 | |
2594ca30 | 806 | return gdm_lte_receive_pkt(phy_dev, data, len); |
61e12104 WK |
807 | } |
808 | ||
809 | void start_rx_proc(struct phy_dev *phy_dev) | |
810 | { | |
811 | int i; | |
812 | ||
813 | for (i = 0; i < MAX_RX_SUBMIT_COUNT; i++) | |
097b4d8c GK |
814 | phy_dev->rcv_func(phy_dev->priv_dev, |
815 | rx_complete, phy_dev, USB_COMPLETE); | |
61e12104 WK |
816 | } |
817 | ||
ce4b80fb | 818 | static const struct net_device_ops gdm_netdev_ops = { |
61e12104 WK |
819 | .ndo_open = gdm_lte_open, |
820 | .ndo_stop = gdm_lte_close, | |
821 | .ndo_set_config = gdm_lte_set_config, | |
822 | .ndo_start_xmit = gdm_lte_tx, | |
823 | .ndo_get_stats = gdm_lte_stats, | |
61e12104 WK |
824 | }; |
825 | ||
826 | static u8 gdm_lte_macaddr[ETH_ALEN] = {0x00, 0x0a, 0x3b, 0x00, 0x00, 0x00}; | |
827 | ||
097b4d8c | 828 | static void form_mac_address(u8 *dev_addr, u8 *nic_src, u8 *nic_dest, |
a4785ef8 | 829 | u8 *mac_address, u8 index) |
61e12104 WK |
830 | { |
831 | /* Form the dev_addr */ | |
832 | if (!mac_address) | |
39952134 | 833 | ether_addr_copy(dev_addr, gdm_lte_macaddr); |
61e12104 | 834 | else |
39952134 | 835 | ether_addr_copy(dev_addr, mac_address); |
61e12104 | 836 | |
097b4d8c GK |
837 | /* The last byte of the mac address |
838 | * should be less than or equal to 0xFC | |
839 | */ | |
ba7f55b7 | 840 | dev_addr[ETH_ALEN - 1] += index; |
61e12104 | 841 | |
097b4d8c GK |
842 | /* Create random nic src and copy the first |
843 | * 3 bytes to be the same as dev_addr | |
844 | */ | |
4e4acff7 | 845 | eth_random_addr(nic_src); |
61e12104 WK |
846 | memcpy(nic_src, dev_addr, 3); |
847 | ||
848 | /* Copy the nic_dest from dev_addr*/ | |
39952134 | 849 | ether_addr_copy(nic_dest, dev_addr); |
61e12104 WK |
850 | } |
851 | ||
852 | static void validate_mac_address(u8 *mac_address) | |
853 | { | |
854 | /* if zero address or multicast bit set, restore the default value */ | |
855 | if (is_zero_ether_addr(mac_address) || (mac_address[0] & 0x01)) { | |
0ec473b5 | 856 | pr_err("MAC invalid, restoring default\n"); |
61e12104 WK |
857 | memcpy(mac_address, gdm_lte_macaddr, 6); |
858 | } | |
859 | } | |
860 | ||
097b4d8c GK |
861 | int register_lte_device(struct phy_dev *phy_dev, |
862 | struct device *dev, u8 *mac_address) | |
61e12104 WK |
863 | { |
864 | struct nic *nic; | |
865 | struct net_device *net; | |
866 | char pdn_dev_name[16]; | |
867 | int ret = 0; | |
868 | u8 index; | |
869 | ||
870 | validate_mac_address(mac_address); | |
871 | ||
872 | for (index = 0; index < MAX_NIC_TYPE; index++) { | |
873 | /* Create device name lteXpdnX */ | |
874 | sprintf(pdn_dev_name, "lte%%dpdn%d", index); | |
875 | ||
876 | /* Allocate netdev */ | |
097b4d8c | 877 | net = alloc_netdev(sizeof(struct nic), pdn_dev_name, |
c835a677 | 878 | NET_NAME_UNKNOWN, ether_setup); |
9d877fdb | 879 | if (!net) { |
0ec473b5 | 880 | pr_err("alloc_netdev failed\n"); |
61e12104 WK |
881 | ret = -ENOMEM; |
882 | goto err; | |
883 | } | |
884 | net->netdev_ops = &gdm_netdev_ops; | |
885 | net->flags &= ~IFF_MULTICAST; | |
886 | net->mtu = DEFAULT_MTU_SIZE; | |
887 | ||
888 | nic = netdev_priv(net); | |
889 | memset(nic, 0, sizeof(struct nic)); | |
890 | nic->netdev = net; | |
891 | nic->phy_dev = phy_dev; | |
892 | nic->nic_id = index; | |
893 | ||
894 | form_mac_address( | |
895 | net->dev_addr, | |
896 | nic->src_mac_addr, | |
897 | nic->dest_mac_addr, | |
898 | mac_address, | |
899 | index); | |
900 | ||
901 | SET_NETDEV_DEV(net, dev); | |
902 | SET_NETDEV_DEVTYPE(net, &wwan_type); | |
903 | ||
904 | ret = register_netdev(net); | |
905 | if (ret) | |
906 | goto err; | |
907 | ||
908 | netif_carrier_on(net); | |
909 | ||
910 | phy_dev->dev[index] = net; | |
911 | } | |
912 | ||
913 | return 0; | |
914 | ||
915 | err: | |
916 | unregister_lte_device(phy_dev); | |
917 | ||
918 | return ret; | |
919 | } | |
920 | ||
921 | void unregister_lte_device(struct phy_dev *phy_dev) | |
922 | { | |
923 | struct net_device *net; | |
924 | int index; | |
925 | ||
926 | for (index = 0; index < MAX_NIC_TYPE; index++) { | |
927 | net = phy_dev->dev[index]; | |
b6f6fd8a | 928 | if (!net) |
61e12104 WK |
929 | continue; |
930 | ||
931 | unregister_netdev(net); | |
932 | free_netdev(net); | |
933 | } | |
934 | } |