drivers/platform/mellanox/mlxbf-tmfifo.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Mellanox BlueField SoC TmFifo driver
   4  *
   5  * Copyright (C) 2019 Mellanox Technologies
   6  */
   7
   8 #include <linux/acpi.h>
   9 #include <linux/bitfield.h>
  10 #include <linux/circ_buf.h>
  11 #include <linux/efi.h>
  12 #include <linux/irq.h>
  13 #include <linux/module.h>
  14 #include <linux/mutex.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/types.h>
  17
  18 #include <linux/virtio_config.h>
  19 #include <linux/virtio_console.h>
  20 #include <linux/virtio_ids.h>
  21 #include <linux/virtio_net.h>
  22 #include <linux/virtio_ring.h>
  23
  24 #include "mlxbf-tmfifo-regs.h"
  25
  26 /* Vring size. */
  27 #define MLXBF_TMFIFO_VRING_SIZE                 SZ_1K
  28
  29 /* Console Tx buffer size. */
  30 #define MLXBF_TMFIFO_CON_TX_BUF_SIZE            SZ_32K
  31
  32 /* Console Tx buffer reserved space. */
  33 #define MLXBF_TMFIFO_CON_TX_BUF_RSV_SIZE        8
  34
  35 /* House-keeping timer interval. */
  36 #define MLXBF_TMFIFO_TIMER_INTERVAL             (HZ / 10)
  37
  38 /* Virtual devices sharing the TM FIFO. */
  39 #define MLXBF_TMFIFO_VDEV_MAX           (VIRTIO_ID_CONSOLE + 1)
  40
  41 /*
  42  * Reserve 1/16 of TmFifo space, so console messages are not starved by
  43  * the networking traffic.
  44  */
  45 #define MLXBF_TMFIFO_RESERVE_RATIO              16
  46
  47 /* Message with data needs at least two words (for header & data). */
  48 #define MLXBF_TMFIFO_DATA_MIN_WORDS             2
  49
  50 /* ACPI UID for BlueField-3. */
  51 #define TMFIFO_BF3_UID                          1
  52
  53 struct mlxbf_tmfifo;
  54
  55 /**
  56  * mlxbf_tmfifo_vring - Structure of the TmFifo virtual ring
  57  * @va: virtual address of the ring
  58  * @dma: dma address of the ring
  59  * @vq: pointer to the virtio virtqueue
  60  * @desc: current descriptor of the pending packet
  61  * @desc_head: head descriptor of the pending packet
  62  * @drop_desc: dummy desc for packet dropping
  63  * @cur_len: processed length of the current descriptor
  64  * @rem_len: remaining length of the pending packet
  65  * @pkt_len: total length of the pending packet
  66  * @next_avail: next avail descriptor id
  67  * @num: vring size (number of descriptors)
  68  * @align: vring alignment size
  69  * @index: vring index
  70  * @vdev_id: vring virtio id (VIRTIO_ID_xxx)
  71  * @fifo: pointer to the tmfifo structure
  72  */
  73 struct mlxbf_tmfifo_vring {
  74         void *va;
  75         dma_addr_t dma;
  76         struct virtqueue *vq;
  77         struct vring_desc *desc;
  78         struct vring_desc *desc_head;
  79         struct vring_desc drop_desc;
  80         int cur_len;
  81         int rem_len;
  82         u32 pkt_len;
  83         u16 next_avail;
  84         int num;
  85         int align;
  86         int index;
  87         int vdev_id;
  88         struct mlxbf_tmfifo *fifo;
  89 };
  90
  91 /* Check whether vring is in drop mode. */
  92 #define IS_VRING_DROP(_r) ({ \
  93         typeof(_r) (r) = (_r); \
  94         (r->desc_head == &r->drop_desc ? true : false); })
  95
  96 /* A stub length to drop maximum length packet. */
  97 #define VRING_DROP_DESC_MAX_LEN         GENMASK(15, 0)
  98
  99 /* Interrupt types. */
 100 enum {
 101         MLXBF_TM_RX_LWM_IRQ,
 102         MLXBF_TM_RX_HWM_IRQ,
 103         MLXBF_TM_TX_LWM_IRQ,
 104         MLXBF_TM_TX_HWM_IRQ,
 105         MLXBF_TM_MAX_IRQ
 106 };
 107
 108 /* Ring types (Rx & Tx). */
 109 enum {
 110         MLXBF_TMFIFO_VRING_RX,
 111         MLXBF_TMFIFO_VRING_TX,
 112         MLXBF_TMFIFO_VRING_MAX
 113 };
 114
 115 /**
 116  * mlxbf_tmfifo_vdev - Structure of the TmFifo virtual device
 117  * @vdev: virtio device, in which the vdev.id.device field has the
 118  *        VIRTIO_ID_xxx id to distinguish the virtual device.
 119  * @status: status of the device
 120  * @features: supported features of the device
 121  * @vrings: array of tmfifo vrings of this device
 122  * @config.cons: virtual console config -
 123  *               select if vdev.id.device is VIRTIO_ID_CONSOLE
 124  * @config.net: virtual network config -
 125  *              select if vdev.id.device is VIRTIO_ID_NET
 126  * @tx_buf: tx buffer used to buffer data before writing into the FIFO
 127  */
 128 struct mlxbf_tmfifo_vdev {
 129         struct virtio_device vdev;
 130         u8 status;
 131         u64 features;
 132         struct mlxbf_tmfifo_vring vrings[MLXBF_TMFIFO_VRING_MAX];
 133         union {
 134                 struct virtio_console_config cons;
 135                 struct virtio_net_config net;
 136         } config;
 137         struct circ_buf tx_buf;
 138 };
 139
 140 /**
 141  * mlxbf_tmfifo_irq_info - Structure of the interrupt information
 142  * @fifo: pointer to the tmfifo structure
 143  * @irq: interrupt number
 144  * @index: index into the interrupt array
 145  */
 146 struct mlxbf_tmfifo_irq_info {
 147         struct mlxbf_tmfifo *fifo;
 148         int irq;
 149         int index;
 150 };
 151
 152 /**
 153  * mlxbf_tmfifo_io - Structure of the TmFifo IO resource (for both rx & tx)
 154  * @ctl: control register offset (TMFIFO_RX_CTL / TMFIFO_TX_CTL)
 155  * @sts: status register offset (TMFIFO_RX_STS / TMFIFO_TX_STS)
 156  * @data: data register offset (TMFIFO_RX_DATA / TMFIFO_TX_DATA)
 157  */
 158 struct mlxbf_tmfifo_io {
 159         void __iomem *ctl;
 160         void __iomem *sts;
 161         void __iomem *data;
 162 };
 163
 164 /**
 165  * mlxbf_tmfifo - Structure of the TmFifo
 166  * @vdev: array of the virtual devices running over the TmFifo
 167  * @lock: lock to protect the TmFifo access
 168  * @res0: mapped resource block 0
 169  * @res1: mapped resource block 1
 170  * @rx: rx io resource
 171  * @tx: tx io resource
 172  * @rx_fifo_size: number of entries of the Rx FIFO
 173  * @tx_fifo_size: number of entries of the Tx FIFO
 174  * @pend_events: pending bits for deferred events
 175  * @irq_info: interrupt information
 176  * @work: work struct for deferred process
 177  * @timer: background timer
 178  * @vring: Tx/Rx ring
 179  * @spin_lock: Tx/Rx spin lock
 180  * @is_ready: ready flag
 181  */
 182 struct mlxbf_tmfifo {
 183         struct mlxbf_tmfifo_vdev *vdev[MLXBF_TMFIFO_VDEV_MAX];
 184         struct mutex lock;              /* TmFifo lock */
 185         void __iomem *res0;
 186         void __iomem *res1;
 187         struct mlxbf_tmfifo_io rx;
 188         struct mlxbf_tmfifo_io tx;
 189         int rx_fifo_size;
 190         int tx_fifo_size;
 191         unsigned long pend_events;
 192         struct mlxbf_tmfifo_irq_info irq_info[MLXBF_TM_MAX_IRQ];
 193         struct work_struct work;
 194         struct timer_list timer;
 195         struct mlxbf_tmfifo_vring *vring[2];
 196         spinlock_t spin_lock[2];        /* spin lock */
 197         bool is_ready;
 198 };
 199
 200 /**
 201  * mlxbf_tmfifo_msg_hdr - Structure of the TmFifo message header
 202  * @type: message type
 203  * @len: payload length in network byte order. Messages sent into the FIFO
 204  *       will be read by the other side as data stream in the same byte order.
 205  *       The length needs to be encoded into network order so both sides
 206  *       could understand it.
 207  */
 208 struct mlxbf_tmfifo_msg_hdr {
 209         u8 type;
 210         __be16 len;
 211         u8 unused[5];
 212 } __packed __aligned(sizeof(u64));
 213
 214 /*
 215  * Default MAC.
 216  * This MAC address will be read from EFI persistent variable if configured.
 217  * It can also be reconfigured with standard Linux tools.
 218  */
 219 static u8 mlxbf_tmfifo_net_default_mac[ETH_ALEN] = {
 220         0x00, 0x1A, 0xCA, 0xFF, 0xFF, 0x01
 221 };
 222
 223 /* EFI variable name of the MAC address. */
 224 static efi_char16_t mlxbf_tmfifo_efi_name[] = L"RshimMacAddr";
 225
 226 /* Maximum L2 header length. */
 227 #define MLXBF_TMFIFO_NET_L2_OVERHEAD    (ETH_HLEN + VLAN_HLEN)
 228
 229 /* Supported virtio-net features. */
 230 #define MLXBF_TMFIFO_NET_FEATURES \
 231         (BIT_ULL(VIRTIO_NET_F_MTU) | BIT_ULL(VIRTIO_NET_F_STATUS) | \
 232          BIT_ULL(VIRTIO_NET_F_MAC))
 233
 234 #define mlxbf_vdev_to_tmfifo(d) container_of(d, struct mlxbf_tmfifo_vdev, vdev)
 235
 236 /* Free vrings of the FIFO device. */
 237 static void mlxbf_tmfifo_free_vrings(struct mlxbf_tmfifo *fifo,
 238                                      struct mlxbf_tmfifo_vdev *tm_vdev)
 239 {
 240         struct mlxbf_tmfifo_vring *vring;
 241         int i, size;
 242
 243         for (i = 0; i < ARRAY_SIZE(tm_vdev->vrings); i++) {
 244                 vring = &tm_vdev->vrings[i];
 245                 if (vring->va) {
 246                         size = vring_size(vring->num, vring->align);
 247                         dma_free_coherent(tm_vdev->vdev.dev.parent, size,
 248                                           vring->va, vring->dma);
 249                         vring->va = NULL;
 250                         if (vring->vq) {
 251                                 vring_del_virtqueue(vring->vq);
 252                                 vring->vq = NULL;
 253                         }
 254                 }
 255         }
 256 }
 257
 258 /* Allocate vrings for the FIFO. */
 259 static int mlxbf_tmfifo_alloc_vrings(struct mlxbf_tmfifo *fifo,
 260                                      struct mlxbf_tmfifo_vdev *tm_vdev)
 261 {
 262         struct mlxbf_tmfifo_vring *vring;
 263         struct device *dev;
 264         dma_addr_t dma;
 265         int i, size;
 266         void *va;
 267
 268         for (i = 0; i < ARRAY_SIZE(tm_vdev->vrings); i++) {
 269                 vring = &tm_vdev->vrings[i];
 270                 vring->fifo = fifo;
 271                 vring->num = MLXBF_TMFIFO_VRING_SIZE;
 272                 vring->align = SMP_CACHE_BYTES;
 273                 vring->index = i;
 274                 vring->vdev_id = tm_vdev->vdev.id.device;
 275                 vring->drop_desc.len = VRING_DROP_DESC_MAX_LEN;
 276                 dev = &tm_vdev->vdev.dev;
 277
 278                 size = vring_size(vring->num, vring->align);
 279                 va = dma_alloc_coherent(dev->parent, size, &dma, GFP_KERNEL);
 280                 if (!va) {
 281                         mlxbf_tmfifo_free_vrings(fifo, tm_vdev);
 282                         dev_err(dev->parent, "dma_alloc_coherent failed\n");
 283                         return -ENOMEM;
 284                 }
 285
 286                 vring->va = va;
 287                 vring->dma = dma;
 288         }
 289
 290         return 0;
 291 }
 292
 293 /* Disable interrupts of the FIFO device. */
 294 static void mlxbf_tmfifo_disable_irqs(struct mlxbf_tmfifo *fifo)
 295 {
 296         int i, irq;
 297
 298         for (i = 0; i < MLXBF_TM_MAX_IRQ; i++) {
 299                 irq = fifo->irq_info[i].irq;
 300                 fifo->irq_info[i].irq = 0;
 301                 disable_irq(irq);
 302         }
 303 }
 304
 305 /* Interrupt handler. */
 306 static irqreturn_t mlxbf_tmfifo_irq_handler(int irq, void *arg)
 307 {
 308         struct mlxbf_tmfifo_irq_info *irq_info = arg;
 309
 310         if (!test_and_set_bit(irq_info->index, &irq_info->fifo->pend_events))
 311                 schedule_work(&irq_info->fifo->work);
 312
 313         return IRQ_HANDLED;
 314 }
 315
 316 /* Get the next packet descriptor from the vring. */
 317 static struct vring_desc *
 318 mlxbf_tmfifo_get_next_desc(struct mlxbf_tmfifo_vring *vring)
 319 {
 320         const struct vring *vr = virtqueue_get_vring(vring->vq);
 321         struct virtio_device *vdev = vring->vq->vdev;
 322         unsigned int idx, head;
 323
 324         if (vring->next_avail == virtio16_to_cpu(vdev, vr->avail->idx))
 325                 return NULL;
 326
 327         /* Make sure 'avail->idx' is visible already. */
 328         virtio_rmb(false);
 329
 330         idx = vring->next_avail % vr->num;
 331         head = virtio16_to_cpu(vdev, vr->avail->ring[idx]);
 332         if (WARN_ON(head >= vr->num))
 333                 return NULL;
 334
 335         vring->next_avail++;
 336
 337         return &vr->desc[head];
 338 }
 339
 340 /* Release virtio descriptor. */
 341 static void mlxbf_tmfifo_release_desc(struct mlxbf_tmfifo_vring *vring,
 342                                       struct vring_desc *desc, u32 len)
 343 {
 344         const struct vring *vr = virtqueue_get_vring(vring->vq);
 345         struct virtio_device *vdev = vring->vq->vdev;
 346         u16 idx, vr_idx;
 347
 348         vr_idx = virtio16_to_cpu(vdev, vr->used->idx);
 349         idx = vr_idx % vr->num;
 350         vr->used->ring[idx].id = cpu_to_virtio32(vdev, desc - vr->desc);
 351         vr->used->ring[idx].len = cpu_to_virtio32(vdev, len);
 352
 353         /*
 354          * Virtio could poll and check the 'idx' to decide whether the desc is
 355          * done or not. Add a memory barrier here to make sure the update above
 356          * completes before updating the idx.
 357          */
 358         virtio_mb(false);
 359         vr->used->idx = cpu_to_virtio16(vdev, vr_idx + 1);
 360 }
 361
 362 /* Get the total length of the descriptor chain. */
 363 static u32 mlxbf_tmfifo_get_pkt_len(struct mlxbf_tmfifo_vring *vring,
 364                                     struct vring_desc *desc)
 365 {
 366         const struct vring *vr = virtqueue_get_vring(vring->vq);
 367         struct virtio_device *vdev = vring->vq->vdev;
 368         u32 len = 0, idx;
 369
 370         while (desc) {
 371                 len += virtio32_to_cpu(vdev, desc->len);
 372                 if (!(virtio16_to_cpu(vdev, desc->flags) & VRING_DESC_F_NEXT))
 373                         break;
 374                 idx = virtio16_to_cpu(vdev, desc->next);
 375                 desc = &vr->desc[idx];
 376         }
 377
 378         return len;
 379 }
 380
 381 static void mlxbf_tmfifo_release_pkt(struct mlxbf_tmfifo_vring *vring)
 382 {
 383         struct vring_desc *desc_head;
 384         u32 len = 0;
 385
 386         if (vring->desc_head) {
 387                 desc_head = vring->desc_head;
 388                 len = vring->pkt_len;
 389         } else {
 390                 desc_head = mlxbf_tmfifo_get_next_desc(vring);
 391                 len = mlxbf_tmfifo_get_pkt_len(vring, desc_head);
 392         }
 393
 394         if (desc_head)
 395                 mlxbf_tmfifo_release_desc(vring, desc_head, len);
 396
 397         vring->pkt_len = 0;
 398         vring->desc = NULL;
 399         vring->desc_head = NULL;
 400 }
 401
 402 static void mlxbf_tmfifo_init_net_desc(struct mlxbf_tmfifo_vring *vring,
 403                                        struct vring_desc *desc, bool is_rx)
 404 {
 405         struct virtio_device *vdev = vring->vq->vdev;
 406         struct virtio_net_hdr *net_hdr;
 407
 408         net_hdr = phys_to_virt(virtio64_to_cpu(vdev, desc->addr));
 409         memset(net_hdr, 0, sizeof(*net_hdr));
 410 }
 411
 412 /* Get and initialize the next packet. */
 413 static struct vring_desc *
 414 mlxbf_tmfifo_get_next_pkt(struct mlxbf_tmfifo_vring *vring, bool is_rx)
 415 {
 416         struct vring_desc *desc;
 417
 418         desc = mlxbf_tmfifo_get_next_desc(vring);
 419         if (desc && is_rx && vring->vdev_id == VIRTIO_ID_NET)
 420                 mlxbf_tmfifo_init_net_desc(vring, desc, is_rx);
 421
 422         vring->desc_head = desc;
 423         vring->desc = desc;
 424
 425         return desc;
 426 }
 427
 428 /* House-keeping timer. */
 429 static void mlxbf_tmfifo_timer(struct timer_list *t)
 430 {
 431         struct mlxbf_tmfifo *fifo = container_of(t, struct mlxbf_tmfifo, timer);
 432         int rx, tx;
 433
 434         rx = !test_and_set_bit(MLXBF_TM_RX_HWM_IRQ, &fifo->pend_events);
 435         tx = !test_and_set_bit(MLXBF_TM_TX_LWM_IRQ, &fifo->pend_events);
 436
 437         if (rx || tx)
 438                 schedule_work(&fifo->work);
 439
 440         mod_timer(&fifo->timer, jiffies + MLXBF_TMFIFO_TIMER_INTERVAL);
 441 }
 442
 443 /* Copy one console packet into the output buffer. */
 444 static void mlxbf_tmfifo_console_output_one(struct mlxbf_tmfifo_vdev *cons,
 445                                             struct mlxbf_tmfifo_vring *vring,
 446                                             struct vring_desc *desc)
 447 {
 448         const struct vring *vr = virtqueue_get_vring(vring->vq);
 449         struct virtio_device *vdev = &cons->vdev;
 450         u32 len, idx, seg;
 451         void *addr;
 452
 453         while (desc) {
 454                 addr = phys_to_virt(virtio64_to_cpu(vdev, desc->addr));
 455                 len = virtio32_to_cpu(vdev, desc->len);
 456
 457                 seg = CIRC_SPACE_TO_END(cons->tx_buf.head, cons->tx_buf.tail,
 458                                         MLXBF_TMFIFO_CON_TX_BUF_SIZE);
 459                 if (len <= seg) {
 460                         memcpy(cons->tx_buf.buf + cons->tx_buf.head, addr, len);
 461                 } else {
 462                         memcpy(cons->tx_buf.buf + cons->tx_buf.head, addr, seg);
 463                         addr += seg;
 464                         memcpy(cons->tx_buf.buf, addr, len - seg);
 465                 }
 466                 cons->tx_buf.head = (cons->tx_buf.head + len) %
 467                         MLXBF_TMFIFO_CON_TX_BUF_SIZE;
 468
 469                 if (!(virtio16_to_cpu(vdev, desc->flags) & VRING_DESC_F_NEXT))
 470                         break;
 471                 idx = virtio16_to_cpu(vdev, desc->next);
 472                 desc = &vr->desc[idx];
 473         }
 474 }
 475
 476 /* Copy console data into the output buffer. */
 477 static void mlxbf_tmfifo_console_output(struct mlxbf_tmfifo_vdev *cons,
 478                                         struct mlxbf_tmfifo_vring *vring)
 479 {
 480         struct vring_desc *desc;
 481         u32 len, avail;
 482
 483         desc = mlxbf_tmfifo_get_next_desc(vring);
 484         while (desc) {
 485                 /* Release the packet if not enough space. */
 486                 len = mlxbf_tmfifo_get_pkt_len(vring, desc);
 487                 avail = CIRC_SPACE(cons->tx_buf.head, cons->tx_buf.tail,
 488                                    MLXBF_TMFIFO_CON_TX_BUF_SIZE);
 489                 if (len + MLXBF_TMFIFO_CON_TX_BUF_RSV_SIZE > avail) {
 490                         mlxbf_tmfifo_release_desc(vring, desc, len);
 491                         break;
 492                 }
 493
 494                 mlxbf_tmfifo_console_output_one(cons, vring, desc);
 495                 mlxbf_tmfifo_release_desc(vring, desc, len);
 496                 desc = mlxbf_tmfifo_get_next_desc(vring);
 497         }
 498 }
 499
 500 /* Get the number of available words in Rx FIFO for receiving. */
 501 static int mlxbf_tmfifo_get_rx_avail(struct mlxbf_tmfifo *fifo)
 502 {
 503         u64 sts;
 504
 505         sts = readq(fifo->rx.sts);
 506         return FIELD_GET(MLXBF_TMFIFO_RX_STS__COUNT_MASK, sts);
 507 }
 508
 509 /* Get the number of available words in the TmFifo for sending. */
 510 static int mlxbf_tmfifo_get_tx_avail(struct mlxbf_tmfifo *fifo, int vdev_id)
 511 {
 512         int tx_reserve;
 513         u32 count;
 514         u64 sts;
 515
 516         /* Reserve some room in FIFO for console messages. */
 517         if (vdev_id == VIRTIO_ID_NET)
 518                 tx_reserve = fifo->tx_fifo_size / MLXBF_TMFIFO_RESERVE_RATIO;
 519         else
 520                 tx_reserve = 1;
 521
 522         sts = readq(fifo->tx.sts);
 523         count = FIELD_GET(MLXBF_TMFIFO_TX_STS__COUNT_MASK, sts);
 524         return fifo->tx_fifo_size - tx_reserve - count;
 525 }
 526
 527 /* Console Tx (move data from the output buffer into the TmFifo). */
 528 static void mlxbf_tmfifo_console_tx(struct mlxbf_tmfifo *fifo, int avail)
 529 {
 530         struct mlxbf_tmfifo_msg_hdr hdr;
 531         struct mlxbf_tmfifo_vdev *cons;
 532         unsigned long flags;
 533         int size, seg;
 534         void *addr;
 535         u64 data;
 536
 537         /* Return if not enough space available. */
 538         if (avail < MLXBF_TMFIFO_DATA_MIN_WORDS)
 539                 return;
 540
 541         cons = fifo->vdev[VIRTIO_ID_CONSOLE];
 542         if (!cons || !cons->tx_buf.buf)
 543                 return;
 544
 545         /* Return if no data to send. */
 546         size = CIRC_CNT(cons->tx_buf.head, cons->tx_buf.tail,
 547                         MLXBF_TMFIFO_CON_TX_BUF_SIZE);
 548         if (size == 0)
 549                 return;
 550
 551         /* Adjust the size to available space. */
 552         if (size + sizeof(hdr) > avail * sizeof(u64))
 553                 size = avail * sizeof(u64) - sizeof(hdr);
 554
 555         /* Write header. */
 556         hdr.type = VIRTIO_ID_CONSOLE;
 557         hdr.len = htons(size);
 558         writeq(*(u64 *)&hdr, fifo->tx.data);
 559
 560         /* Use spin-lock to protect the 'cons->tx_buf'. */
 561         spin_lock_irqsave(&fifo->spin_lock[0], flags);
 562
 563         while (size > 0) {
 564                 addr = cons->tx_buf.buf + cons->tx_buf.tail;
 565
 566                 seg = CIRC_CNT_TO_END(cons->tx_buf.head, cons->tx_buf.tail,
 567                                       MLXBF_TMFIFO_CON_TX_BUF_SIZE);
 568                 if (seg >= sizeof(u64)) {
 569                         memcpy(&data, addr, sizeof(u64));
 570                 } else {
 571                         memcpy(&data, addr, seg);
 572                         memcpy((u8 *)&data + seg, cons->tx_buf.buf,
 573                                sizeof(u64) - seg);
 574                 }
 575                 writeq(data, fifo->tx.data);
 576
 577                 if (size >= sizeof(u64)) {
 578                         cons->tx_buf.tail = (cons->tx_buf.tail + sizeof(u64)) %
 579                                 MLXBF_TMFIFO_CON_TX_BUF_SIZE;
 580                         size -= sizeof(u64);
 581                 } else {
 582                         cons->tx_buf.tail = (cons->tx_buf.tail + size) %
 583                                 MLXBF_TMFIFO_CON_TX_BUF_SIZE;
 584                         size = 0;
 585                 }
 586         }
 587
 588         spin_unlock_irqrestore(&fifo->spin_lock[0], flags);
 589 }
 590
 591 /* Rx/Tx one word in the descriptor buffer. */
 592 static void mlxbf_tmfifo_rxtx_word(struct mlxbf_tmfifo_vring *vring,
 593                                    struct vring_desc *desc,
 594                                    bool is_rx, int len)
 595 {
 596         struct virtio_device *vdev = vring->vq->vdev;
 597         struct mlxbf_tmfifo *fifo = vring->fifo;
 598         void *addr;
 599         u64 data;
 600
 601         /* Get the buffer address of this desc. */
 602         addr = phys_to_virt(virtio64_to_cpu(vdev, desc->addr));
 603
 604         /* Read a word from FIFO for Rx. */
 605         if (is_rx)
 606                 data = readq(fifo->rx.data);
 607
 608         if (vring->cur_len + sizeof(u64) <= len) {
 609                 /* The whole word. */
 610                 if (!IS_VRING_DROP(vring)) {
 611                         if (is_rx)
 612                                 memcpy(addr + vring->cur_len, &data,
 613                                        sizeof(u64));
 614                         else
 615                                 memcpy(&data, addr + vring->cur_len,
 616                                        sizeof(u64));
 617                 }
 618                 vring->cur_len += sizeof(u64);
 619         } else {
 620                 /* Leftover bytes. */
 621                 if (!IS_VRING_DROP(vring)) {
 622                         if (is_rx)
 623                                 memcpy(addr + vring->cur_len, &data,
 624                                        len - vring->cur_len);
 625                         else
 626                                 memcpy(&data, addr + vring->cur_len,
 627                                        len - vring->cur_len);
 628                 }
 629                 vring->cur_len = len;
 630         }
 631
 632         /* Write the word into FIFO for Tx. */
 633         if (!is_rx)
 634                 writeq(data, fifo->tx.data);
 635 }
 636
 637 /*
 638  * Rx/Tx packet header.
 639  *
 640  * In Rx case, the packet might be found to belong to a different vring since
 641  * the TmFifo is shared by different services. In such case, the 'vring_change'
 642  * flag is set.
 643  */
 644 static void mlxbf_tmfifo_rxtx_header(struct mlxbf_tmfifo_vring *vring,
 645                                      struct vring_desc **desc,
 646                                      bool is_rx, bool *vring_change)
 647 {
 648         struct mlxbf_tmfifo *fifo = vring->fifo;
 649         struct virtio_net_config *config;
 650         struct mlxbf_tmfifo_msg_hdr hdr;
 651         int vdev_id, hdr_len;
 652         bool drop_rx = false;
 653
 654         /* Read/Write packet header. */
 655         if (is_rx) {
 656                 /* Drain one word from the FIFO. */
 657                 *(u64 *)&hdr = readq(fifo->rx.data);
 658
 659                 /* Skip the length 0 packets (keepalive). */
 660                 if (hdr.len == 0)
 661                         return;
 662
 663                 /* Check packet type. */
 664                 if (hdr.type == VIRTIO_ID_NET) {
 665                         vdev_id = VIRTIO_ID_NET;
 666                         hdr_len = sizeof(struct virtio_net_hdr);
 667                         config = &fifo->vdev[vdev_id]->config.net;
 668                         /* A legacy-only interface for now. */
 669                         if (ntohs(hdr.len) >
 670                             __virtio16_to_cpu(virtio_legacy_is_little_endian(),
 671                                               config->mtu) +
 672                                               MLXBF_TMFIFO_NET_L2_OVERHEAD)
 673                                 drop_rx = true;
 674                 } else {
 675                         vdev_id = VIRTIO_ID_CONSOLE;
 676                         hdr_len = 0;
 677                 }
 678
 679                 /*
 680                  * Check whether the new packet still belongs to this vring.
 681                  * If not, update the pkt_len of the new vring.
 682                  */
 683                 if (vdev_id != vring->vdev_id) {
 684                         struct mlxbf_tmfifo_vdev *tm_dev2 = fifo->vdev[vdev_id];
 685
 686                         if (!tm_dev2)
 687                                 return;
 688                         vring->desc = *desc;
 689                         vring = &tm_dev2->vrings[MLXBF_TMFIFO_VRING_RX];
 690                         *vring_change = true;
 691                 }
 692
 693                 if (drop_rx && !IS_VRING_DROP(vring)) {
 694                         if (vring->desc_head)
 695                                 mlxbf_tmfifo_release_pkt(vring);
 696                         *desc = &vring->drop_desc;
 697                         vring->desc_head = *desc;
 698                         vring->desc = *desc;
 699                 }
 700
 701                 vring->pkt_len = ntohs(hdr.len) + hdr_len;
 702         } else {
 703                 /* Network virtio has an extra header. */
 704                 hdr_len = (vring->vdev_id == VIRTIO_ID_NET) ?
 705                            sizeof(struct virtio_net_hdr) : 0;
 706                 vring->pkt_len = mlxbf_tmfifo_get_pkt_len(vring, *desc);
 707                 hdr.type = (vring->vdev_id == VIRTIO_ID_NET) ?
 708                             VIRTIO_ID_NET : VIRTIO_ID_CONSOLE;
 709                 hdr.len = htons(vring->pkt_len - hdr_len);
 710                 writeq(*(u64 *)&hdr, fifo->tx.data);
 711         }
 712
 713         vring->cur_len = hdr_len;
 714         vring->rem_len = vring->pkt_len;
 715         fifo->vring[is_rx] = vring;
 716 }
 717
 718 /*
 719  * Rx/Tx one descriptor.
 720  *
 721  * Return true to indicate more data available.
 722  */
 723 static bool mlxbf_tmfifo_rxtx_one_desc(struct mlxbf_tmfifo_vring *vring,
 724                                        bool is_rx, int *avail)
 725 {
 726         const struct vring *vr = virtqueue_get_vring(vring->vq);
 727         struct mlxbf_tmfifo *fifo = vring->fifo;
 728         struct virtio_device *vdev;
 729         bool vring_change = false;
 730         struct vring_desc *desc;
 731         unsigned long flags;
 732         u32 len, idx;
 733
 734         vdev = &fifo->vdev[vring->vdev_id]->vdev;
 735
 736         /* Get the descriptor of the next packet. */
 737         if (!vring->desc) {
 738                 desc = mlxbf_tmfifo_get_next_pkt(vring, is_rx);
 739                 if (!desc) {
 740                         /* Drop next Rx packet to avoid stuck. */
 741                         if (is_rx) {
 742                                 desc = &vring->drop_desc;
 743                                 vring->desc_head = desc;
 744                                 vring->desc = desc;
 745                         } else {
 746                                 return false;
 747                         }
 748                 }
 749         } else {
 750                 desc = vring->desc;
 751         }
 752
 753         /* Beginning of a packet. Start to Rx/Tx packet header. */
 754         if (vring->pkt_len == 0) {
 755                 mlxbf_tmfifo_rxtx_header(vring, &desc, is_rx, &vring_change);
 756                 (*avail)--;
 757
 758                 /* Return if new packet is for another ring. */
 759                 if (vring_change)
 760                         return false;
 761                 goto mlxbf_tmfifo_desc_done;
 762         }
 763
 764         /* Get the length of this desc. */
 765         len = virtio32_to_cpu(vdev, desc->len);
 766         if (len > vring->rem_len)
 767                 len = vring->rem_len;
 768
 769         /* Rx/Tx one word (8 bytes) if not done. */
 770         if (vring->cur_len < len) {
 771                 mlxbf_tmfifo_rxtx_word(vring, desc, is_rx, len);
 772                 (*avail)--;
 773         }
 774
 775         /* Check again whether it's done. */
 776         if (vring->cur_len == len) {
 777                 vring->cur_len = 0;
 778                 vring->rem_len -= len;
 779
 780                 /* Get the next desc on the chain. */
 781                 if (!IS_VRING_DROP(vring) && vring->rem_len > 0 &&
 782                     (virtio16_to_cpu(vdev, desc->flags) & VRING_DESC_F_NEXT)) {
 783                         idx = virtio16_to_cpu(vdev, desc->next);
 784                         desc = &vr->desc[idx];
 785                         goto mlxbf_tmfifo_desc_done;
 786                 }
 787
 788                 /* Done and release the packet. */
 789                 desc = NULL;
 790                 fifo->vring[is_rx] = NULL;
 791                 if (!IS_VRING_DROP(vring)) {
 792                         mlxbf_tmfifo_release_pkt(vring);
 793                 } else {
 794                         vring->pkt_len = 0;
 795                         vring->desc_head = NULL;
 796                         vring->desc = NULL;
 797                         return false;
 798                 }
 799
 800                 /*
 801                  * Make sure the load/store are in order before
 802                  * returning back to virtio.
 803                  */
 804                 virtio_mb(false);
 805
 806                 /* Notify upper layer that packet is done. */
 807                 spin_lock_irqsave(&fifo->spin_lock[is_rx], flags);
 808                 vring_interrupt(0, vring->vq);
 809                 spin_unlock_irqrestore(&fifo->spin_lock[is_rx], flags);
 810         }
 811
 812 mlxbf_tmfifo_desc_done:
 813         /* Save the current desc. */
 814         vring->desc = desc;
 815
 816         return true;
 817 }
 818
 819 /* Rx & Tx processing of a queue. */
 820 static void mlxbf_tmfifo_rxtx(struct mlxbf_tmfifo_vring *vring, bool is_rx)
 821 {
 822         int avail = 0, devid = vring->vdev_id;
 823         struct mlxbf_tmfifo *fifo;
 824         bool more;
 825
 826         fifo = vring->fifo;
 827
 828         /* Return if vdev is not ready. */
 829         if (!fifo || !fifo->vdev[devid])
 830                 return;
 831
 832         /* Return if another vring is running. */
 833         if (fifo->vring[is_rx] && fifo->vring[is_rx] != vring)
 834                 return;
 835
 836         /* Only handle console and network for now. */
 837         if (WARN_ON(devid != VIRTIO_ID_NET && devid != VIRTIO_ID_CONSOLE))
 838                 return;
 839
 840         do {
 841                 /* Get available FIFO space. */
 842                 if (avail == 0) {
 843                         if (is_rx)
 844                                 avail = mlxbf_tmfifo_get_rx_avail(fifo);
 845                         else
 846                                 avail = mlxbf_tmfifo_get_tx_avail(fifo, devid);
 847                         if (avail <= 0)
 848                                 break;
 849                 }
 850
 851                 /* Console output always comes from the Tx buffer. */
 852                 if (!is_rx && devid == VIRTIO_ID_CONSOLE) {
 853                         mlxbf_tmfifo_console_tx(fifo, avail);
 854                         break;
 855                 }
 856
 857                 /* Handle one descriptor. */
 858                 more = mlxbf_tmfifo_rxtx_one_desc(vring, is_rx, &avail);
 859         } while (more);
 860 }
 861
 862 /* Handle Rx or Tx queues. */
 863 static void mlxbf_tmfifo_work_rxtx(struct mlxbf_tmfifo *fifo, int queue_id,
 864                                    int irq_id, bool is_rx)
 865 {
 866         struct mlxbf_tmfifo_vdev *tm_vdev;
 867         struct mlxbf_tmfifo_vring *vring;
 868         int i;
 869
 870         if (!test_and_clear_bit(irq_id, &fifo->pend_events) ||
 871             !fifo->irq_info[irq_id].irq)
 872                 return;
 873
 874         for (i = 0; i < MLXBF_TMFIFO_VDEV_MAX; i++) {
 875                 tm_vdev = fifo->vdev[i];
 876                 if (tm_vdev) {
 877                         vring = &tm_vdev->vrings[queue_id];
 878                         if (vring->vq)
 879                                 mlxbf_tmfifo_rxtx(vring, is_rx);
 880                 }
 881         }
 882 }
 883
 884 /* Work handler for Rx and Tx case. */
 885 static void mlxbf_tmfifo_work_handler(struct work_struct *work)
 886 {
 887         struct mlxbf_tmfifo *fifo;
 888
 889         fifo = container_of(work, struct mlxbf_tmfifo, work);
 890         if (!fifo->is_ready)
 891                 return;
 892
 893         mutex_lock(&fifo->lock);
 894
 895         /* Tx (Send data to the TmFifo). */
 896         mlxbf_tmfifo_work_rxtx(fifo, MLXBF_TMFIFO_VRING_TX,
 897                                MLXBF_TM_TX_LWM_IRQ, false);
 898
 899         /* Rx (Receive data from the TmFifo). */
 900         mlxbf_tmfifo_work_rxtx(fifo, MLXBF_TMFIFO_VRING_RX,
 901                                MLXBF_TM_RX_HWM_IRQ, true);
 902
 903         mutex_unlock(&fifo->lock);
 904 }
 905
 906 /* The notify function is called when new buffers are posted. */
 907 static bool mlxbf_tmfifo_virtio_notify(struct virtqueue *vq)
 908 {
 909         struct mlxbf_tmfifo_vring *vring = vq->priv;
 910         struct mlxbf_tmfifo_vdev *tm_vdev;
 911         struct mlxbf_tmfifo *fifo;
 912         unsigned long flags;
 913
 914         fifo = vring->fifo;
 915
 916         /*
 917          * Virtio maintains vrings in pairs, even number ring for Rx
 918          * and odd number ring for Tx.
 919          */
 920         if (vring->index & BIT(0)) {
 921                 /*
 922                  * Console could make blocking call with interrupts disabled.
 923                  * In such case, the vring needs to be served right away. For
 924                  * other cases, just set the TX LWM bit to start Tx in the
 925                  * worker handler.
 926                  */
 927                 if (vring->vdev_id == VIRTIO_ID_CONSOLE) {
 928                         spin_lock_irqsave(&fifo->spin_lock[0], flags);
 929                         tm_vdev = fifo->vdev[VIRTIO_ID_CONSOLE];
 930                         mlxbf_tmfifo_console_output(tm_vdev, vring);
 931                         spin_unlock_irqrestore(&fifo->spin_lock[0], flags);
 932                         set_bit(MLXBF_TM_TX_LWM_IRQ, &fifo->pend_events);
 933                 } else if (test_and_set_bit(MLXBF_TM_TX_LWM_IRQ,
 934                                             &fifo->pend_events)) {
 935                         return true;
 936                 }
 937         } else {
 938                 if (test_and_set_bit(MLXBF_TM_RX_HWM_IRQ, &fifo->pend_events))
 939                         return true;
 940         }
 941
 942         schedule_work(&fifo->work);
 943
 944         return true;
 945 }
 946
 947 /* Get the array of feature bits for this device. */
 948 static u64 mlxbf_tmfifo_virtio_get_features(struct virtio_device *vdev)
 949 {
 950         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
 951
 952         return tm_vdev->features;
 953 }
 954
 955 /* Confirm device features to use. */
 956 static int mlxbf_tmfifo_virtio_finalize_features(struct virtio_device *vdev)
 957 {
 958         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
 959
 960         tm_vdev->features = vdev->features;
 961
 962         return 0;
 963 }
 964
 965 /* Free virtqueues found by find_vqs(). */
 966 static void mlxbf_tmfifo_virtio_del_vqs(struct virtio_device *vdev)
 967 {
 968         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
 969         struct mlxbf_tmfifo_vring *vring;
 970         struct virtqueue *vq;
 971         int i;
 972
 973         for (i = 0; i < ARRAY_SIZE(tm_vdev->vrings); i++) {
 974                 vring = &tm_vdev->vrings[i];
 975
 976                 /* Release the pending packet. */
 977                 if (vring->desc)
 978                         mlxbf_tmfifo_release_pkt(vring);
 979                 vq = vring->vq;
 980                 if (vq) {
 981                         vring->vq = NULL;
 982                         vring_del_virtqueue(vq);
 983                 }
 984         }
 985 }
 986
 987 /* Create and initialize the virtual queues. */
 988 static int mlxbf_tmfifo_virtio_find_vqs(struct virtio_device *vdev,
 989                                         unsigned int nvqs,
 990                                         struct virtqueue *vqs[],
 991                                         vq_callback_t *callbacks[],
 992                                         const char * const names[],
 993                                         const bool *ctx,
 994                                         struct irq_affinity *desc)
 995 {
 996         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
 997         struct mlxbf_tmfifo_vring *vring;
 998         struct virtqueue *vq;
 999         int i, ret, size;
1000
1001         if (nvqs > ARRAY_SIZE(tm_vdev->vrings))
1002                 return -EINVAL;
1003
1004         for (i = 0; i < nvqs; ++i) {
1005                 if (!names[i]) {
1006                         ret = -EINVAL;
1007                         goto error;
1008                 }
1009                 vring = &tm_vdev->vrings[i];
1010
1011                 /* zero vring */
1012                 size = vring_size(vring->num, vring->align);
1013                 memset(vring->va, 0, size);
1014                 vq = vring_new_virtqueue(i, vring->num, vring->align, vdev,
1015                                          false, false, vring->va,
1016                                          mlxbf_tmfifo_virtio_notify,
1017                                          callbacks[i], names[i]);
1018                 if (!vq) {
1019                         dev_err(&vdev->dev, "vring_new_virtqueue failed\n");
1020                         ret = -ENOMEM;
1021                         goto error;
1022                 }
1023
1024                 vq->num_max = vring->num;
1025
1026                 vq->priv = vring;
1027
1028                 /* Make vq update visible before using it. */
1029                 virtio_mb(false);
1030
1031                 vqs[i] = vq;
1032                 vring->vq = vq;
1033         }
1034
1035         return 0;
1036
1037 error:
1038         mlxbf_tmfifo_virtio_del_vqs(vdev);
1039         return ret;
1040 }
1041
1042 /* Read the status byte. */
1043 static u8 mlxbf_tmfifo_virtio_get_status(struct virtio_device *vdev)
1044 {
1045         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1046
1047         return tm_vdev->status;
1048 }
1049
1050 /* Write the status byte. */
1051 static void mlxbf_tmfifo_virtio_set_status(struct virtio_device *vdev,
1052                                            u8 status)
1053 {
1054         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1055
1056         tm_vdev->status = status;
1057 }
1058
1059 /* Reset the device. Not much here for now. */
1060 static void mlxbf_tmfifo_virtio_reset(struct virtio_device *vdev)
1061 {
1062         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1063
1064         tm_vdev->status = 0;
1065 }
1066
1067 /* Read the value of a configuration field. */
1068 static void mlxbf_tmfifo_virtio_get(struct virtio_device *vdev,
1069                                     unsigned int offset,
1070                                     void *buf,
1071                                     unsigned int len)
1072 {
1073         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1074
1075         if ((u64)offset + len > sizeof(tm_vdev->config))
1076                 return;
1077
1078         memcpy(buf, (u8 *)&tm_vdev->config + offset, len);
1079 }
1080
1081 /* Write the value of a configuration field. */
1082 static void mlxbf_tmfifo_virtio_set(struct virtio_device *vdev,
1083                                     unsigned int offset,
1084                                     const void *buf,
1085                                     unsigned int len)
1086 {
1087         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1088
1089         if ((u64)offset + len > sizeof(tm_vdev->config))
1090                 return;
1091
1092         memcpy((u8 *)&tm_vdev->config + offset, buf, len);
1093 }
1094
1095 static void tmfifo_virtio_dev_release(struct device *device)
1096 {
1097         struct virtio_device *vdev =
1098                         container_of(device, struct virtio_device, dev);
1099         struct mlxbf_tmfifo_vdev *tm_vdev = mlxbf_vdev_to_tmfifo(vdev);
1100
1101         kfree(tm_vdev);
1102 }
1103
1104 /* Virtio config operations. */
1105 static const struct virtio_config_ops mlxbf_tmfifo_virtio_config_ops = {
1106         .get_features = mlxbf_tmfifo_virtio_get_features,
1107         .finalize_features = mlxbf_tmfifo_virtio_finalize_features,
1108         .find_vqs = mlxbf_tmfifo_virtio_find_vqs,
1109         .del_vqs = mlxbf_tmfifo_virtio_del_vqs,
1110         .reset = mlxbf_tmfifo_virtio_reset,
1111         .set_status = mlxbf_tmfifo_virtio_set_status,
1112         .get_status = mlxbf_tmfifo_virtio_get_status,
1113         .get = mlxbf_tmfifo_virtio_get,
1114         .set = mlxbf_tmfifo_virtio_set,
1115 };
1116
1117 /* Create vdev for the FIFO. */
1118 static int mlxbf_tmfifo_create_vdev(struct device *dev,
1119                                     struct mlxbf_tmfifo *fifo,
1120                                     int vdev_id, u64 features,
1121                                     void *config, u32 size)
1122 {
1123         struct mlxbf_tmfifo_vdev *tm_vdev, *reg_dev = NULL;
1124         int ret;
1125
1126         mutex_lock(&fifo->lock);
1127
1128         tm_vdev = fifo->vdev[vdev_id];
1129         if (tm_vdev) {
1130                 dev_err(dev, "vdev %d already exists\n", vdev_id);
1131                 ret = -EEXIST;
1132                 goto fail;
1133         }
1134
1135         tm_vdev = kzalloc(sizeof(*tm_vdev), GFP_KERNEL);
1136         if (!tm_vdev) {
1137                 ret = -ENOMEM;
1138                 goto fail;
1139         }
1140
1141         tm_vdev->vdev.id.device = vdev_id;
1142         tm_vdev->vdev.config = &mlxbf_tmfifo_virtio_config_ops;
1143         tm_vdev->vdev.dev.parent = dev;
1144         tm_vdev->vdev.dev.release = tmfifo_virtio_dev_release;
1145         tm_vdev->features = features;
1146         if (config)
1147                 memcpy(&tm_vdev->config, config, size);
1148
1149         if (mlxbf_tmfifo_alloc_vrings(fifo, tm_vdev)) {
1150                 dev_err(dev, "unable to allocate vring\n");
1151                 ret = -ENOMEM;
1152                 goto vdev_fail;
1153         }
1154
1155         /* Allocate an output buffer for the console device. */
1156         if (vdev_id == VIRTIO_ID_CONSOLE)
1157                 tm_vdev->tx_buf.buf = devm_kmalloc(dev,
1158                                                    MLXBF_TMFIFO_CON_TX_BUF_SIZE,
1159                                                    GFP_KERNEL);
1160         fifo->vdev[vdev_id] = tm_vdev;
1161
1162         /* Register the virtio device. */
1163         ret = register_virtio_device(&tm_vdev->vdev);
1164         reg_dev = tm_vdev;
1165         if (ret) {
1166                 dev_err(dev, "register_virtio_device failed\n");
1167                 goto vdev_fail;
1168         }
1169
1170         mutex_unlock(&fifo->lock);
1171         return 0;
1172
1173 vdev_fail:
1174         mlxbf_tmfifo_free_vrings(fifo, tm_vdev);
1175         fifo->vdev[vdev_id] = NULL;
1176         if (reg_dev)
1177                 put_device(&tm_vdev->vdev.dev);
1178         else
1179                 kfree(tm_vdev);
1180 fail:
1181         mutex_unlock(&fifo->lock);
1182         return ret;
1183 }
1184
1185 /* Delete vdev for the FIFO. */
1186 static int mlxbf_tmfifo_delete_vdev(struct mlxbf_tmfifo *fifo, int vdev_id)
1187 {
1188         struct mlxbf_tmfifo_vdev *tm_vdev;
1189
1190         mutex_lock(&fifo->lock);
1191
1192         /* Unregister vdev. */
1193         tm_vdev = fifo->vdev[vdev_id];
1194         if (tm_vdev) {
1195                 unregister_virtio_device(&tm_vdev->vdev);
1196                 mlxbf_tmfifo_free_vrings(fifo, tm_vdev);
1197                 fifo->vdev[vdev_id] = NULL;
1198         }
1199
1200         mutex_unlock(&fifo->lock);
1201
1202         return 0;
1203 }
1204
1205 /* Read the configured network MAC address from efi variable. */
1206 static void mlxbf_tmfifo_get_cfg_mac(u8 *mac)
1207 {
1208         efi_guid_t guid = EFI_GLOBAL_VARIABLE_GUID;
1209         unsigned long size = ETH_ALEN;
1210         u8 buf[ETH_ALEN];
1211         efi_status_t rc;
1212
1213         rc = efi.get_variable(mlxbf_tmfifo_efi_name, &guid, NULL, &size, buf);
1214         if (rc == EFI_SUCCESS && size == ETH_ALEN)
1215                 ether_addr_copy(mac, buf);
1216         else
1217                 ether_addr_copy(mac, mlxbf_tmfifo_net_default_mac);
1218 }
1219
1220 /* Set TmFifo thresolds which is used to trigger interrupts. */
1221 static void mlxbf_tmfifo_set_threshold(struct mlxbf_tmfifo *fifo)
1222 {
1223         u64 ctl;
1224
1225         /* Get Tx FIFO size and set the low/high watermark. */
1226         ctl = readq(fifo->tx.ctl);
1227         fifo->tx_fifo_size =
1228                 FIELD_GET(MLXBF_TMFIFO_TX_CTL__MAX_ENTRIES_MASK, ctl);
1229         ctl = (ctl & ~MLXBF_TMFIFO_TX_CTL__LWM_MASK) |
1230                 FIELD_PREP(MLXBF_TMFIFO_TX_CTL__LWM_MASK,
1231                            fifo->tx_fifo_size / 2);
1232         ctl = (ctl & ~MLXBF_TMFIFO_TX_CTL__HWM_MASK) |
1233                 FIELD_PREP(MLXBF_TMFIFO_TX_CTL__HWM_MASK,
1234                            fifo->tx_fifo_size - 1);
1235         writeq(ctl, fifo->tx.ctl);
1236
1237         /* Get Rx FIFO size and set the low/high watermark. */
1238         ctl = readq(fifo->rx.ctl);
1239         fifo->rx_fifo_size =
1240                 FIELD_GET(MLXBF_TMFIFO_RX_CTL__MAX_ENTRIES_MASK, ctl);
1241         ctl = (ctl & ~MLXBF_TMFIFO_RX_CTL__LWM_MASK) |
1242                 FIELD_PREP(MLXBF_TMFIFO_RX_CTL__LWM_MASK, 0);
1243         ctl = (ctl & ~MLXBF_TMFIFO_RX_CTL__HWM_MASK) |
1244                 FIELD_PREP(MLXBF_TMFIFO_RX_CTL__HWM_MASK, 1);
1245         writeq(ctl, fifo->rx.ctl);
1246 }
1247
1248 static void mlxbf_tmfifo_cleanup(struct mlxbf_tmfifo *fifo)
1249 {
1250         int i;
1251
1252         fifo->is_ready = false;
1253         del_timer_sync(&fifo->timer);
1254         mlxbf_tmfifo_disable_irqs(fifo);
1255         cancel_work_sync(&fifo->work);
1256         for (i = 0; i < MLXBF_TMFIFO_VDEV_MAX; i++)
1257                 mlxbf_tmfifo_delete_vdev(fifo, i);
1258 }
1259
1260 /* Probe the TMFIFO. */
1261 static int mlxbf_tmfifo_probe(struct platform_device *pdev)
1262 {
1263         struct virtio_net_config net_config;
1264         struct device *dev = &pdev->dev;
1265         struct mlxbf_tmfifo *fifo;
1266         u64 dev_id;
1267         int i, rc;
1268
1269         rc = acpi_dev_uid_to_integer(ACPI_COMPANION(dev), &dev_id);
1270         if (rc) {
1271                 dev_err(dev, "Cannot retrieve UID\n");
1272                 return rc;
1273         }
1274
1275         fifo = devm_kzalloc(dev, sizeof(*fifo), GFP_KERNEL);
1276         if (!fifo)
1277                 return -ENOMEM;
1278
1279         spin_lock_init(&fifo->spin_lock[0]);
1280         spin_lock_init(&fifo->spin_lock[1]);
1281         INIT_WORK(&fifo->work, mlxbf_tmfifo_work_handler);
1282         mutex_init(&fifo->lock);
1283
1284         /* Get the resource of the Rx FIFO. */
1285         fifo->res0 = devm_platform_ioremap_resource(pdev, 0);
1286         if (IS_ERR(fifo->res0))
1287                 return PTR_ERR(fifo->res0);
1288
1289         /* Get the resource of the Tx FIFO. */
1290         fifo->res1 = devm_platform_ioremap_resource(pdev, 1);
1291         if (IS_ERR(fifo->res1))
1292                 return PTR_ERR(fifo->res1);
1293
1294         if (dev_id == TMFIFO_BF3_UID) {
1295                 fifo->rx.ctl = fifo->res1 + MLXBF_TMFIFO_RX_CTL_BF3;
1296                 fifo->rx.sts = fifo->res1 + MLXBF_TMFIFO_RX_STS_BF3;
1297                 fifo->rx.data = fifo->res0 + MLXBF_TMFIFO_RX_DATA_BF3;
1298                 fifo->tx.ctl = fifo->res1 + MLXBF_TMFIFO_TX_CTL_BF3;
1299                 fifo->tx.sts = fifo->res1 + MLXBF_TMFIFO_TX_STS_BF3;
1300                 fifo->tx.data = fifo->res0 + MLXBF_TMFIFO_TX_DATA_BF3;
1301         } else {
1302                 fifo->rx.ctl = fifo->res0 + MLXBF_TMFIFO_RX_CTL;
1303                 fifo->rx.sts = fifo->res0 + MLXBF_TMFIFO_RX_STS;
1304                 fifo->rx.data = fifo->res0 + MLXBF_TMFIFO_RX_DATA;
1305                 fifo->tx.ctl = fifo->res1 + MLXBF_TMFIFO_TX_CTL;
1306                 fifo->tx.sts = fifo->res1 + MLXBF_TMFIFO_TX_STS;
1307                 fifo->tx.data = fifo->res1 + MLXBF_TMFIFO_TX_DATA;
1308         }
1309
1310         platform_set_drvdata(pdev, fifo);
1311
1312         timer_setup(&fifo->timer, mlxbf_tmfifo_timer, 0);
1313
1314         for (i = 0; i < MLXBF_TM_MAX_IRQ; i++) {
1315                 fifo->irq_info[i].index = i;
1316                 fifo->irq_info[i].fifo = fifo;
1317                 fifo->irq_info[i].irq = platform_get_irq(pdev, i);
1318                 rc = devm_request_irq(dev, fifo->irq_info[i].irq,
1319                                       mlxbf_tmfifo_irq_handler, 0,
1320                                       "tmfifo", &fifo->irq_info[i]);
1321                 if (rc) {
1322                         dev_err(dev, "devm_request_irq failed\n");
1323                         fifo->irq_info[i].irq = 0;
1324                         return rc;
1325                 }
1326         }
1327
1328         mlxbf_tmfifo_set_threshold(fifo);
1329
1330         /* Create the console vdev. */
1331         rc = mlxbf_tmfifo_create_vdev(dev, fifo, VIRTIO_ID_CONSOLE, 0, NULL, 0);
1332         if (rc)
1333                 goto fail;
1334
1335         /* Create the network vdev. */
1336         memset(&net_config, 0, sizeof(net_config));
1337
1338         /* A legacy-only interface for now. */
1339         net_config.mtu = __cpu_to_virtio16(virtio_legacy_is_little_endian(),
1340                                            ETH_DATA_LEN);
1341         net_config.status = __cpu_to_virtio16(virtio_legacy_is_little_endian(),
1342                                               VIRTIO_NET_S_LINK_UP);
1343         mlxbf_tmfifo_get_cfg_mac(net_config.mac);
1344         rc = mlxbf_tmfifo_create_vdev(dev, fifo, VIRTIO_ID_NET,
1345                                       MLXBF_TMFIFO_NET_FEATURES, &net_config,
1346                                       sizeof(net_config));
1347         if (rc)
1348                 goto fail;
1349
1350         mod_timer(&fifo->timer, jiffies + MLXBF_TMFIFO_TIMER_INTERVAL);
1351
1352         /* Make all updates visible before setting the 'is_ready' flag. */
1353         virtio_mb(false);
1354
1355         fifo->is_ready = true;
1356         return 0;
1357
1358 fail:
1359         mlxbf_tmfifo_cleanup(fifo);
1360         return rc;
1361 }
1362
1363 /* Device remove function. */
1364 static int mlxbf_tmfifo_remove(struct platform_device *pdev)
1365 {
1366         struct mlxbf_tmfifo *fifo = platform_get_drvdata(pdev);
1367
1368         mlxbf_tmfifo_cleanup(fifo);
1369
1370         return 0;
1371 }
1372
1373 static const struct acpi_device_id mlxbf_tmfifo_acpi_match[] = {
1374         { "MLNXBF01", 0 },
1375         {}
1376 };
1377 MODULE_DEVICE_TABLE(acpi, mlxbf_tmfifo_acpi_match);
1378
1379 static struct platform_driver mlxbf_tmfifo_driver = {
1380         .probe = mlxbf_tmfifo_probe,
1381         .remove = mlxbf_tmfifo_remove,
1382         .driver = {
1383                 .name = "bf-tmfifo",
1384                 .acpi_match_table = mlxbf_tmfifo_acpi_match,
1385         },
1386 };
1387
1388 module_platform_driver(mlxbf_tmfifo_driver);
1389
1390 MODULE_DESCRIPTION("Mellanox BlueField SoC TmFifo Driver");
1391 MODULE_LICENSE("GPL v2");
1392 MODULE_AUTHOR("Mellanox Technologies");