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