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