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