1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */ |
3 | |
4 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
5 | |
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> |
14 | #include <linux/in6.h> |
15 | #include <linux/tcp.h> |
16 | #include <linux/icmp.h> |
17 | #include <linux/icmpv6.h> |
18 | #include <linux/uaccess.h> |
19 | #include <linux/errno.h> |
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" |
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 | |
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 const 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(dev: nic->netdev)) |
74 | netif_wake_queue(dev: nic->netdev); |
75 | } |
76 | |
77 | static int gdm_lte_rx(struct sk_buff *skb, struct nic *nic, int nic_type) |
78 | { |
79 | int ret, len; |
80 | |
81 | len = skb->len + ETH_HLEN; |
82 | ret = netif_rx(skb); |
83 | if (ret == NET_RX_DROP) { |
84 | nic->stats.rx_dropped++; |
85 | } else { |
86 | nic->stats.rx_packets++; |
87 | nic->stats.rx_bytes += len; |
88 | } |
89 | |
90 | return 0; |
91 | } |
92 | |
93 | static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type) |
94 | { |
95 | struct nic *nic = netdev_priv(dev: skb_in->dev); |
96 | struct sk_buff *skb_out; |
97 | struct ethhdr eth; |
98 | struct vlan_ethhdr vlan_eth; |
99 | struct arphdr *arp_in; |
100 | struct arphdr *arp_out; |
101 | struct arpdata { |
102 | u8 ar_sha[ETH_ALEN]; |
103 | u8 ar_sip[4]; |
104 | u8 ar_tha[ETH_ALEN]; |
105 | u8 ar_tip[4]; |
106 | }; |
107 | struct arpdata *arp_data_in; |
108 | struct arpdata *arp_data_out; |
109 | u8 arp_temp[60]; |
110 | void *; |
111 | u32 ; |
112 | |
113 | /* Check for skb->len, discard if empty */ |
114 | if (skb_in->len == 0) |
115 | return -ENODATA; |
116 | |
117 | /* Format the mac header so that it can be put to skb */ |
118 | if (ntohs(((struct ethhdr *)skb_in->data)->h_proto) == ETH_P_8021Q) { |
119 | memcpy(&vlan_eth, skb_in->data, sizeof(struct vlan_ethhdr)); |
120 | mac_header_data = &vlan_eth; |
121 | mac_header_len = VLAN_ETH_HLEN; |
122 | } else { |
123 | memcpy(ð, skb_in->data, sizeof(struct ethhdr)); |
124 | mac_header_data = ð |
125 | mac_header_len = ETH_HLEN; |
126 | } |
127 | |
128 | /* Get the pointer of the original request */ |
129 | arp_in = (struct arphdr *)(skb_in->data + mac_header_len); |
130 | arp_data_in = (struct arpdata *)(skb_in->data + mac_header_len + |
131 | sizeof(struct arphdr)); |
132 | |
133 | /* Get the pointer of the outgoing response */ |
134 | arp_out = (struct arphdr *)arp_temp; |
135 | arp_data_out = (struct arpdata *)(arp_temp + sizeof(struct arphdr)); |
136 | |
137 | /* Copy the arp header */ |
138 | memcpy(arp_out, arp_in, sizeof(struct arphdr)); |
139 | arp_out->ar_op = htons(ARPOP_REPLY); |
140 | |
141 | /* Copy the arp payload: based on 2 bytes of mac and fill the IP */ |
142 | arp_data_out->ar_sha[0] = arp_data_in->ar_sha[0]; |
143 | arp_data_out->ar_sha[1] = arp_data_in->ar_sha[1]; |
144 | memcpy(&arp_data_out->ar_sha[2], &arp_data_in->ar_tip[0], 4); |
145 | memcpy(&arp_data_out->ar_sip[0], &arp_data_in->ar_tip[0], 4); |
146 | memcpy(&arp_data_out->ar_tha[0], &arp_data_in->ar_sha[0], 6); |
147 | memcpy(&arp_data_out->ar_tip[0], &arp_data_in->ar_sip[0], 4); |
148 | |
149 | /* Fill the destination mac with source mac of the received packet */ |
150 | memcpy(mac_header_data, mac_header_data + ETH_ALEN, ETH_ALEN); |
151 | /* Fill the source mac with nic's source mac */ |
152 | memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN); |
153 | |
154 | /* Alloc skb and reserve align */ |
155 | skb_out = dev_alloc_skb(length: skb_in->len); |
156 | if (!skb_out) |
157 | return -ENOMEM; |
158 | skb_reserve(skb: skb_out, NET_IP_ALIGN); |
159 | |
160 | skb_put_data(skb: skb_out, data: mac_header_data, len: mac_header_len); |
161 | skb_put_data(skb: skb_out, data: arp_out, len: sizeof(struct arphdr)); |
162 | skb_put_data(skb: skb_out, data: arp_data_out, len: sizeof(struct arpdata)); |
163 | |
164 | skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto; |
165 | skb_out->dev = skb_in->dev; |
166 | skb_reset_mac_header(skb: skb_out); |
167 | skb_pull(skb: skb_out, ETH_HLEN); |
168 | |
169 | gdm_lte_rx(skb: skb_out, nic, nic_type); |
170 | |
171 | return 0; |
172 | } |
173 | |
174 | static __sum16 icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len) |
175 | { |
176 | unsigned short *w; |
177 | __wsum sum = 0; |
178 | int i; |
179 | u16 pa; |
180 | |
181 | union { |
182 | struct { |
183 | u8 ph_src[16]; |
184 | u8 ph_dst[16]; |
185 | u32 ph_len; |
186 | u8 ph_zero[3]; |
187 | u8 ph_nxt; |
188 | } ph __packed; |
189 | u16 pa[20]; |
190 | } ; |
191 | |
192 | memset(&pseudo_header, 0, sizeof(pseudo_header)); |
193 | memcpy(&pseudo_header.ph.ph_src, &ipv6->saddr.in6_u.u6_addr8, 16); |
194 | memcpy(&pseudo_header.ph.ph_dst, &ipv6->daddr.in6_u.u6_addr8, 16); |
195 | pseudo_header.ph.ph_len = be16_to_cpu(ipv6->payload_len); |
196 | pseudo_header.ph.ph_nxt = ipv6->nexthdr; |
197 | |
198 | for (i = 0; i < ARRAY_SIZE(pseudo_header.pa); i++) { |
199 | pa = pseudo_header.pa[i]; |
200 | sum = csum_add(csum: sum, addend: csum_unfold(n: (__force __sum16)pa)); |
201 | } |
202 | |
203 | w = ptr; |
204 | while (len > 1) { |
205 | sum = csum_add(csum: sum, addend: csum_unfold(n: (__force __sum16)*w++)); |
206 | len -= 2; |
207 | } |
208 | |
209 | return csum_fold(sum); |
210 | } |
211 | |
212 | static int gdm_lte_emulate_ndp(struct sk_buff *skb_in, u32 nic_type) |
213 | { |
214 | struct nic *nic = netdev_priv(dev: 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 *; |
235 | u32 ; |
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) |
241 | return -EPROTONOSUPPORT; |
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) |
247 | return -EPROTONOSUPPORT; |
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) |
255 | return -EPROTONOSUPPORT; |
256 | |
257 | /* Check if this is NDP packet */ |
258 | icmp6_in = (struct icmp6hdr *)(skb_in->data + mac_header_len + |
259 | sizeof(struct ipv6hdr)); |
260 | if (icmp6_in->icmp6_type == NDISC_ROUTER_SOLICITATION) { /* Check RS */ |
261 | return -EPROTONOSUPPORT; |
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)]; |
266 | u8 zero_addr8[16] = {0,}; |
267 | |
268 | if (memcmp(p: ipv6_in->saddr.in6_u.u6_addr8, q: zero_addr8, size: 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; |
275 | /* R=0, S=1, O=1 */ |
276 | icmp6_out.icmp6_dataun.un_data32[0] = htonl(0x60000000); |
277 | |
278 | ns = (struct neighbour_solicitation *) |
279 | (skb_in->data + mac_header_len + |
280 | sizeof(struct ipv6hdr) + sizeof(struct icmp6hdr)); |
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); |
293 | memcpy(ipv6_out.daddr.in6_u.u6_addr8, |
294 | ipv6_in->saddr.in6_u.u6_addr8, 16); |
295 | ipv6_out.payload_len = htons(sizeof(struct icmp6hdr) + |
296 | sizeof(struct neighbour_advertisement)); |
297 | |
298 | memcpy(icmp_na, &icmp6_out, sizeof(struct icmp6hdr)); |
299 | memcpy(icmp_na + sizeof(struct icmp6hdr), &na, |
300 | sizeof(struct neighbour_advertisement)); |
301 | |
302 | icmp6_out.icmp6_cksum = icmp6_checksum(ipv6: &ipv6_out, |
303 | ptr: (u16 *)icmp_na, |
304 | len: sizeof(icmp_na)); |
305 | } else { |
306 | return -EINVAL; |
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(length: skb_in->len); |
316 | if (!skb_out) |
317 | return -ENOMEM; |
318 | skb_reserve(skb: skb_out, NET_IP_ALIGN); |
319 | |
320 | skb_put_data(skb: skb_out, data: mac_header_data, len: mac_header_len); |
321 | skb_put_data(skb: skb_out, data: &ipv6_out, len: sizeof(struct ipv6hdr)); |
322 | skb_put_data(skb: skb_out, data: &icmp6_out, len: sizeof(struct icmp6hdr)); |
323 | skb_put_data(skb: skb_out, data: &na, len: sizeof(struct neighbour_advertisement)); |
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: skb_out); |
328 | skb_pull(skb: skb_out, ETH_HLEN); |
329 | |
330 | gdm_lte_rx(skb: 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; |
344 | u32 nic_type; |
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 = skb_vlan_eth_hdr(skb); |
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; |
368 | ip = network_data; |
369 | |
370 | /* Check DHCPv4 */ |
371 | if (ip->protocol == IPPROTO_UDP) { |
372 | struct udphdr *udp = |
373 | network_data + sizeof(struct iphdr); |
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; |
380 | ipv6 = network_data; |
381 | |
382 | if (ipv6->nexthdr == IPPROTO_ICMPV6) /* Check NDP request */ { |
383 | struct icmp6hdr *icmp6 = |
384 | network_data + sizeof(struct ipv6hdr); |
385 | if (icmp6->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) |
386 | nic_type |= NIC_TYPE_ICMPV6; |
387 | } else if (ipv6->nexthdr == IPPROTO_UDP) /* Check DHCPv6 */ { |
388 | struct udphdr *udp = |
389 | network_data + sizeof(struct ipv6hdr); |
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 | |
401 | static netdev_tx_t gdm_lte_tx(struct sk_buff *skb, struct net_device *dev) |
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) { |
412 | netdev_err(dev, format: "tx - invalid nic_type\n" ); |
413 | return -EMEDIUMTYPE; |
414 | } |
415 | |
416 | if (nic_type & NIC_TYPE_ARP) { |
417 | if (gdm_lte_emulate_arp(skb_in: 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_in: skb, nic_type) == 0) { |
425 | dev_kfree_skb(skb); |
426 | return 0; |
427 | } |
428 | } |
429 | |
430 | /* |
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 | */ |
437 | if (nic_type & NIC_TYPE_F_VLAN) { |
438 | struct vlan_ethhdr *vlan_eth = skb_vlan_eth_hdr(skb); |
439 | |
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 | |
449 | /* If it is a ICMPV6 packet, clear all the other bits : |
450 | * for backward compatibility with the firmware |
451 | */ |
452 | if (nic_type & NIC_TYPE_ICMPV6) |
453 | nic_type = NIC_TYPE_ICMPV6; |
454 | |
455 | /* If it is not a dhcp packet, clear all the flag bits : |
456 | * original NIC, otherwise the special flag (IPVX | DHCP) |
457 | */ |
458 | if (!(nic_type & NIC_TYPE_F_DHCP)) |
459 | nic_type &= NIC_TYPE_MASK; |
460 | |
461 | ret = sscanf(dev->name, "lte%d" , &idx); |
462 | if (ret != 1) { |
463 | dev_kfree_skb(skb); |
464 | return -EINVAL; |
465 | } |
466 | |
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); |
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); |
498 | |
499 | return &nic->stats; |
500 | } |
501 | |
502 | static int gdm_lte_event_send(struct net_device *dev, char *buf, int len) |
503 | { |
504 | struct phy_dev *phy_dev = ((struct nic *)netdev_priv(dev))->phy_dev; |
505 | struct hci_packet *hci = (struct hci_packet *)buf; |
506 | int length; |
507 | int idx; |
508 | int ret; |
509 | |
510 | ret = sscanf(dev->name, "lte%d" , &idx); |
511 | if (ret != 1) |
512 | return -EINVAL; |
513 | |
514 | length = gdm_dev16_to_cpu(dev_ed: phy_dev->get_endian(phy_dev->priv_dev), |
515 | x: hci->len) + HCI_HEADER_SIZE; |
516 | return netlink_send(sock: lte_event.sock, group: idx, type: 0, msg: buf, len: length, dev); |
517 | } |
518 | |
519 | static void gdm_lte_event_rcv(struct net_device *dev, u16 type, |
520 | void *msg, int len) |
521 | { |
522 | struct nic *nic = netdev_priv(dev); |
523 | |
524 | nic->phy_dev->send_hci_func(nic->phy_dev->priv_dev, msg, len, NULL, |
525 | NULL); |
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, cb: gdm_lte_event_rcv); |
532 | |
533 | if (lte_event.sock) { |
534 | lte_event.ref_cnt++; |
535 | return 0; |
536 | } |
537 | |
538 | pr_err("event init failed\n" ); |
539 | return -ENODATA; |
540 | } |
541 | |
542 | void gdm_lte_event_exit(void) |
543 | { |
544 | if (lte_event.sock && --lte_event.ref_cnt == 0) { |
545 | sock_release(sock: lte_event.sock->sk_socket); |
546 | lte_event.sock = NULL; |
547 | } |
548 | } |
549 | |
550 | static int find_dev_index(u32 nic_type) |
551 | { |
552 | u8 index; |
553 | |
554 | index = (u8)(nic_type & 0x0000000f); |
555 | if (index >= MAX_NIC_TYPE) |
556 | return -EINVAL; |
557 | |
558 | return index; |
559 | } |
560 | |
561 | static void gdm_lte_netif_rx(struct net_device *dev, char *buf, |
562 | int len, int flagged_nic_type) |
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 *; |
570 | u32 ; |
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) { |
577 | /* Change the destination mac address |
578 | * with the one requested the IP |
579 | */ |
580 | if (flagged_nic_type & NIC_TYPE_F_IPV4) { |
581 | struct dhcp_packet { |
582 | u8 op; /* BOOTREQUEST or BOOTREPLY */ |
583 | u8 htype; /* hardware address type. |
584 | * 1 = 10mb ethernet |
585 | */ |
586 | u8 hlen; /* hardware address length */ |
587 | u8 hops; /* used by relay agents only */ |
588 | u32 xid; /* unique id */ |
589 | u16 secs; /* elapsed since client began |
590 | * acquisition/renewal |
591 | */ |
592 | u16 flags; /* only one flag so far: */ |
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 | */ |
598 | u32 yiaddr; /* 'your' (client) IP address */ |
599 | /* IP address of next server to use in |
600 | * bootstrap, returned in DHCPOFFER, |
601 | * DHCPACK by server |
602 | */ |
603 | u32 siaddr_nip; |
604 | u32 gateway_nip; /* relay agent IP address */ |
605 | u8 chaddr[16]; /* link-layer client hardware |
606 | * address (MAC) |
607 | */ |
608 | u8 sname[64]; /* server host name (ASCIZ) */ |
609 | u8 file[128]; /* boot file name (ASCIZ) */ |
610 | u32 cookie; /* fixed first four option |
611 | * bytes (99,130,83,99 dec) |
612 | */ |
613 | } __packed; |
614 | int offset = sizeof(struct iphdr) + |
615 | sizeof(struct udphdr) + |
616 | offsetof(struct dhcp_packet, chaddr); |
617 | if (offset + ETH_ALEN > len) |
618 | return; |
619 | ether_addr_copy(dst: nic->dest_mac_addr, src: buf + offset); |
620 | } |
621 | } |
622 | |
623 | if (nic->vlan_id > 0) { |
624 | mac_header_data = (void *)&vlan_eth; |
625 | mac_header_len = VLAN_ETH_HLEN; |
626 | } else { |
627 | mac_header_data = (void *)ð |
628 | mac_header_len = ETH_HLEN; |
629 | } |
630 | |
631 | /* Format the data so that it can be put to skb */ |
632 | ether_addr_copy(dst: mac_header_data, src: nic->dest_mac_addr); |
633 | memcpy(mac_header_data + ETH_ALEN, nic->src_mac_addr, ETH_ALEN); |
634 | |
635 | vlan_eth.h_vlan_TCI = htons(nic->vlan_id); |
636 | vlan_eth.h_vlan_proto = htons(ETH_P_8021Q); |
637 | |
638 | if (nic_type == NIC_TYPE_ARP) { |
639 | /* Should be response: Only happens because |
640 | * there was a request from the host |
641 | */ |
642 | eth.h_proto = htons(ETH_P_ARP); |
643 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_ARP); |
644 | } else { |
645 | ip_version = buf[0] >> 4; |
646 | if (ip_version == IP_VERSION_4) { |
647 | eth.h_proto = htons(ETH_P_IP); |
648 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IP); |
649 | } else if (ip_version == IP_VERSION_6) { |
650 | eth.h_proto = htons(ETH_P_IPV6); |
651 | vlan_eth.h_vlan_encapsulated_proto = htons(ETH_P_IPV6); |
652 | } else { |
653 | netdev_err(dev, format: "Unknown IP version %d\n" , ip_version); |
654 | return; |
655 | } |
656 | } |
657 | |
658 | /* Alloc skb and reserve align */ |
659 | skb = dev_alloc_skb(length: len + mac_header_len + NET_IP_ALIGN); |
660 | if (!skb) |
661 | return; |
662 | skb_reserve(skb, NET_IP_ALIGN); |
663 | |
664 | skb_put_data(skb, data: mac_header_data, len: mac_header_len); |
665 | skb_put_data(skb, data: buf, len); |
666 | |
667 | skb->protocol = ((struct ethhdr *)mac_header_data)->h_proto; |
668 | skb->dev = dev; |
669 | skb_reset_mac_header(skb); |
670 | skb_pull(skb, ETH_HLEN); |
671 | |
672 | gdm_lte_rx(skb, nic, nic_type); |
673 | } |
674 | |
675 | static void gdm_lte_multi_sdu_pkt(struct phy_dev *phy_dev, char *buf, int len) |
676 | { |
677 | struct net_device *dev; |
678 | struct multi_sdu *multi_sdu = (struct multi_sdu *)buf; |
679 | struct sdu *sdu = NULL; |
680 | u8 endian = phy_dev->get_endian(phy_dev->priv_dev); |
681 | u8 *data = (u8 *)multi_sdu->data; |
682 | int copied; |
683 | u16 i = 0; |
684 | u16 num_packet; |
685 | u16 hci_len; |
686 | u16 cmd_evt; |
687 | u32 nic_type; |
688 | int index; |
689 | |
690 | num_packet = gdm_dev16_to_cpu(dev_ed: endian, x: multi_sdu->num_packet); |
691 | |
692 | for (i = 0; i < num_packet; i++) { |
693 | copied = data - multi_sdu->data; |
694 | if (len < copied + sizeof(*sdu)) { |
695 | pr_err("rx prevent buffer overflow" ); |
696 | return; |
697 | } |
698 | |
699 | sdu = (struct sdu *)data; |
700 | |
701 | cmd_evt = gdm_dev16_to_cpu(dev_ed: endian, x: sdu->cmd_evt); |
702 | hci_len = gdm_dev16_to_cpu(dev_ed: endian, x: sdu->len); |
703 | nic_type = gdm_dev32_to_cpu(dev_ed: endian, x: sdu->nic_type); |
704 | |
705 | if (cmd_evt != LTE_RX_SDU) { |
706 | pr_err("rx sdu wrong hci %04x\n" , cmd_evt); |
707 | return; |
708 | } |
709 | if (hci_len < 12 || |
710 | len < copied + sizeof(*sdu) + (hci_len - 12)) { |
711 | pr_err("rx sdu invalid len %d\n" , hci_len); |
712 | return; |
713 | } |
714 | |
715 | index = find_dev_index(nic_type); |
716 | if (index < 0) { |
717 | pr_err("rx sdu invalid nic_type :%x\n" , nic_type); |
718 | return; |
719 | } |
720 | dev = phy_dev->dev[index]; |
721 | gdm_lte_netif_rx(dev, buf: (char *)sdu->data, |
722 | len: (int)(hci_len - 12), flagged_nic_type: nic_type); |
723 | |
724 | data += ((hci_len + 3) & 0xfffc) + HCI_HEADER_SIZE; |
725 | } |
726 | } |
727 | |
728 | static void gdm_lte_pdn_table(struct net_device *dev, char *buf, int len) |
729 | { |
730 | struct nic *nic = netdev_priv(dev); |
731 | struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf; |
732 | u8 ed = nic->phy_dev->get_endian(nic->phy_dev->priv_dev); |
733 | |
734 | if (!pdn_table->activate) { |
735 | memset(&nic->pdn_table, 0x00, sizeof(struct pdn_table)); |
736 | netdev_info(dev, format: "pdn deactivated\n" ); |
737 | |
738 | return; |
739 | } |
740 | |
741 | nic->pdn_table.activate = pdn_table->activate; |
742 | nic->pdn_table.dft_eps_id = gdm_dev32_to_cpu(dev_ed: ed, x: pdn_table->dft_eps_id); |
743 | nic->pdn_table.nic_type = gdm_dev32_to_cpu(dev_ed: ed, x: pdn_table->nic_type); |
744 | |
745 | netdev_info(dev, format: "pdn activated, nic_type=0x%x\n" , |
746 | nic->pdn_table.nic_type); |
747 | } |
748 | |
749 | static int gdm_lte_receive_pkt(struct phy_dev *phy_dev, char *buf, int len) |
750 | { |
751 | struct hci_packet *hci = (struct hci_packet *)buf; |
752 | struct hci_pdn_table_ind *pdn_table = (struct hci_pdn_table_ind *)buf; |
753 | struct sdu *sdu; |
754 | struct net_device *dev; |
755 | u8 endian = phy_dev->get_endian(phy_dev->priv_dev); |
756 | int ret = 0; |
757 | u16 cmd_evt; |
758 | u32 nic_type; |
759 | int index; |
760 | |
761 | if (!len) |
762 | return ret; |
763 | |
764 | cmd_evt = gdm_dev16_to_cpu(dev_ed: endian, x: hci->cmd_evt); |
765 | |
766 | dev = phy_dev->dev[0]; |
767 | if (!dev) |
768 | return 0; |
769 | |
770 | switch (cmd_evt) { |
771 | case LTE_RX_SDU: |
772 | sdu = (struct sdu *)hci->data; |
773 | nic_type = gdm_dev32_to_cpu(dev_ed: endian, x: sdu->nic_type); |
774 | index = find_dev_index(nic_type); |
775 | if (index < 0) |
776 | return index; |
777 | dev = phy_dev->dev[index]; |
778 | gdm_lte_netif_rx(dev, buf: hci->data, len, flagged_nic_type: nic_type); |
779 | break; |
780 | case LTE_RX_MULTI_SDU: |
781 | gdm_lte_multi_sdu_pkt(phy_dev, buf, len); |
782 | break; |
783 | case LTE_LINK_ON_OFF_INDICATION: |
784 | netdev_info(dev, format: "link %s\n" , |
785 | ((struct hci_connect_ind *)buf)->connect |
786 | ? "on" : "off" ); |
787 | break; |
788 | case LTE_PDN_TABLE_IND: |
789 | pdn_table = (struct hci_pdn_table_ind *)buf; |
790 | nic_type = gdm_dev32_to_cpu(dev_ed: endian, x: pdn_table->nic_type); |
791 | index = find_dev_index(nic_type); |
792 | if (index < 0) |
793 | return index; |
794 | dev = phy_dev->dev[index]; |
795 | gdm_lte_pdn_table(dev, buf, len); |
796 | fallthrough; |
797 | default: |
798 | ret = gdm_lte_event_send(dev, buf, len); |
799 | break; |
800 | } |
801 | |
802 | return ret; |
803 | } |
804 | |
805 | static int rx_complete(void *arg, void *data, int len, int context) |
806 | { |
807 | struct phy_dev *phy_dev = arg; |
808 | |
809 | return gdm_lte_receive_pkt(phy_dev, buf: data, len); |
810 | } |
811 | |
812 | void start_rx_proc(struct phy_dev *phy_dev) |
813 | { |
814 | int i; |
815 | |
816 | for (i = 0; i < MAX_RX_SUBMIT_COUNT; i++) |
817 | phy_dev->rcv_func(phy_dev->priv_dev, |
818 | rx_complete, phy_dev, USB_COMPLETE); |
819 | } |
820 | |
821 | static const struct net_device_ops gdm_netdev_ops = { |
822 | .ndo_open = gdm_lte_open, |
823 | .ndo_stop = gdm_lte_close, |
824 | .ndo_set_config = gdm_lte_set_config, |
825 | .ndo_start_xmit = gdm_lte_tx, |
826 | .ndo_get_stats = gdm_lte_stats, |
827 | }; |
828 | |
829 | static u8 gdm_lte_macaddr[ETH_ALEN] = {0x00, 0x0a, 0x3b, 0x00, 0x00, 0x00}; |
830 | |
831 | static void form_mac_address(u8 *dev_addr, u8 *nic_src, u8 *nic_dest, |
832 | u8 *mac_address, u8 index) |
833 | { |
834 | /* Form the dev_addr */ |
835 | if (!mac_address) |
836 | ether_addr_copy(dst: dev_addr, src: gdm_lte_macaddr); |
837 | else |
838 | ether_addr_copy(dst: dev_addr, src: mac_address); |
839 | |
840 | /* The last byte of the mac address |
841 | * should be less than or equal to 0xFC |
842 | */ |
843 | dev_addr[ETH_ALEN - 1] += index; |
844 | |
845 | /* Create random nic src and copy the first |
846 | * 3 bytes to be the same as dev_addr |
847 | */ |
848 | eth_random_addr(addr: nic_src); |
849 | memcpy(nic_src, dev_addr, 3); |
850 | |
851 | /* Copy the nic_dest from dev_addr*/ |
852 | ether_addr_copy(dst: nic_dest, src: dev_addr); |
853 | } |
854 | |
855 | static void validate_mac_address(u8 *mac_address) |
856 | { |
857 | /* if zero address or multicast bit set, restore the default value */ |
858 | if (is_zero_ether_addr(addr: mac_address) || (mac_address[0] & 0x01)) { |
859 | pr_err("MAC invalid, restoring default\n" ); |
860 | memcpy(mac_address, gdm_lte_macaddr, 6); |
861 | } |
862 | } |
863 | |
864 | int register_lte_device(struct phy_dev *phy_dev, |
865 | struct device *dev, u8 *mac_address) |
866 | { |
867 | struct nic *nic; |
868 | struct net_device *net; |
869 | char pdn_dev_name[16]; |
870 | u8 addr[ETH_ALEN]; |
871 | int ret = 0; |
872 | u8 index; |
873 | |
874 | validate_mac_address(mac_address); |
875 | |
876 | for (index = 0; index < MAX_NIC_TYPE; index++) { |
877 | /* Create device name lteXpdnX */ |
878 | sprintf(buf: pdn_dev_name, fmt: "lte%%dpdn%d" , index); |
879 | |
880 | /* Allocate netdev */ |
881 | net = alloc_netdev(sizeof(struct nic), pdn_dev_name, |
882 | NET_NAME_UNKNOWN, ether_setup); |
883 | if (!net) { |
884 | ret = -ENOMEM; |
885 | goto err; |
886 | } |
887 | net->netdev_ops = &gdm_netdev_ops; |
888 | net->flags &= ~IFF_MULTICAST; |
889 | net->mtu = DEFAULT_MTU_SIZE; |
890 | |
891 | nic = netdev_priv(dev: net); |
892 | memset(nic, 0, sizeof(struct nic)); |
893 | nic->netdev = net; |
894 | nic->phy_dev = phy_dev; |
895 | nic->nic_id = index; |
896 | |
897 | form_mac_address(dev_addr: addr, |
898 | nic_src: nic->src_mac_addr, |
899 | nic_dest: nic->dest_mac_addr, |
900 | mac_address, |
901 | index); |
902 | eth_hw_addr_set(dev: net, addr); |
903 | |
904 | SET_NETDEV_DEV(net, dev); |
905 | SET_NETDEV_DEVTYPE(net, &wwan_type); |
906 | |
907 | ret = register_netdev(dev: net); |
908 | if (ret) |
909 | goto err; |
910 | |
911 | netif_carrier_on(dev: net); |
912 | |
913 | phy_dev->dev[index] = net; |
914 | } |
915 | |
916 | return 0; |
917 | |
918 | err: |
919 | unregister_lte_device(phy_dev); |
920 | |
921 | return ret; |
922 | } |
923 | |
924 | void unregister_lte_device(struct phy_dev *phy_dev) |
925 | { |
926 | struct net_device *net; |
927 | int index; |
928 | |
929 | for (index = 0; index < MAX_NIC_TYPE; index++) { |
930 | net = phy_dev->dev[index]; |
931 | if (!net) |
932 | continue; |
933 | |
934 | unregister_netdev(dev: net); |
935 | free_netdev(dev: net); |
936 | } |
937 | } |
938 | |