drivers/net/mctp/mctp-i3c.c

   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(mi->mbus->ndev,
 111                                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, sizeof(struct mctp_i3c_internal_hdr));
 122         put_unaligned_be48(mi->pid, ihdr->source);
 123         put_unaligned_be48(mi->mbus->pid, ihdr->dest);
 124         skb_pull(skb, sizeof(struct mctp_i3c_internal_hdr));
 125
 126         xfer.data.in = skb_put(skb, mi->mrl);
 127
 128         rc = i3c_device_do_priv_xfers(mi->i3c, &xfer, 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(0, &addr, 1);
 146         pec = i2c_smbus_pec(pec, xfer.data.in, 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, xfer.len - 1);
 155
 156         cb = __mctp_cb(skb);
 157         cb->halen = PID_SIZE;
 158         put_unaligned_be48(mi->pid, 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(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(mi->i3c, &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(mi->i3c, &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(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(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(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(&mi->list, &mbus->devs);
 267
 268         i3cdev_set_drvdata(i3c, 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(&mi->list);
 277         kfree(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(&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(mi->i3c);
 313         i3c_device_free_ibi(mi->i3c);
 314
 315         /* Counterpart of mctp_i3c_add_device */
 316         i3cdev_set_drvdata(mi->i3c, NULL);
 317         list_del(&mi->list);
 318
 319         /* Safe to unlock after removing from the list */
 320         mutex_unlock(&mi->lock);
 321         kfree(mi);
 322 }
 323
 324 static void mctp_i3c_remove(struct i3c_device *i3c)
 325 {
 326         struct mctp_i3c_device *mi = i3cdev_get_drvdata(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(&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(&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, 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(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, 1);
 394                 data = skb->data;
 395         } else {
 396                 /* Otherwise need to copy the buffer */
 397                 skb_copy_bits(skb, 0, mbus->tx_scratch, skb->len);
 398                 data = mbus->tx_scratch;
 399         }
 400
 401         /* PEC calculation */
 402         addr = mi->addr << 1;
 403         pec = i2c_smbus_pec(0, &addr, 1);
 404         pec = i2c_smbus_pec(pec, data, data_len);
 405         data[data_len] = pec;
 406
 407         xfer.data.out = data;
 408         rc = i3c_device_do_priv_xfers(mi->i3c, &xfer, 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(&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(&mbus->tx_lock);
 431                 skb = mbus->tx_skb;
 432                 mbus->tx_skb = NULL;
 433                 spin_unlock_bh(&mbus->tx_lock);
 434
 435                 if (netif_queue_stopped(mbus->ndev))
 436                         netif_wake_queue(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(ndev);
 454         netdev_tx_t ret;
 455
 456         spin_lock(&mbus->tx_lock);
 457         netif_stop_queue(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(&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(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(mbus->tx_skb);
 489         list_del(&mbus->list);
 490 }
 491
 492 static void mctp_i3c_ndo_uninit(struct net_device *ndev)
 493 {
 494         struct mctp_i3c_bus *mbus = netdev_priv(ndev);
 495
 496         /* Perform cleanup here to ensure there are no remaining references */
 497         mctp_i3c_bus_free(mbus);
 498 }
 499
 500 static int mctp_i3c_header_create(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, 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(master->common.master->dev.of_node,
 547                                      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(-ENOENT);
 575
 576         snprintf(namebuf, sizeof(namebuf), "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(ndev);
 585         mbus->ndev = ndev;
 586         mbus->bus = bus;
 587         INIT_LIST_HEAD(&mbus->devs);
 588         list_add(&mbus->list, &busdevs);
 589
 590         rc = mctp_i3c_bus_local_pid(bus, &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(mbus->pid, addr);
 596         dev_addr_set(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(mbus->tx_thread)) {
 603                 dev_warn(&ndev->dev, "Error creating thread: %pe\n",
 604                          mbus->tx_thread);
 605                 rc = PTR_ERR(mbus->tx_thread);
 606                 mbus->tx_thread = NULL;
 607                 goto err_free_uninit;
 608         }
 609
 610         rc = mctp_register_netdev(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(ndev);
 625
 626 err:
 627         return ERR_PTR(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(mbus->ndev);
 635
 636         free_netdev(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(&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(&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(&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((struct i3c_bus *)data, NULL);
 691                 break;
 692         case I3C_NOTIFY_BUS_REMOVE:
 693                 mctp_i3c_notify_bus_remove((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(&mctp_i3c_notifier);
 722         if (rc < 0) {
 723                 i3c_driver_unregister(&mctp_i3c_driver);
 724                 return rc;
 725         }
 726
 727         i3c_for_each_bus_locked(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(&mctp_i3c_driver);
 741
 742         rc = i3c_unregister_notifier(&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>");