1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* MHI Network driver - Network over MHI bus
3 *
4 * Copyright (C) 2021 Linaro Ltd <loic.poulain@linaro.org>
5 *
6 * This driver copy some code from cdc_ncm, which is:
7 * Copyright (C) ST-Ericsson 2010-2012
8 * and cdc_mbim, which is:
9 * Copyright (c) 2012 Smith Micro Software, Inc.
10 * Copyright (c) 2012 Bjørn Mork <bjorn@mork.no>
11 *
12 */
13
14 #include <linux/ethtool.h>
15 #include <linux/if_vlan.h>
16 #include <linux/ip.h>
17 #include <linux/mii.h>
18 #include <linux/netdevice.h>
19 #include <linux/skbuff.h>
20 #include <linux/usb.h>
21 #include <linux/usb/cdc.h>
22 #include <linux/usb/usbnet.h>
23 #include <linux/usb/cdc_ncm.h>
24
25 #include "mhi.h"
26
27 #define MBIM_NDP16_SIGN_MASK 0x00ffffff
28
29 /* Usual WWAN MTU */
30 #define MHI_MBIM_DEFAULT_MTU 1500
31
32 /* 3500 allows to optimize skb allocation, the skbs will basically fit in
33 * one 4K page. Large MBIM packets will simply be split over several MHI
34 * transfers and chained by the MHI net layer (zerocopy).
35 */
36 #define MHI_MBIM_DEFAULT_MRU 3500
37
38 struct mbim_context {
39 u16 rx_seq;
40 u16 tx_seq;
41 };
42
__mbim_length_errors_inc(struct mhi_net_dev * dev)43 static void __mbim_length_errors_inc(struct mhi_net_dev *dev)
44 {
45 u64_stats_update_begin(&dev->stats.rx_syncp);
46 u64_stats_inc(&dev->stats.rx_length_errors);
47 u64_stats_update_end(&dev->stats.rx_syncp);
48 }
49
__mbim_errors_inc(struct mhi_net_dev * dev)50 static void __mbim_errors_inc(struct mhi_net_dev *dev)
51 {
52 u64_stats_update_begin(&dev->stats.rx_syncp);
53 u64_stats_inc(&dev->stats.rx_errors);
54 u64_stats_update_end(&dev->stats.rx_syncp);
55 }
56
mbim_rx_verify_nth16(struct sk_buff * skb)57 static int mbim_rx_verify_nth16(struct sk_buff *skb)
58 {
59 struct mhi_net_dev *dev = netdev_priv(skb->dev);
60 struct mbim_context *ctx = dev->proto_data;
61 struct usb_cdc_ncm_nth16 *nth16;
62 int len;
63
64 if (skb->len < sizeof(struct usb_cdc_ncm_nth16) +
65 sizeof(struct usb_cdc_ncm_ndp16)) {
66 netif_dbg(dev, rx_err, dev->ndev, "frame too short\n");
67 __mbim_length_errors_inc(dev);
68 return -EINVAL;
69 }
70
71 nth16 = (struct usb_cdc_ncm_nth16 *)skb->data;
72
73 if (nth16->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH16_SIGN)) {
74 netif_dbg(dev, rx_err, dev->ndev,
75 "invalid NTH16 signature <%#010x>\n",
76 le32_to_cpu(nth16->dwSignature));
77 __mbim_errors_inc(dev);
78 return -EINVAL;
79 }
80
81 /* No limit on the block length, except the size of the data pkt */
82 len = le16_to_cpu(nth16->wBlockLength);
83 if (len > skb->len) {
84 netif_dbg(dev, rx_err, dev->ndev,
85 "NTB does not fit into the skb %u/%u\n", len,
86 skb->len);
87 __mbim_length_errors_inc(dev);
88 return -EINVAL;
89 }
90
91 if (ctx->rx_seq + 1 != le16_to_cpu(nth16->wSequence) &&
92 (ctx->rx_seq || le16_to_cpu(nth16->wSequence)) &&
93 !(ctx->rx_seq == 0xffff && !le16_to_cpu(nth16->wSequence))) {
94 netif_dbg(dev, rx_err, dev->ndev,
95 "sequence number glitch prev=%d curr=%d\n",
96 ctx->rx_seq, le16_to_cpu(nth16->wSequence));
97 }
98 ctx->rx_seq = le16_to_cpu(nth16->wSequence);
99
100 return le16_to_cpu(nth16->wNdpIndex);
101 }
102
mbim_rx_verify_ndp16(struct sk_buff * skb,struct usb_cdc_ncm_ndp16 * ndp16)103 static int mbim_rx_verify_ndp16(struct sk_buff *skb, struct usb_cdc_ncm_ndp16 *ndp16)
104 {
105 struct mhi_net_dev *dev = netdev_priv(skb->dev);
106 int ret;
107
108 if (le16_to_cpu(ndp16->wLength) < USB_CDC_NCM_NDP16_LENGTH_MIN) {
109 netif_dbg(dev, rx_err, dev->ndev, "invalid DPT16 length <%u>\n",
110 le16_to_cpu(ndp16->wLength));
111 return -EINVAL;
112 }
113
114 ret = ((le16_to_cpu(ndp16->wLength) - sizeof(struct usb_cdc_ncm_ndp16))
115 / sizeof(struct usb_cdc_ncm_dpe16));
116 ret--; /* Last entry is always a NULL terminator */
117
118 if (sizeof(struct usb_cdc_ncm_ndp16) +
119 ret * sizeof(struct usb_cdc_ncm_dpe16) > skb->len) {
120 netif_dbg(dev, rx_err, dev->ndev,
121 "Invalid nframes = %d\n", ret);
122 return -EINVAL;
123 }
124
125 return ret;
126 }
127
mbim_rx(struct mhi_net_dev * mhi_netdev,struct sk_buff * skb)128 static void mbim_rx(struct mhi_net_dev *mhi_netdev, struct sk_buff *skb)
129 {
130 struct net_device *ndev = mhi_netdev->ndev;
131 int ndpoffset;
132
133 /* Check NTB header and retrieve first NDP offset */
134 ndpoffset = mbim_rx_verify_nth16(skb);
135 if (ndpoffset < 0) {
136 net_err_ratelimited("%s: Incorrect NTB header\n", ndev->name);
137 goto error;
138 }
139
140 /* Process each NDP */
141 while (1) {
142 struct usb_cdc_ncm_ndp16 ndp16;
143 struct usb_cdc_ncm_dpe16 dpe16;
144 int nframes, n, dpeoffset;
145
146 if (skb_copy_bits(skb, ndpoffset, &ndp16, sizeof(ndp16))) {
147 net_err_ratelimited("%s: Incorrect NDP offset (%u)\n",
148 ndev->name, ndpoffset);
149 __mbim_length_errors_inc(mhi_netdev);
150 goto error;
151 }
152
153 /* Check NDP header and retrieve number of datagrams */
154 nframes = mbim_rx_verify_ndp16(skb, &ndp16);
155 if (nframes < 0) {
156 net_err_ratelimited("%s: Incorrect NDP16\n", ndev->name);
157 __mbim_length_errors_inc(mhi_netdev);
158 goto error;
159 }
160
161 /* Only IP data type supported, no DSS in MHI context */
162 if ((ndp16.dwSignature & cpu_to_le32(MBIM_NDP16_SIGN_MASK))
163 != cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN)) {
164 net_err_ratelimited("%s: Unsupported NDP type\n", ndev->name);
165 __mbim_errors_inc(mhi_netdev);
166 goto next_ndp;
167 }
168
169 /* Only primary IP session 0 (0x00) supported for now */
170 if (ndp16.dwSignature & ~cpu_to_le32(MBIM_NDP16_SIGN_MASK)) {
171 net_err_ratelimited("%s: bad packet session\n", ndev->name);
172 __mbim_errors_inc(mhi_netdev);
173 goto next_ndp;
174 }
175
176 /* de-aggregate and deliver IP packets */
177 dpeoffset = ndpoffset + sizeof(struct usb_cdc_ncm_ndp16);
178 for (n = 0; n < nframes; n++, dpeoffset += sizeof(dpe16)) {
179 u16 dgram_offset, dgram_len;
180 struct sk_buff *skbn;
181
182 if (skb_copy_bits(skb, dpeoffset, &dpe16, sizeof(dpe16)))
183 break;
184
185 dgram_offset = le16_to_cpu(dpe16.wDatagramIndex);
186 dgram_len = le16_to_cpu(dpe16.wDatagramLength);
187
188 if (!dgram_offset || !dgram_len)
189 break; /* null terminator */
190
191 skbn = netdev_alloc_skb(ndev, dgram_len);
192 if (!skbn)
193 continue;
194
195 skb_put(skbn, dgram_len);
196 skb_copy_bits(skb, dgram_offset, skbn->data, dgram_len);
197
198 switch (skbn->data[0] & 0xf0) {
199 case 0x40:
200 skbn->protocol = htons(ETH_P_IP);
201 break;
202 case 0x60:
203 skbn->protocol = htons(ETH_P_IPV6);
204 break;
205 default:
206 net_err_ratelimited("%s: unknown protocol\n",
207 ndev->name);
208 __mbim_errors_inc(mhi_netdev);
209 dev_kfree_skb_any(skbn);
210 continue;
211 }
212
213 netif_rx(skbn);
214 }
215 next_ndp:
216 /* Other NDP to process? */
217 ndpoffset = (int)le16_to_cpu(ndp16.wNextNdpIndex);
218 if (!ndpoffset)
219 break;
220 }
221
222 /* free skb */
223 dev_consume_skb_any(skb);
224 return;
225 error:
226 dev_kfree_skb_any(skb);
227 }
228
229 struct mbim_tx_hdr {
230 struct usb_cdc_ncm_nth16 nth16;
231 struct usb_cdc_ncm_ndp16 ndp16;
232 struct usb_cdc_ncm_dpe16 dpe16[2];
233 } __packed;
234
mbim_tx_fixup(struct mhi_net_dev * mhi_netdev,struct sk_buff * skb)235 static struct sk_buff *mbim_tx_fixup(struct mhi_net_dev *mhi_netdev,
236 struct sk_buff *skb)
237 {
238 struct mbim_context *ctx = mhi_netdev->proto_data;
239 unsigned int dgram_size = skb->len;
240 struct usb_cdc_ncm_nth16 *nth16;
241 struct usb_cdc_ncm_ndp16 *ndp16;
242 struct mbim_tx_hdr *mbim_hdr;
243
244 /* For now, this is a partial implementation of CDC MBIM, only one NDP
245 * is sent, containing the IP packet (no aggregation).
246 */
247
248 /* Ensure we have enough headroom for crafting MBIM header */
249 if (skb_cow_head(skb, sizeof(struct mbim_tx_hdr))) {
250 dev_kfree_skb_any(skb);
251 return NULL;
252 }
253
254 mbim_hdr = skb_push(skb, sizeof(struct mbim_tx_hdr));
255
256 /* Fill NTB header */
257 nth16 = &mbim_hdr->nth16;
258 nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
259 nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
260 nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
261 nth16->wBlockLength = cpu_to_le16(skb->len);
262 nth16->wNdpIndex = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
263
264 /* Fill the unique NDP */
265 ndp16 = &mbim_hdr->ndp16;
266 ndp16->dwSignature = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
267 ndp16->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp16)
268 + sizeof(struct usb_cdc_ncm_dpe16) * 2);
269 ndp16->wNextNdpIndex = 0;
270
271 /* Datagram follows the mbim header */
272 ndp16->dpe16[0].wDatagramIndex = cpu_to_le16(sizeof(struct mbim_tx_hdr));
273 ndp16->dpe16[0].wDatagramLength = cpu_to_le16(dgram_size);
274
275 /* null termination */
276 ndp16->dpe16[1].wDatagramIndex = 0;
277 ndp16->dpe16[1].wDatagramLength = 0;
278
279 return skb;
280 }
281
mbim_init(struct mhi_net_dev * mhi_netdev)282 static int mbim_init(struct mhi_net_dev *mhi_netdev)
283 {
284 struct net_device *ndev = mhi_netdev->ndev;
285
286 mhi_netdev->proto_data = devm_kzalloc(&ndev->dev,
287 sizeof(struct mbim_context),
288 GFP_KERNEL);
289 if (!mhi_netdev->proto_data)
290 return -ENOMEM;
291
292 ndev->needed_headroom = sizeof(struct mbim_tx_hdr);
293 ndev->mtu = MHI_MBIM_DEFAULT_MTU;
294 mhi_netdev->mru = MHI_MBIM_DEFAULT_MRU;
295
296 return 0;
297 }
298
299 const struct mhi_net_proto proto_mbim = {
300 .init = mbim_init,
301 .rx = mbim_rx,
302 .tx_fixup = mbim_tx_fixup,
303 };
304