1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Implements DMTF specification |
4 | * "DSP0233 Management Component Transport Protocol (MCTP) I3C Transport |
5 | * Binding" |
6 | * https://www.dmtf.org/sites/default/files/standards/documents/DSP0233_1.0.0.pdf |
7 | * |
8 | * Copyright (c) 2023 Code Construct |
9 | */ |
10 | |
11 | #include <linux/module.h> |
12 | #include <linux/netdevice.h> |
13 | #include <linux/i3c/device.h> |
14 | #include <linux/i3c/master.h> |
15 | #include <linux/if_arp.h> |
16 | #include <asm/unaligned.h> |
17 | #include <net/mctp.h> |
18 | #include <net/mctpdevice.h> |
19 | |
20 | #define MCTP_I3C_MAXBUF 65536 |
21 | /* 48 bit Provisioned Id */ |
22 | #define PID_SIZE 6 |
23 | |
24 | /* 64 byte payload, 4 byte MCTP header */ |
25 | static const int MCTP_I3C_MINMTU = 64 + 4; |
26 | /* One byte less to allow for the PEC */ |
27 | static const int MCTP_I3C_MAXMTU = MCTP_I3C_MAXBUF - 1; |
28 | /* 4 byte MCTP header, no data, 1 byte PEC */ |
29 | static const int MCTP_I3C_MINLEN = 4 + 1; |
30 | |
31 | /* Sufficient for 64kB at min mtu */ |
32 | static const int MCTP_I3C_TX_QUEUE_LEN = 1100; |
33 | |
34 | /* Somewhat arbitrary */ |
35 | static const int MCTP_I3C_IBI_SLOTS = 8; |
36 | |
37 | /* Mandatory Data Byte in an IBI, from DSP0233 */ |
38 | #define I3C_MDB_MCTP 0xAE |
39 | /* From MIPI Device Characteristics Register (DCR) Assignments */ |
40 | #define I3C_DCR_MCTP 0xCC |
41 | |
42 | static const char *MCTP_I3C_OF_PROP = "mctp-controller" ; |
43 | |
44 | /* List of mctp_i3c_busdev */ |
45 | static LIST_HEAD(busdevs); |
46 | /* Protects busdevs, as well as mctp_i3c_bus.devs lists */ |
47 | static DEFINE_MUTEX(busdevs_lock); |
48 | |
49 | struct mctp_i3c_bus { |
50 | struct net_device *ndev; |
51 | |
52 | struct task_struct *tx_thread; |
53 | wait_queue_head_t tx_wq; |
54 | /* tx_lock protects tx_skb and devs */ |
55 | spinlock_t tx_lock; |
56 | /* Next skb to transmit */ |
57 | struct sk_buff *tx_skb; |
58 | /* Scratch buffer for xmit */ |
59 | u8 tx_scratch[MCTP_I3C_MAXBUF]; |
60 | |
61 | /* Element of busdevs */ |
62 | struct list_head list; |
63 | |
64 | /* Provisioned ID of our controller */ |
65 | u64 pid; |
66 | |
67 | struct i3c_bus *bus; |
68 | /* Head of mctp_i3c_device.list. Protected by busdevs_lock */ |
69 | struct list_head devs; |
70 | }; |
71 | |
72 | struct mctp_i3c_device { |
73 | struct i3c_device *i3c; |
74 | struct mctp_i3c_bus *mbus; |
75 | struct list_head list; /* Element of mctp_i3c_bus.devs */ |
76 | |
77 | /* Held while tx_thread is using this device */ |
78 | struct mutex lock; |
79 | |
80 | /* Whether BCR indicates MDB is present in IBI */ |
81 | bool have_mdb; |
82 | /* I3C dynamic address */ |
83 | u8 addr; |
84 | /* Maximum read length */ |
85 | u16 mrl; |
86 | /* Maximum write length */ |
87 | u16 mwl; |
88 | /* Provisioned ID */ |
89 | u64 pid; |
90 | }; |
91 | |
92 | /* We synthesise a mac header using the Provisioned ID. |
93 | * Used to pass dest to mctp_i3c_start_xmit. |
94 | */ |
95 | struct mctp_i3c_internal_hdr { |
96 | u8 dest[PID_SIZE]; |
97 | u8 source[PID_SIZE]; |
98 | } __packed; |
99 | |
100 | static int mctp_i3c_read(struct mctp_i3c_device *mi) |
101 | { |
102 | struct i3c_priv_xfer xfer = { .rnw = 1, .len = mi->mrl }; |
103 | struct net_device_stats *stats = &mi->mbus->ndev->stats; |
104 | struct mctp_i3c_internal_hdr *ihdr = NULL; |
105 | struct sk_buff *skb = NULL; |
106 | struct mctp_skb_cb *cb; |
107 | int net_status, rc; |
108 | u8 pec, addr; |
109 | |
110 | skb = netdev_alloc_skb(dev: mi->mbus->ndev, |
111 | length: mi->mrl + sizeof(struct mctp_i3c_internal_hdr)); |
112 | if (!skb) { |
113 | stats->rx_dropped++; |
114 | rc = -ENOMEM; |
115 | goto err; |
116 | } |
117 | |
118 | skb->protocol = htons(ETH_P_MCTP); |
119 | /* Create a header for internal use */ |
120 | skb_reset_mac_header(skb); |
121 | ihdr = skb_put(skb, len: sizeof(struct mctp_i3c_internal_hdr)); |
122 | put_unaligned_be48(val: mi->pid, p: ihdr->source); |
123 | put_unaligned_be48(val: mi->mbus->pid, p: ihdr->dest); |
124 | skb_pull(skb, len: sizeof(struct mctp_i3c_internal_hdr)); |
125 | |
126 | xfer.data.in = skb_put(skb, len: mi->mrl); |
127 | |
128 | rc = i3c_device_do_priv_xfers(dev: mi->i3c, xfers: &xfer, nxfers: 1); |
129 | if (rc < 0) |
130 | goto err; |
131 | |
132 | if (WARN_ON_ONCE(xfer.len > mi->mrl)) { |
133 | /* Bad i3c bus driver */ |
134 | rc = -EIO; |
135 | goto err; |
136 | } |
137 | if (xfer.len < MCTP_I3C_MINLEN) { |
138 | stats->rx_length_errors++; |
139 | rc = -EIO; |
140 | goto err; |
141 | } |
142 | |
143 | /* check PEC, including address byte */ |
144 | addr = mi->addr << 1 | 1; |
145 | pec = i2c_smbus_pec(crc: 0, p: &addr, count: 1); |
146 | pec = i2c_smbus_pec(crc: pec, p: xfer.data.in, count: xfer.len - 1); |
147 | if (pec != ((u8 *)xfer.data.in)[xfer.len - 1]) { |
148 | stats->rx_crc_errors++; |
149 | rc = -EINVAL; |
150 | goto err; |
151 | } |
152 | |
153 | /* Remove PEC */ |
154 | skb_trim(skb, len: xfer.len - 1); |
155 | |
156 | cb = __mctp_cb(skb); |
157 | cb->halen = PID_SIZE; |
158 | put_unaligned_be48(val: mi->pid, p: cb->haddr); |
159 | |
160 | net_status = netif_rx(skb); |
161 | |
162 | if (net_status == NET_RX_SUCCESS) { |
163 | stats->rx_packets++; |
164 | stats->rx_bytes += xfer.len - 1; |
165 | } else { |
166 | stats->rx_dropped++; |
167 | } |
168 | |
169 | return 0; |
170 | err: |
171 | kfree_skb(skb); |
172 | return rc; |
173 | } |
174 | |
175 | static void mctp_i3c_ibi_handler(struct i3c_device *i3c, |
176 | const struct i3c_ibi_payload *payload) |
177 | { |
178 | struct mctp_i3c_device *mi = i3cdev_get_drvdata(i3cdev: i3c); |
179 | |
180 | if (WARN_ON_ONCE(!mi)) |
181 | return; |
182 | |
183 | if (mi->have_mdb) { |
184 | if (payload->len > 0) { |
185 | if (((u8 *)payload->data)[0] != I3C_MDB_MCTP) { |
186 | /* Not a mctp-i3c interrupt, ignore it */ |
187 | return; |
188 | } |
189 | } else { |
190 | /* The BCR advertised a Mandatory Data Byte but the |
191 | * device didn't send one. |
192 | */ |
193 | dev_warn_once(i3cdev_to_dev(i3c), "IBI with missing MDB" ); |
194 | } |
195 | } |
196 | |
197 | mctp_i3c_read(mi); |
198 | } |
199 | |
200 | static int mctp_i3c_setup(struct mctp_i3c_device *mi) |
201 | { |
202 | const struct i3c_ibi_setup ibi = { |
203 | .max_payload_len = 1, |
204 | .num_slots = MCTP_I3C_IBI_SLOTS, |
205 | .handler = mctp_i3c_ibi_handler, |
206 | }; |
207 | struct i3c_device_info info; |
208 | int rc; |
209 | |
210 | i3c_device_get_info(dev: mi->i3c, info: &info); |
211 | mi->have_mdb = info.bcr & BIT(2); |
212 | mi->addr = info.dyn_addr; |
213 | mi->mwl = info.max_write_len; |
214 | mi->mrl = info.max_read_len; |
215 | mi->pid = info.pid; |
216 | |
217 | rc = i3c_device_request_ibi(dev: mi->i3c, setup: &ibi); |
218 | if (rc == -ENOTSUPP) { |
219 | /* This driver only supports In-Band Interrupt mode. |
220 | * Support for Polling Mode could be added if required. |
221 | * (ENOTSUPP is from the i3c layer, not EOPNOTSUPP). |
222 | */ |
223 | dev_warn(i3cdev_to_dev(mi->i3c), |
224 | "Failed, bus driver doesn't support In-Band Interrupts" ); |
225 | goto err; |
226 | } else if (rc < 0) { |
227 | dev_err(i3cdev_to_dev(mi->i3c), |
228 | "Failed requesting IBI (%d)\n" , rc); |
229 | goto err; |
230 | } |
231 | |
232 | rc = i3c_device_enable_ibi(dev: mi->i3c); |
233 | if (rc < 0) { |
234 | /* Assume a driver supporting request_ibi also |
235 | * supports enable_ibi. |
236 | */ |
237 | dev_err(i3cdev_to_dev(mi->i3c), "Failed enabling IBI (%d)\n" , rc); |
238 | goto err_free_ibi; |
239 | } |
240 | |
241 | return 0; |
242 | |
243 | err_free_ibi: |
244 | i3c_device_free_ibi(dev: mi->i3c); |
245 | |
246 | err: |
247 | return rc; |
248 | } |
249 | |
250 | /* Adds a new MCTP i3c_device to a bus */ |
251 | static int mctp_i3c_add_device(struct mctp_i3c_bus *mbus, |
252 | struct i3c_device *i3c) |
253 | __must_hold(&busdevs_lock) |
254 | { |
255 | struct mctp_i3c_device *mi = NULL; |
256 | int rc; |
257 | |
258 | mi = kzalloc(size: sizeof(*mi), GFP_KERNEL); |
259 | if (!mi) { |
260 | rc = -ENOMEM; |
261 | goto err; |
262 | } |
263 | mi->mbus = mbus; |
264 | mi->i3c = i3c; |
265 | mutex_init(&mi->lock); |
266 | list_add(new: &mi->list, head: &mbus->devs); |
267 | |
268 | i3cdev_set_drvdata(i3cdev: i3c, data: mi); |
269 | rc = mctp_i3c_setup(mi); |
270 | if (rc < 0) |
271 | goto err_free; |
272 | |
273 | return 0; |
274 | |
275 | err_free: |
276 | list_del(entry: &mi->list); |
277 | kfree(objp: mi); |
278 | |
279 | err: |
280 | dev_warn(i3cdev_to_dev(i3c), "Error adding mctp-i3c device, %d\n" , rc); |
281 | return rc; |
282 | } |
283 | |
284 | static int mctp_i3c_probe(struct i3c_device *i3c) |
285 | { |
286 | struct mctp_i3c_bus *b = NULL, *mbus = NULL; |
287 | |
288 | /* Look for a known bus */ |
289 | mutex_lock(&busdevs_lock); |
290 | list_for_each_entry(b, &busdevs, list) |
291 | if (b->bus == i3c->bus) { |
292 | mbus = b; |
293 | break; |
294 | } |
295 | mutex_unlock(lock: &busdevs_lock); |
296 | |
297 | if (!mbus) { |
298 | /* probably no "mctp-controller" property on the i3c bus */ |
299 | return -ENODEV; |
300 | } |
301 | |
302 | return mctp_i3c_add_device(mbus, i3c); |
303 | } |
304 | |
305 | static void mctp_i3c_remove_device(struct mctp_i3c_device *mi) |
306 | __must_hold(&busdevs_lock) |
307 | { |
308 | /* Ensure the tx thread isn't using the device */ |
309 | mutex_lock(&mi->lock); |
310 | |
311 | /* Counterpart of mctp_i3c_setup */ |
312 | i3c_device_disable_ibi(dev: mi->i3c); |
313 | i3c_device_free_ibi(dev: mi->i3c); |
314 | |
315 | /* Counterpart of mctp_i3c_add_device */ |
316 | i3cdev_set_drvdata(i3cdev: mi->i3c, NULL); |
317 | list_del(entry: &mi->list); |
318 | |
319 | /* Safe to unlock after removing from the list */ |
320 | mutex_unlock(lock: &mi->lock); |
321 | kfree(objp: mi); |
322 | } |
323 | |
324 | static void mctp_i3c_remove(struct i3c_device *i3c) |
325 | { |
326 | struct mctp_i3c_device *mi = i3cdev_get_drvdata(i3cdev: i3c); |
327 | |
328 | /* We my have received a Bus Remove notify prior to device remove, |
329 | * so mi will already be removed. |
330 | */ |
331 | if (!mi) |
332 | return; |
333 | |
334 | mutex_lock(&busdevs_lock); |
335 | mctp_i3c_remove_device(mi); |
336 | mutex_unlock(lock: &busdevs_lock); |
337 | } |
338 | |
339 | /* Returns the device for an address, with mi->lock held */ |
340 | static struct mctp_i3c_device * |
341 | mctp_i3c_lookup(struct mctp_i3c_bus *mbus, u64 pid) |
342 | { |
343 | struct mctp_i3c_device *mi = NULL, *ret = NULL; |
344 | |
345 | mutex_lock(&busdevs_lock); |
346 | list_for_each_entry(mi, &mbus->devs, list) |
347 | if (mi->pid == pid) { |
348 | ret = mi; |
349 | mutex_lock(&mi->lock); |
350 | break; |
351 | } |
352 | mutex_unlock(lock: &busdevs_lock); |
353 | return ret; |
354 | } |
355 | |
356 | static void mctp_i3c_xmit(struct mctp_i3c_bus *mbus, struct sk_buff *skb) |
357 | { |
358 | struct net_device_stats *stats = &mbus->ndev->stats; |
359 | struct i3c_priv_xfer xfer = { .rnw = false }; |
360 | struct mctp_i3c_internal_hdr *ihdr = NULL; |
361 | struct mctp_i3c_device *mi = NULL; |
362 | unsigned int data_len; |
363 | u8 *data = NULL; |
364 | u8 addr, pec; |
365 | int rc = 0; |
366 | u64 pid; |
367 | |
368 | skb_pull(skb, len: sizeof(struct mctp_i3c_internal_hdr)); |
369 | data_len = skb->len; |
370 | |
371 | ihdr = (void *)skb_mac_header(skb); |
372 | |
373 | pid = get_unaligned_be48(p: ihdr->dest); |
374 | mi = mctp_i3c_lookup(mbus, pid); |
375 | if (!mi) { |
376 | /* I3C endpoint went away after the packet was enqueued? */ |
377 | stats->tx_dropped++; |
378 | goto out; |
379 | } |
380 | |
381 | if (WARN_ON_ONCE(data_len + 1 > MCTP_I3C_MAXBUF)) |
382 | goto out; |
383 | |
384 | if (data_len + 1 > (unsigned int)mi->mwl) { |
385 | /* Route MTU was larger than supported by the endpoint */ |
386 | stats->tx_dropped++; |
387 | goto out; |
388 | } |
389 | |
390 | /* Need a linear buffer with space for the PEC */ |
391 | xfer.len = data_len + 1; |
392 | if (skb_tailroom(skb) >= 1) { |
393 | skb_put(skb, len: 1); |
394 | data = skb->data; |
395 | } else { |
396 | /* Otherwise need to copy the buffer */ |
397 | skb_copy_bits(skb, offset: 0, to: mbus->tx_scratch, len: skb->len); |
398 | data = mbus->tx_scratch; |
399 | } |
400 | |
401 | /* PEC calculation */ |
402 | addr = mi->addr << 1; |
403 | pec = i2c_smbus_pec(crc: 0, p: &addr, count: 1); |
404 | pec = i2c_smbus_pec(crc: pec, p: data, count: data_len); |
405 | data[data_len] = pec; |
406 | |
407 | xfer.data.out = data; |
408 | rc = i3c_device_do_priv_xfers(dev: mi->i3c, xfers: &xfer, nxfers: 1); |
409 | if (rc == 0) { |
410 | stats->tx_bytes += data_len; |
411 | stats->tx_packets++; |
412 | } else { |
413 | stats->tx_errors++; |
414 | } |
415 | |
416 | out: |
417 | if (mi) |
418 | mutex_unlock(lock: &mi->lock); |
419 | } |
420 | |
421 | static int mctp_i3c_tx_thread(void *data) |
422 | { |
423 | struct mctp_i3c_bus *mbus = data; |
424 | struct sk_buff *skb; |
425 | |
426 | for (;;) { |
427 | if (kthread_should_stop()) |
428 | break; |
429 | |
430 | spin_lock_bh(lock: &mbus->tx_lock); |
431 | skb = mbus->tx_skb; |
432 | mbus->tx_skb = NULL; |
433 | spin_unlock_bh(lock: &mbus->tx_lock); |
434 | |
435 | if (netif_queue_stopped(dev: mbus->ndev)) |
436 | netif_wake_queue(dev: mbus->ndev); |
437 | |
438 | if (skb) { |
439 | mctp_i3c_xmit(mbus, skb); |
440 | kfree_skb(skb); |
441 | } else { |
442 | wait_event_idle(mbus->tx_wq, |
443 | mbus->tx_skb || kthread_should_stop()); |
444 | } |
445 | } |
446 | |
447 | return 0; |
448 | } |
449 | |
450 | static netdev_tx_t mctp_i3c_start_xmit(struct sk_buff *skb, |
451 | struct net_device *ndev) |
452 | { |
453 | struct mctp_i3c_bus *mbus = netdev_priv(dev: ndev); |
454 | netdev_tx_t ret; |
455 | |
456 | spin_lock(lock: &mbus->tx_lock); |
457 | netif_stop_queue(dev: ndev); |
458 | if (mbus->tx_skb) { |
459 | dev_warn_ratelimited(&ndev->dev, "TX with queue stopped" ); |
460 | ret = NETDEV_TX_BUSY; |
461 | } else { |
462 | mbus->tx_skb = skb; |
463 | ret = NETDEV_TX_OK; |
464 | } |
465 | spin_unlock(lock: &mbus->tx_lock); |
466 | |
467 | if (ret == NETDEV_TX_OK) |
468 | wake_up(&mbus->tx_wq); |
469 | |
470 | return ret; |
471 | } |
472 | |
473 | static void mctp_i3c_bus_free(struct mctp_i3c_bus *mbus) |
474 | __must_hold(&busdevs_lock) |
475 | { |
476 | struct mctp_i3c_device *mi = NULL, *tmp = NULL; |
477 | |
478 | if (mbus->tx_thread) { |
479 | kthread_stop(k: mbus->tx_thread); |
480 | mbus->tx_thread = NULL; |
481 | } |
482 | |
483 | /* Remove any child devices */ |
484 | list_for_each_entry_safe(mi, tmp, &mbus->devs, list) { |
485 | mctp_i3c_remove_device(mi); |
486 | } |
487 | |
488 | kfree_skb(skb: mbus->tx_skb); |
489 | list_del(entry: &mbus->list); |
490 | } |
491 | |
492 | static void mctp_i3c_ndo_uninit(struct net_device *ndev) |
493 | { |
494 | struct mctp_i3c_bus *mbus = netdev_priv(dev: ndev); |
495 | |
496 | /* Perform cleanup here to ensure there are no remaining references */ |
497 | mctp_i3c_bus_free(mbus); |
498 | } |
499 | |
500 | static int (struct sk_buff *skb, struct net_device *dev, |
501 | unsigned short type, const void *daddr, |
502 | const void *saddr, unsigned int len) |
503 | { |
504 | struct mctp_i3c_internal_hdr *ihdr; |
505 | |
506 | skb_push(skb, len: sizeof(struct mctp_i3c_internal_hdr)); |
507 | skb_reset_mac_header(skb); |
508 | ihdr = (void *)skb_mac_header(skb); |
509 | memcpy(ihdr->dest, daddr, PID_SIZE); |
510 | memcpy(ihdr->source, saddr, PID_SIZE); |
511 | return 0; |
512 | } |
513 | |
514 | static const struct net_device_ops mctp_i3c_ops = { |
515 | .ndo_start_xmit = mctp_i3c_start_xmit, |
516 | .ndo_uninit = mctp_i3c_ndo_uninit, |
517 | }; |
518 | |
519 | static const struct header_ops mctp_i3c_headops = { |
520 | .create = mctp_i3c_header_create, |
521 | }; |
522 | |
523 | static void mctp_i3c_net_setup(struct net_device *dev) |
524 | { |
525 | dev->type = ARPHRD_MCTP; |
526 | |
527 | dev->mtu = MCTP_I3C_MAXMTU; |
528 | dev->min_mtu = MCTP_I3C_MINMTU; |
529 | dev->max_mtu = MCTP_I3C_MAXMTU; |
530 | dev->tx_queue_len = MCTP_I3C_TX_QUEUE_LEN; |
531 | |
532 | dev->hard_header_len = sizeof(struct mctp_i3c_internal_hdr); |
533 | dev->addr_len = PID_SIZE; |
534 | |
535 | dev->netdev_ops = &mctp_i3c_ops; |
536 | dev->header_ops = &mctp_i3c_headops; |
537 | } |
538 | |
539 | static bool mctp_i3c_is_mctp_controller(struct i3c_bus *bus) |
540 | { |
541 | struct i3c_dev_desc *master = bus->cur_master; |
542 | |
543 | if (!master) |
544 | return false; |
545 | |
546 | return of_property_read_bool(np: master->common.master->dev.of_node, |
547 | propname: MCTP_I3C_OF_PROP); |
548 | } |
549 | |
550 | /* Returns the Provisioned Id of a local bus master */ |
551 | static int mctp_i3c_bus_local_pid(struct i3c_bus *bus, u64 *ret_pid) |
552 | { |
553 | struct i3c_dev_desc *master; |
554 | |
555 | master = bus->cur_master; |
556 | if (WARN_ON_ONCE(!master)) |
557 | return -ENOENT; |
558 | *ret_pid = master->info.pid; |
559 | |
560 | return 0; |
561 | } |
562 | |
563 | /* Returns an ERR_PTR on failure */ |
564 | static struct mctp_i3c_bus *mctp_i3c_bus_add(struct i3c_bus *bus) |
565 | __must_hold(&busdevs_lock) |
566 | { |
567 | struct mctp_i3c_bus *mbus = NULL; |
568 | struct net_device *ndev = NULL; |
569 | char namebuf[IFNAMSIZ]; |
570 | u8 addr[PID_SIZE]; |
571 | int rc; |
572 | |
573 | if (!mctp_i3c_is_mctp_controller(bus)) |
574 | return ERR_PTR(error: -ENOENT); |
575 | |
576 | snprintf(buf: namebuf, size: sizeof(namebuf), fmt: "mctpi3c%d" , bus->id); |
577 | ndev = alloc_netdev(sizeof(*mbus), namebuf, NET_NAME_ENUM, |
578 | mctp_i3c_net_setup); |
579 | if (!ndev) { |
580 | rc = -ENOMEM; |
581 | goto err; |
582 | } |
583 | |
584 | mbus = netdev_priv(dev: ndev); |
585 | mbus->ndev = ndev; |
586 | mbus->bus = bus; |
587 | INIT_LIST_HEAD(list: &mbus->devs); |
588 | list_add(new: &mbus->list, head: &busdevs); |
589 | |
590 | rc = mctp_i3c_bus_local_pid(bus, ret_pid: &mbus->pid); |
591 | if (rc < 0) { |
592 | dev_err(&ndev->dev, "No I3C PID available\n" ); |
593 | goto err_free_uninit; |
594 | } |
595 | put_unaligned_be48(val: mbus->pid, p: addr); |
596 | dev_addr_set(dev: ndev, addr); |
597 | |
598 | init_waitqueue_head(&mbus->tx_wq); |
599 | spin_lock_init(&mbus->tx_lock); |
600 | mbus->tx_thread = kthread_run(mctp_i3c_tx_thread, mbus, |
601 | "%s/tx" , ndev->name); |
602 | if (IS_ERR(ptr: mbus->tx_thread)) { |
603 | dev_warn(&ndev->dev, "Error creating thread: %pe\n" , |
604 | mbus->tx_thread); |
605 | rc = PTR_ERR(ptr: mbus->tx_thread); |
606 | mbus->tx_thread = NULL; |
607 | goto err_free_uninit; |
608 | } |
609 | |
610 | rc = mctp_register_netdev(dev: ndev, NULL); |
611 | if (rc < 0) { |
612 | dev_warn(&ndev->dev, "netdev register failed: %d\n" , rc); |
613 | goto err_free_netdev; |
614 | } |
615 | return mbus; |
616 | |
617 | err_free_uninit: |
618 | /* uninit will not get called if a netdev has not been registered, |
619 | * so we perform the same mbus cleanup manually. |
620 | */ |
621 | mctp_i3c_bus_free(mbus); |
622 | |
623 | err_free_netdev: |
624 | free_netdev(dev: ndev); |
625 | |
626 | err: |
627 | return ERR_PTR(error: rc); |
628 | } |
629 | |
630 | static void mctp_i3c_bus_remove(struct mctp_i3c_bus *mbus) |
631 | __must_hold(&busdevs_lock) |
632 | { |
633 | /* Unregister calls through to ndo_uninit -> mctp_i3c_bus_free() */ |
634 | mctp_unregister_netdev(dev: mbus->ndev); |
635 | |
636 | free_netdev(dev: mbus->ndev); |
637 | /* mbus is deallocated */ |
638 | } |
639 | |
640 | /* Removes all mctp-i3c busses */ |
641 | static void mctp_i3c_bus_remove_all(void) |
642 | { |
643 | struct mctp_i3c_bus *mbus = NULL, *tmp = NULL; |
644 | |
645 | mutex_lock(&busdevs_lock); |
646 | list_for_each_entry_safe(mbus, tmp, &busdevs, list) { |
647 | mctp_i3c_bus_remove(mbus); |
648 | } |
649 | mutex_unlock(lock: &busdevs_lock); |
650 | } |
651 | |
652 | /* Adds a i3c_bus if it isn't already in the busdevs list. |
653 | * Suitable as an i3c_for_each_bus_locked callback. |
654 | */ |
655 | static int mctp_i3c_bus_add_new(struct i3c_bus *bus, void *data) |
656 | { |
657 | struct mctp_i3c_bus *mbus = NULL, *tmp = NULL; |
658 | bool exists = false; |
659 | |
660 | mutex_lock(&busdevs_lock); |
661 | list_for_each_entry_safe(mbus, tmp, &busdevs, list) |
662 | if (mbus->bus == bus) |
663 | exists = true; |
664 | |
665 | /* It is OK for a bus to already exist. That can occur due to |
666 | * the race in mod_init between notifier and for_each_bus |
667 | */ |
668 | if (!exists) |
669 | mctp_i3c_bus_add(bus); |
670 | mutex_unlock(lock: &busdevs_lock); |
671 | return 0; |
672 | } |
673 | |
674 | static void mctp_i3c_notify_bus_remove(struct i3c_bus *bus) |
675 | { |
676 | struct mctp_i3c_bus *mbus = NULL, *tmp; |
677 | |
678 | mutex_lock(&busdevs_lock); |
679 | list_for_each_entry_safe(mbus, tmp, &busdevs, list) |
680 | if (mbus->bus == bus) |
681 | mctp_i3c_bus_remove(mbus); |
682 | mutex_unlock(lock: &busdevs_lock); |
683 | } |
684 | |
685 | static int mctp_i3c_notifier_call(struct notifier_block *nb, |
686 | unsigned long action, void *data) |
687 | { |
688 | switch (action) { |
689 | case I3C_NOTIFY_BUS_ADD: |
690 | mctp_i3c_bus_add_new(bus: (struct i3c_bus *)data, NULL); |
691 | break; |
692 | case I3C_NOTIFY_BUS_REMOVE: |
693 | mctp_i3c_notify_bus_remove(bus: (struct i3c_bus *)data); |
694 | break; |
695 | } |
696 | return NOTIFY_DONE; |
697 | } |
698 | |
699 | static struct notifier_block mctp_i3c_notifier = { |
700 | .notifier_call = mctp_i3c_notifier_call, |
701 | }; |
702 | |
703 | static const struct i3c_device_id mctp_i3c_ids[] = { |
704 | I3C_CLASS(I3C_DCR_MCTP, NULL), |
705 | { 0 }, |
706 | }; |
707 | |
708 | static struct i3c_driver mctp_i3c_driver = { |
709 | .driver = { |
710 | .name = "mctp-i3c" , |
711 | }, |
712 | .probe = mctp_i3c_probe, |
713 | .remove = mctp_i3c_remove, |
714 | .id_table = mctp_i3c_ids, |
715 | }; |
716 | |
717 | static __init int mctp_i3c_mod_init(void) |
718 | { |
719 | int rc; |
720 | |
721 | rc = i3c_register_notifier(nb: &mctp_i3c_notifier); |
722 | if (rc < 0) { |
723 | i3c_driver_unregister(drv: &mctp_i3c_driver); |
724 | return rc; |
725 | } |
726 | |
727 | i3c_for_each_bus_locked(fn: mctp_i3c_bus_add_new, NULL); |
728 | |
729 | rc = i3c_driver_register(&mctp_i3c_driver); |
730 | if (rc < 0) |
731 | return rc; |
732 | |
733 | return 0; |
734 | } |
735 | |
736 | static __exit void mctp_i3c_mod_exit(void) |
737 | { |
738 | int rc; |
739 | |
740 | i3c_driver_unregister(drv: &mctp_i3c_driver); |
741 | |
742 | rc = i3c_unregister_notifier(nb: &mctp_i3c_notifier); |
743 | if (rc < 0) |
744 | pr_warn("MCTP I3C could not unregister notifier, %d\n" , rc); |
745 | |
746 | mctp_i3c_bus_remove_all(); |
747 | } |
748 | |
749 | module_init(mctp_i3c_mod_init); |
750 | module_exit(mctp_i3c_mod_exit); |
751 | |
752 | MODULE_DEVICE_TABLE(i3c, mctp_i3c_ids); |
753 | MODULE_DESCRIPTION("MCTP I3C device" ); |
754 | MODULE_LICENSE("GPL" ); |
755 | MODULE_AUTHOR("Matt Johnston <matt@codeconstruct.com.au>" ); |
756 | |