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