drivers/pci/endpoint/functions/pci-epf-test.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /**
   3  * Test driver to test endpoint functionality
   4  *
   5  * Copyright (C) 2017 Texas Instruments
   6  * Author: Kishon Vijay Abraham I <kishon@ti.com>
   7  */
   8
   9 #include <linux/crc32.h>
  10 #include <linux/delay.h>
  11 #include <linux/dmaengine.h>
  12 #include <linux/io.h>
  13 #include <linux/module.h>
  14 #include <linux/slab.h>
  15 #include <linux/pci_ids.h>
  16 #include <linux/random.h>
  17
  18 #include <linux/pci-epc.h>
  19 #include <linux/pci-epf.h>
  20 #include <linux/pci_regs.h>
  21
  22 #define IRQ_TYPE_LEGACY                 0
  23 #define IRQ_TYPE_MSI                    1
  24 #define IRQ_TYPE_MSIX                   2
  25
  26 #define COMMAND_RAISE_LEGACY_IRQ        BIT(0)
  27 #define COMMAND_RAISE_MSI_IRQ           BIT(1)
  28 #define COMMAND_RAISE_MSIX_IRQ          BIT(2)
  29 #define COMMAND_READ                    BIT(3)
  30 #define COMMAND_WRITE                   BIT(4)
  31 #define COMMAND_COPY                    BIT(5)
  32
  33 #define STATUS_READ_SUCCESS             BIT(0)
  34 #define STATUS_READ_FAIL                BIT(1)
  35 #define STATUS_WRITE_SUCCESS            BIT(2)
  36 #define STATUS_WRITE_FAIL               BIT(3)
  37 #define STATUS_COPY_SUCCESS             BIT(4)
  38 #define STATUS_COPY_FAIL                BIT(5)
  39 #define STATUS_IRQ_RAISED               BIT(6)
  40 #define STATUS_SRC_ADDR_INVALID         BIT(7)
  41 #define STATUS_DST_ADDR_INVALID         BIT(8)
  42
  43 #define FLAG_USE_DMA                    BIT(0)
  44
  45 #define TIMER_RESOLUTION                1
  46
  47 static struct workqueue_struct *kpcitest_workqueue;
  48
  49 struct pci_epf_test {
  50         void                    *reg[PCI_STD_NUM_BARS];
  51         struct pci_epf          *epf;
  52         enum pci_barno          test_reg_bar;
  53         size_t                  msix_table_offset;
  54         struct delayed_work     cmd_handler;
  55         struct dma_chan         *dma_chan;
  56         struct completion       transfer_complete;
  57         bool                    dma_supported;
  58         const struct pci_epc_features *epc_features;
  59 };
  60
  61 struct pci_epf_test_reg {
  62         u32     magic;
  63         u32     command;
  64         u32     status;
  65         u64     src_addr;
  66         u64     dst_addr;
  67         u32     size;
  68         u32     checksum;
  69         u32     irq_type;
  70         u32     irq_number;
  71         u32     flags;
  72 } __packed;
  73
  74 static struct pci_epf_header test_header = {
  75         .vendorid       = PCI_ANY_ID,
  76         .deviceid       = PCI_ANY_ID,
  77         .baseclass_code = PCI_CLASS_OTHERS,
  78         .interrupt_pin  = PCI_INTERRUPT_INTA,
  79 };
  80
  81 static size_t bar_size[] = { 512, 512, 1024, 16384, 131072, 1048576 };
  82
  83 static void pci_epf_test_dma_callback(void *param)
  84 {
  85         struct pci_epf_test *epf_test = param;
  86
  87         complete(&epf_test->transfer_complete);
  88 }
  89
  90 /**
  91  * pci_epf_test_data_transfer() - Function that uses dmaengine API to transfer
  92  *                                data between PCIe EP and remote PCIe RC
  93  * @epf_test: the EPF test device that performs the data transfer operation
  94  * @dma_dst: The destination address of the data transfer. It can be a physical
  95  *           address given by pci_epc_mem_alloc_addr or DMA mapping APIs.
  96  * @dma_src: The source address of the data transfer. It can be a physical
  97  *           address given by pci_epc_mem_alloc_addr or DMA mapping APIs.
  98  * @len: The size of the data transfer
  99  *
 100  * Function that uses dmaengine API to transfer data between PCIe EP and remote
 101  * PCIe RC. The source and destination address can be a physical address given
 102  * by pci_epc_mem_alloc_addr or the one obtained using DMA mapping APIs.
 103  *
 104  * The function returns '0' on success and negative value on failure.
 105  */
 106 static int pci_epf_test_data_transfer(struct pci_epf_test *epf_test,
 107                                       dma_addr_t dma_dst, dma_addr_t dma_src,
 108                                       size_t len)
 109 {
 110         enum dma_ctrl_flags flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
 111         struct dma_chan *chan = epf_test->dma_chan;
 112         struct pci_epf *epf = epf_test->epf;
 113         struct dma_async_tx_descriptor *tx;
 114         struct device *dev = &epf->dev;
 115         dma_cookie_t cookie;
 116         int ret;
 117
 118         if (IS_ERR_OR_NULL(chan)) {
 119                 dev_err(dev, "Invalid DMA memcpy channel\n");
 120                 return -EINVAL;
 121         }
 122
 123         tx = dmaengine_prep_dma_memcpy(chan, dma_dst, dma_src, len, flags);
 124         if (!tx) {
 125                 dev_err(dev, "Failed to prepare DMA memcpy\n");
 126                 return -EIO;
 127         }
 128
 129         tx->callback = pci_epf_test_dma_callback;
 130         tx->callback_param = epf_test;
 131         cookie = tx->tx_submit(tx);
 132         reinit_completion(&epf_test->transfer_complete);
 133
 134         ret = dma_submit_error(cookie);
 135         if (ret) {
 136                 dev_err(dev, "Failed to do DMA tx_submit %d\n", cookie);
 137                 return -EIO;
 138         }
 139
 140         dma_async_issue_pending(chan);
 141         ret = wait_for_completion_interruptible(&epf_test->transfer_complete);
 142         if (ret < 0) {
 143                 dmaengine_terminate_sync(chan);
 144                 dev_err(dev, "DMA wait_for_completion_timeout\n");
 145                 return -ETIMEDOUT;
 146         }
 147
 148         return 0;
 149 }
 150
 151 /**
 152  * pci_epf_test_init_dma_chan() - Function to initialize EPF test DMA channel
 153  * @epf_test: the EPF test device that performs data transfer operation
 154  *
 155  * Function to initialize EPF test DMA channel.
 156  */
 157 static int pci_epf_test_init_dma_chan(struct pci_epf_test *epf_test)
 158 {
 159         struct pci_epf *epf = epf_test->epf;
 160         struct device *dev = &epf->dev;
 161         struct dma_chan *dma_chan;
 162         dma_cap_mask_t mask;
 163         int ret;
 164
 165         dma_cap_zero(mask);
 166         dma_cap_set(DMA_MEMCPY, mask);
 167
 168         dma_chan = dma_request_chan_by_mask(&mask);
 169         if (IS_ERR(dma_chan)) {
 170                 ret = PTR_ERR(dma_chan);
 171                 if (ret != -EPROBE_DEFER)
 172                         dev_err(dev, "Failed to get DMA channel\n");
 173                 return ret;
 174         }
 175         init_completion(&epf_test->transfer_complete);
 176
 177         epf_test->dma_chan = dma_chan;
 178
 179         return 0;
 180 }
 181
 182 /**
 183  * pci_epf_test_clean_dma_chan() - Function to cleanup EPF test DMA channel
 184  * @epf_test: the EPF test device that performs data transfer operation
 185  *
 186  * Helper to cleanup EPF test DMA channel.
 187  */
 188 static void pci_epf_test_clean_dma_chan(struct pci_epf_test *epf_test)
 189 {
 190         if (!epf_test->dma_supported)
 191                 return;
 192
 193         dma_release_channel(epf_test->dma_chan);
 194         epf_test->dma_chan = NULL;
 195 }
 196
 197 static void pci_epf_test_print_rate(const char *ops, u64 size,
 198                                     struct timespec64 *start,
 199                                     struct timespec64 *end, bool dma)
 200 {
 201         struct timespec64 ts;
 202         u64 rate, ns;
 203
 204         ts = timespec64_sub(*end, *start);
 205
 206         /* convert both size (stored in 'rate') and time in terms of 'ns' */
 207         ns = timespec64_to_ns(&ts);
 208         rate = size * NSEC_PER_SEC;
 209
 210         /* Divide both size (stored in 'rate') and ns by a common factor */
 211         while (ns > UINT_MAX) {
 212                 rate >>= 1;
 213                 ns >>= 1;
 214         }
 215
 216         if (!ns)
 217                 return;
 218
 219         /* calculate the rate */
 220         do_div(rate, (uint32_t)ns);
 221
 222         pr_info("\n%s => Size: %llu bytes\t DMA: %s\t Time: %llu.%09u seconds\t"
 223                 "Rate: %llu KB/s\n", ops, size, dma ? "YES" : "NO",
 224                 (u64)ts.tv_sec, (u32)ts.tv_nsec, rate / 1024);
 225 }
 226
 227 static int pci_epf_test_copy(struct pci_epf_test *epf_test)
 228 {
 229         int ret;
 230         bool use_dma;
 231         void __iomem *src_addr;
 232         void __iomem *dst_addr;
 233         phys_addr_t src_phys_addr;
 234         phys_addr_t dst_phys_addr;
 235         struct timespec64 start, end;
 236         struct pci_epf *epf = epf_test->epf;
 237         struct device *dev = &epf->dev;
 238         struct pci_epc *epc = epf->epc;
 239         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 240         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 241
 242         src_addr = pci_epc_mem_alloc_addr(epc, &src_phys_addr, reg->size);
 243         if (!src_addr) {
 244                 dev_err(dev, "Failed to allocate source address\n");
 245                 reg->status = STATUS_SRC_ADDR_INVALID;
 246                 ret = -ENOMEM;
 247                 goto err;
 248         }
 249
 250         ret = pci_epc_map_addr(epc, epf->func_no, src_phys_addr, reg->src_addr,
 251                                reg->size);
 252         if (ret) {
 253                 dev_err(dev, "Failed to map source address\n");
 254                 reg->status = STATUS_SRC_ADDR_INVALID;
 255                 goto err_src_addr;
 256         }
 257
 258         dst_addr = pci_epc_mem_alloc_addr(epc, &dst_phys_addr, reg->size);
 259         if (!dst_addr) {
 260                 dev_err(dev, "Failed to allocate destination address\n");
 261                 reg->status = STATUS_DST_ADDR_INVALID;
 262                 ret = -ENOMEM;
 263                 goto err_src_map_addr;
 264         }
 265
 266         ret = pci_epc_map_addr(epc, epf->func_no, dst_phys_addr, reg->dst_addr,
 267                                reg->size);
 268         if (ret) {
 269                 dev_err(dev, "Failed to map destination address\n");
 270                 reg->status = STATUS_DST_ADDR_INVALID;
 271                 goto err_dst_addr;
 272         }
 273
 274         ktime_get_ts64(&start);
 275         use_dma = !!(reg->flags & FLAG_USE_DMA);
 276         if (use_dma) {
 277                 if (!epf_test->dma_supported) {
 278                         dev_err(dev, "Cannot transfer data using DMA\n");
 279                         ret = -EINVAL;
 280                         goto err_map_addr;
 281                 }
 282
 283                 ret = pci_epf_test_data_transfer(epf_test, dst_phys_addr,
 284                                                  src_phys_addr, reg->size);
 285                 if (ret)
 286                         dev_err(dev, "Data transfer failed\n");
 287         } else {
 288                 memcpy(dst_addr, src_addr, reg->size);
 289         }
 290         ktime_get_ts64(&end);
 291         pci_epf_test_print_rate("COPY", reg->size, &start, &end, use_dma);
 292
 293 err_map_addr:
 294         pci_epc_unmap_addr(epc, epf->func_no, dst_phys_addr);
 295
 296 err_dst_addr:
 297         pci_epc_mem_free_addr(epc, dst_phys_addr, dst_addr, reg->size);
 298
 299 err_src_map_addr:
 300         pci_epc_unmap_addr(epc, epf->func_no, src_phys_addr);
 301
 302 err_src_addr:
 303         pci_epc_mem_free_addr(epc, src_phys_addr, src_addr, reg->size);
 304
 305 err:
 306         return ret;
 307 }
 308
 309 static int pci_epf_test_read(struct pci_epf_test *epf_test)
 310 {
 311         int ret;
 312         void __iomem *src_addr;
 313         void *buf;
 314         u32 crc32;
 315         bool use_dma;
 316         phys_addr_t phys_addr;
 317         phys_addr_t dst_phys_addr;
 318         struct timespec64 start, end;
 319         struct pci_epf *epf = epf_test->epf;
 320         struct device *dev = &epf->dev;
 321         struct pci_epc *epc = epf->epc;
 322         struct device *dma_dev = epf->epc->dev.parent;
 323         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 324         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 325
 326         src_addr = pci_epc_mem_alloc_addr(epc, &phys_addr, reg->size);
 327         if (!src_addr) {
 328                 dev_err(dev, "Failed to allocate address\n");
 329                 reg->status = STATUS_SRC_ADDR_INVALID;
 330                 ret = -ENOMEM;
 331                 goto err;
 332         }
 333
 334         ret = pci_epc_map_addr(epc, epf->func_no, phys_addr, reg->src_addr,
 335                                reg->size);
 336         if (ret) {
 337                 dev_err(dev, "Failed to map address\n");
 338                 reg->status = STATUS_SRC_ADDR_INVALID;
 339                 goto err_addr;
 340         }
 341
 342         buf = kzalloc(reg->size, GFP_KERNEL);
 343         if (!buf) {
 344                 ret = -ENOMEM;
 345                 goto err_map_addr;
 346         }
 347
 348         use_dma = !!(reg->flags & FLAG_USE_DMA);
 349         if (use_dma) {
 350                 if (!epf_test->dma_supported) {
 351                         dev_err(dev, "Cannot transfer data using DMA\n");
 352                         ret = -EINVAL;
 353                         goto err_dma_map;
 354                 }
 355
 356                 dst_phys_addr = dma_map_single(dma_dev, buf, reg->size,
 357                                                DMA_FROM_DEVICE);
 358                 if (dma_mapping_error(dma_dev, dst_phys_addr)) {
 359                         dev_err(dev, "Failed to map destination buffer addr\n");
 360                         ret = -ENOMEM;
 361                         goto err_dma_map;
 362                 }
 363
 364                 ktime_get_ts64(&start);
 365                 ret = pci_epf_test_data_transfer(epf_test, dst_phys_addr,
 366                                                  phys_addr, reg->size);
 367                 if (ret)
 368                         dev_err(dev, "Data transfer failed\n");
 369                 ktime_get_ts64(&end);
 370
 371                 dma_unmap_single(dma_dev, dst_phys_addr, reg->size,
 372                                  DMA_FROM_DEVICE);
 373         } else {
 374                 ktime_get_ts64(&start);
 375                 memcpy_fromio(buf, src_addr, reg->size);
 376                 ktime_get_ts64(&end);
 377         }
 378
 379         pci_epf_test_print_rate("READ", reg->size, &start, &end, use_dma);
 380
 381         crc32 = crc32_le(~0, buf, reg->size);
 382         if (crc32 != reg->checksum)
 383                 ret = -EIO;
 384
 385 err_dma_map:
 386         kfree(buf);
 387
 388 err_map_addr:
 389         pci_epc_unmap_addr(epc, epf->func_no, phys_addr);
 390
 391 err_addr:
 392         pci_epc_mem_free_addr(epc, phys_addr, src_addr, reg->size);
 393
 394 err:
 395         return ret;
 396 }
 397
 398 static int pci_epf_test_write(struct pci_epf_test *epf_test)
 399 {
 400         int ret;
 401         void __iomem *dst_addr;
 402         void *buf;
 403         bool use_dma;
 404         phys_addr_t phys_addr;
 405         phys_addr_t src_phys_addr;
 406         struct timespec64 start, end;
 407         struct pci_epf *epf = epf_test->epf;
 408         struct device *dev = &epf->dev;
 409         struct pci_epc *epc = epf->epc;
 410         struct device *dma_dev = epf->epc->dev.parent;
 411         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 412         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 413
 414         dst_addr = pci_epc_mem_alloc_addr(epc, &phys_addr, reg->size);
 415         if (!dst_addr) {
 416                 dev_err(dev, "Failed to allocate address\n");
 417                 reg->status = STATUS_DST_ADDR_INVALID;
 418                 ret = -ENOMEM;
 419                 goto err;
 420         }
 421
 422         ret = pci_epc_map_addr(epc, epf->func_no, phys_addr, reg->dst_addr,
 423                                reg->size);
 424         if (ret) {
 425                 dev_err(dev, "Failed to map address\n");
 426                 reg->status = STATUS_DST_ADDR_INVALID;
 427                 goto err_addr;
 428         }
 429
 430         buf = kzalloc(reg->size, GFP_KERNEL);
 431         if (!buf) {
 432                 ret = -ENOMEM;
 433                 goto err_map_addr;
 434         }
 435
 436         get_random_bytes(buf, reg->size);
 437         reg->checksum = crc32_le(~0, buf, reg->size);
 438
 439         use_dma = !!(reg->flags & FLAG_USE_DMA);
 440         if (use_dma) {
 441                 if (!epf_test->dma_supported) {
 442                         dev_err(dev, "Cannot transfer data using DMA\n");
 443                         ret = -EINVAL;
 444                         goto err_map_addr;
 445                 }
 446
 447                 src_phys_addr = dma_map_single(dma_dev, buf, reg->size,
 448                                                DMA_TO_DEVICE);
 449                 if (dma_mapping_error(dma_dev, src_phys_addr)) {
 450                         dev_err(dev, "Failed to map source buffer addr\n");
 451                         ret = -ENOMEM;
 452                         goto err_dma_map;
 453                 }
 454
 455                 ktime_get_ts64(&start);
 456                 ret = pci_epf_test_data_transfer(epf_test, phys_addr,
 457                                                  src_phys_addr, reg->size);
 458                 if (ret)
 459                         dev_err(dev, "Data transfer failed\n");
 460                 ktime_get_ts64(&end);
 461
 462                 dma_unmap_single(dma_dev, src_phys_addr, reg->size,
 463                                  DMA_TO_DEVICE);
 464         } else {
 465                 ktime_get_ts64(&start);
 466                 memcpy_toio(dst_addr, buf, reg->size);
 467                 ktime_get_ts64(&end);
 468         }
 469
 470         pci_epf_test_print_rate("WRITE", reg->size, &start, &end, use_dma);
 471
 472         /*
 473          * wait 1ms inorder for the write to complete. Without this delay L3
 474          * error in observed in the host system.
 475          */
 476         usleep_range(1000, 2000);
 477
 478 err_dma_map:
 479         kfree(buf);
 480
 481 err_map_addr:
 482         pci_epc_unmap_addr(epc, epf->func_no, phys_addr);
 483
 484 err_addr:
 485         pci_epc_mem_free_addr(epc, phys_addr, dst_addr, reg->size);
 486
 487 err:
 488         return ret;
 489 }
 490
 491 static void pci_epf_test_raise_irq(struct pci_epf_test *epf_test, u8 irq_type,
 492                                    u16 irq)
 493 {
 494         struct pci_epf *epf = epf_test->epf;
 495         struct device *dev = &epf->dev;
 496         struct pci_epc *epc = epf->epc;
 497         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 498         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 499
 500         reg->status |= STATUS_IRQ_RAISED;
 501
 502         switch (irq_type) {
 503         case IRQ_TYPE_LEGACY:
 504                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_LEGACY, 0);
 505                 break;
 506         case IRQ_TYPE_MSI:
 507                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSI, irq);
 508                 break;
 509         case IRQ_TYPE_MSIX:
 510                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSIX, irq);
 511                 break;
 512         default:
 513                 dev_err(dev, "Failed to raise IRQ, unknown type\n");
 514                 break;
 515         }
 516 }
 517
 518 static void pci_epf_test_cmd_handler(struct work_struct *work)
 519 {
 520         int ret;
 521         int count;
 522         u32 command;
 523         struct pci_epf_test *epf_test = container_of(work, struct pci_epf_test,
 524                                                      cmd_handler.work);
 525         struct pci_epf *epf = epf_test->epf;
 526         struct device *dev = &epf->dev;
 527         struct pci_epc *epc = epf->epc;
 528         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 529         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 530
 531         command = reg->command;
 532         if (!command)
 533                 goto reset_handler;
 534
 535         reg->command = 0;
 536         reg->status = 0;
 537
 538         if (reg->irq_type > IRQ_TYPE_MSIX) {
 539                 dev_err(dev, "Failed to detect IRQ type\n");
 540                 goto reset_handler;
 541         }
 542
 543         if (command & COMMAND_RAISE_LEGACY_IRQ) {
 544                 reg->status = STATUS_IRQ_RAISED;
 545                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_LEGACY, 0);
 546                 goto reset_handler;
 547         }
 548
 549         if (command & COMMAND_WRITE) {
 550                 ret = pci_epf_test_write(epf_test);
 551                 if (ret)
 552                         reg->status |= STATUS_WRITE_FAIL;
 553                 else
 554                         reg->status |= STATUS_WRITE_SUCCESS;
 555                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 556                                        reg->irq_number);
 557                 goto reset_handler;
 558         }
 559
 560         if (command & COMMAND_READ) {
 561                 ret = pci_epf_test_read(epf_test);
 562                 if (!ret)
 563                         reg->status |= STATUS_READ_SUCCESS;
 564                 else
 565                         reg->status |= STATUS_READ_FAIL;
 566                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 567                                        reg->irq_number);
 568                 goto reset_handler;
 569         }
 570
 571         if (command & COMMAND_COPY) {
 572                 ret = pci_epf_test_copy(epf_test);
 573                 if (!ret)
 574                         reg->status |= STATUS_COPY_SUCCESS;
 575                 else
 576                         reg->status |= STATUS_COPY_FAIL;
 577                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 578                                        reg->irq_number);
 579                 goto reset_handler;
 580         }
 581
 582         if (command & COMMAND_RAISE_MSI_IRQ) {
 583                 count = pci_epc_get_msi(epc, epf->func_no);
 584                 if (reg->irq_number > count || count <= 0)
 585                         goto reset_handler;
 586                 reg->status = STATUS_IRQ_RAISED;
 587                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSI,
 588                                   reg->irq_number);
 589                 goto reset_handler;
 590         }
 591
 592         if (command & COMMAND_RAISE_MSIX_IRQ) {
 593                 count = pci_epc_get_msix(epc, epf->func_no);
 594                 if (reg->irq_number > count || count <= 0)
 595                         goto reset_handler;
 596                 reg->status = STATUS_IRQ_RAISED;
 597                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSIX,
 598                                   reg->irq_number);
 599                 goto reset_handler;
 600         }
 601
 602 reset_handler:
 603         queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
 604                            msecs_to_jiffies(1));
 605 }
 606
 607 static void pci_epf_test_unbind(struct pci_epf *epf)
 608 {
 609         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 610         struct pci_epc *epc = epf->epc;
 611         struct pci_epf_bar *epf_bar;
 612         int bar;
 613
 614         cancel_delayed_work(&epf_test->cmd_handler);
 615         pci_epf_test_clean_dma_chan(epf_test);
 616         pci_epc_stop(epc);
 617         for (bar = 0; bar < PCI_STD_NUM_BARS; bar++) {
 618                 epf_bar = &epf->bar[bar];
 619
 620                 if (epf_test->reg[bar]) {
 621                         pci_epc_clear_bar(epc, epf->func_no, epf_bar);
 622                         pci_epf_free_space(epf, epf_test->reg[bar], bar);
 623                 }
 624         }
 625 }
 626
 627 static int pci_epf_test_set_bar(struct pci_epf *epf)
 628 {
 629         int bar, add;
 630         int ret;
 631         struct pci_epf_bar *epf_bar;
 632         struct pci_epc *epc = epf->epc;
 633         struct device *dev = &epf->dev;
 634         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 635         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 636         const struct pci_epc_features *epc_features;
 637
 638         epc_features = epf_test->epc_features;
 639
 640         for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
 641                 epf_bar = &epf->bar[bar];
 642                 /*
 643                  * pci_epc_set_bar() sets PCI_BASE_ADDRESS_MEM_TYPE_64
 644                  * if the specific implementation required a 64-bit BAR,
 645                  * even if we only requested a 32-bit BAR.
 646                  */
 647                 add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
 648
 649                 if (!!(epc_features->reserved_bar & (1 << bar)))
 650                         continue;
 651
 652                 ret = pci_epc_set_bar(epc, epf->func_no, epf_bar);
 653                 if (ret) {
 654                         pci_epf_free_space(epf, epf_test->reg[bar], bar);
 655                         dev_err(dev, "Failed to set BAR%d\n", bar);
 656                         if (bar == test_reg_bar)
 657                                 return ret;
 658                 }
 659         }
 660
 661         return 0;
 662 }
 663
 664 static int pci_epf_test_core_init(struct pci_epf *epf)
 665 {
 666         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 667         struct pci_epf_header *header = epf->header;
 668         const struct pci_epc_features *epc_features;
 669         struct pci_epc *epc = epf->epc;
 670         struct device *dev = &epf->dev;
 671         bool msix_capable = false;
 672         bool msi_capable = true;
 673         int ret;
 674
 675         epc_features = pci_epc_get_features(epc, epf->func_no);
 676         if (epc_features) {
 677                 msix_capable = epc_features->msix_capable;
 678                 msi_capable = epc_features->msi_capable;
 679         }
 680
 681         ret = pci_epc_write_header(epc, epf->func_no, header);
 682         if (ret) {
 683                 dev_err(dev, "Configuration header write failed\n");
 684                 return ret;
 685         }
 686
 687         ret = pci_epf_test_set_bar(epf);
 688         if (ret)
 689                 return ret;
 690
 691         if (msi_capable) {
 692                 ret = pci_epc_set_msi(epc, epf->func_no, epf->msi_interrupts);
 693                 if (ret) {
 694                         dev_err(dev, "MSI configuration failed\n");
 695                         return ret;
 696                 }
 697         }
 698
 699         if (msix_capable) {
 700                 ret = pci_epc_set_msix(epc, epf->func_no, epf->msix_interrupts,
 701                                        epf_test->test_reg_bar,
 702                                        epf_test->msix_table_offset);
 703                 if (ret) {
 704                         dev_err(dev, "MSI-X configuration failed\n");
 705                         return ret;
 706                 }
 707         }
 708
 709         return 0;
 710 }
 711
 712 static int pci_epf_test_notifier(struct notifier_block *nb, unsigned long val,
 713                                  void *data)
 714 {
 715         struct pci_epf *epf = container_of(nb, struct pci_epf, nb);
 716         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 717         int ret;
 718
 719         switch (val) {
 720         case CORE_INIT:
 721                 ret = pci_epf_test_core_init(epf);
 722                 if (ret)
 723                         return NOTIFY_BAD;
 724                 break;
 725
 726         case LINK_UP:
 727                 queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
 728                                    msecs_to_jiffies(1));
 729                 break;
 730
 731         default:
 732                 dev_err(&epf->dev, "Invalid EPF test notifier event\n");
 733                 return NOTIFY_BAD;
 734         }
 735
 736         return NOTIFY_OK;
 737 }
 738
 739 static int pci_epf_test_alloc_space(struct pci_epf *epf)
 740 {
 741         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 742         struct device *dev = &epf->dev;
 743         struct pci_epf_bar *epf_bar;
 744         size_t msix_table_size = 0;
 745         size_t test_reg_bar_size;
 746         size_t pba_size = 0;
 747         bool msix_capable;
 748         void *base;
 749         int bar, add;
 750         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 751         const struct pci_epc_features *epc_features;
 752         size_t test_reg_size;
 753
 754         epc_features = epf_test->epc_features;
 755
 756         test_reg_bar_size = ALIGN(sizeof(struct pci_epf_test_reg), 128);
 757
 758         msix_capable = epc_features->msix_capable;
 759         if (msix_capable) {
 760                 msix_table_size = PCI_MSIX_ENTRY_SIZE * epf->msix_interrupts;
 761                 epf_test->msix_table_offset = test_reg_bar_size;
 762                 /* Align to QWORD or 8 Bytes */
 763                 pba_size = ALIGN(DIV_ROUND_UP(epf->msix_interrupts, 8), 8);
 764         }
 765         test_reg_size = test_reg_bar_size + msix_table_size + pba_size;
 766
 767         if (epc_features->bar_fixed_size[test_reg_bar]) {
 768                 if (test_reg_size > bar_size[test_reg_bar])
 769                         return -ENOMEM;
 770                 test_reg_size = bar_size[test_reg_bar];
 771         }
 772
 773         base = pci_epf_alloc_space(epf, test_reg_size, test_reg_bar,
 774                                    epc_features->align);
 775         if (!base) {
 776                 dev_err(dev, "Failed to allocated register space\n");
 777                 return -ENOMEM;
 778         }
 779         epf_test->reg[test_reg_bar] = base;
 780
 781         for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
 782                 epf_bar = &epf->bar[bar];
 783                 add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
 784
 785                 if (bar == test_reg_bar)
 786                         continue;
 787
 788                 if (!!(epc_features->reserved_bar & (1 << bar)))
 789                         continue;
 790
 791                 base = pci_epf_alloc_space(epf, bar_size[bar], bar,
 792                                            epc_features->align);
 793                 if (!base)
 794                         dev_err(dev, "Failed to allocate space for BAR%d\n",
 795                                 bar);
 796                 epf_test->reg[bar] = base;
 797         }
 798
 799         return 0;
 800 }
 801
 802 static void pci_epf_configure_bar(struct pci_epf *epf,
 803                                   const struct pci_epc_features *epc_features)
 804 {
 805         struct pci_epf_bar *epf_bar;
 806         bool bar_fixed_64bit;
 807         int i;
 808
 809         for (i = 0; i < PCI_STD_NUM_BARS; i++) {
 810                 epf_bar = &epf->bar[i];
 811                 bar_fixed_64bit = !!(epc_features->bar_fixed_64bit & (1 << i));
 812                 if (bar_fixed_64bit)
 813                         epf_bar->flags |= PCI_BASE_ADDRESS_MEM_TYPE_64;
 814                 if (epc_features->bar_fixed_size[i])
 815                         bar_size[i] = epc_features->bar_fixed_size[i];
 816         }
 817 }
 818
 819 static int pci_epf_test_bind(struct pci_epf *epf)
 820 {
 821         int ret;
 822         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 823         const struct pci_epc_features *epc_features;
 824         enum pci_barno test_reg_bar = BAR_0;
 825         struct pci_epc *epc = epf->epc;
 826         bool linkup_notifier = false;
 827         bool core_init_notifier = false;
 828
 829         if (WARN_ON_ONCE(!epc))
 830                 return -EINVAL;
 831
 832         epc_features = pci_epc_get_features(epc, epf->func_no);
 833         if (epc_features) {
 834                 linkup_notifier = epc_features->linkup_notifier;
 835                 core_init_notifier = epc_features->core_init_notifier;
 836                 test_reg_bar = pci_epc_get_first_free_bar(epc_features);
 837                 pci_epf_configure_bar(epf, epc_features);
 838         }
 839
 840         epf_test->test_reg_bar = test_reg_bar;
 841         epf_test->epc_features = epc_features;
 842
 843         ret = pci_epf_test_alloc_space(epf);
 844         if (ret)
 845                 return ret;
 846
 847         if (!core_init_notifier) {
 848                 ret = pci_epf_test_core_init(epf);
 849                 if (ret)
 850                         return ret;
 851         }
 852
 853         epf_test->dma_supported = true;
 854
 855         ret = pci_epf_test_init_dma_chan(epf_test);
 856         if (ret)
 857                 epf_test->dma_supported = false;
 858
 859         if (linkup_notifier) {
 860                 epf->nb.notifier_call = pci_epf_test_notifier;
 861                 pci_epc_register_notifier(epc, &epf->nb);
 862         } else {
 863                 queue_work(kpcitest_workqueue, &epf_test->cmd_handler.work);
 864         }
 865
 866         return 0;
 867 }
 868
 869 static const struct pci_epf_device_id pci_epf_test_ids[] = {
 870         {
 871                 .name = "pci_epf_test",
 872         },
 873         {},
 874 };
 875
 876 static int pci_epf_test_probe(struct pci_epf *epf)
 877 {
 878         struct pci_epf_test *epf_test;
 879         struct device *dev = &epf->dev;
 880
 881         epf_test = devm_kzalloc(dev, sizeof(*epf_test), GFP_KERNEL);
 882         if (!epf_test)
 883                 return -ENOMEM;
 884
 885         epf->header = &test_header;
 886         epf_test->epf = epf;
 887
 888         INIT_DELAYED_WORK(&epf_test->cmd_handler, pci_epf_test_cmd_handler);
 889
 890         epf_set_drvdata(epf, epf_test);
 891         return 0;
 892 }
 893
 894 static struct pci_epf_ops ops = {
 895         .unbind = pci_epf_test_unbind,
 896         .bind   = pci_epf_test_bind,
 897 };
 898
 899 static struct pci_epf_driver test_driver = {
 900         .driver.name    = "pci_epf_test",
 901         .probe          = pci_epf_test_probe,
 902         .id_table       = pci_epf_test_ids,
 903         .ops            = &ops,
 904         .owner          = THIS_MODULE,
 905 };
 906
 907 static int __init pci_epf_test_init(void)
 908 {
 909         int ret;
 910
 911         kpcitest_workqueue = alloc_workqueue("kpcitest",
 912                                              WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
 913         if (!kpcitest_workqueue) {
 914                 pr_err("Failed to allocate the kpcitest work queue\n");
 915                 return -ENOMEM;
 916         }
 917
 918         ret = pci_epf_register_driver(&test_driver);
 919         if (ret) {
 920                 pr_err("Failed to register pci epf test driver --> %d\n", ret);
 921                 return ret;
 922         }
 923
 924         return 0;
 925 }
 926 module_init(pci_epf_test_init);
 927
 928 static void __exit pci_epf_test_exit(void)
 929 {
 930         pci_epf_unregister_driver(&test_driver);
 931 }
 932 module_exit(pci_epf_test_exit);
 933
 934 MODULE_DESCRIPTION("PCI EPF TEST DRIVER");
 935 MODULE_AUTHOR("Kishon Vijay Abraham I <kishon@ti.com>");
 936 MODULE_LICENSE("GPL v2");