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