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                 void *buf;
 289
 290                 buf = kzalloc(reg->size, GFP_KERNEL);
 291                 if (!buf) {
 292                         ret = -ENOMEM;
 293                         goto err_map_addr;
 294                 }
 295
 296                 memcpy_fromio(buf, src_addr, reg->size);
 297                 memcpy_toio(dst_addr, buf, reg->size);
 298                 kfree(buf);
 299         }
 300         ktime_get_ts64(&end);
 301         pci_epf_test_print_rate("COPY", reg->size, &start, &end, use_dma);
 302
 303 err_map_addr:
 304         pci_epc_unmap_addr(epc, epf->func_no, dst_phys_addr);
 305
 306 err_dst_addr:
 307         pci_epc_mem_free_addr(epc, dst_phys_addr, dst_addr, reg->size);
 308
 309 err_src_map_addr:
 310         pci_epc_unmap_addr(epc, epf->func_no, src_phys_addr);
 311
 312 err_src_addr:
 313         pci_epc_mem_free_addr(epc, src_phys_addr, src_addr, reg->size);
 314
 315 err:
 316         return ret;
 317 }
 318
 319 static int pci_epf_test_read(struct pci_epf_test *epf_test)
 320 {
 321         int ret;
 322         void __iomem *src_addr;
 323         void *buf;
 324         u32 crc32;
 325         bool use_dma;
 326         phys_addr_t phys_addr;
 327         phys_addr_t dst_phys_addr;
 328         struct timespec64 start, end;
 329         struct pci_epf *epf = epf_test->epf;
 330         struct device *dev = &epf->dev;
 331         struct pci_epc *epc = epf->epc;
 332         struct device *dma_dev = epf->epc->dev.parent;
 333         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 334         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 335
 336         src_addr = pci_epc_mem_alloc_addr(epc, &phys_addr, reg->size);
 337         if (!src_addr) {
 338                 dev_err(dev, "Failed to allocate address\n");
 339                 reg->status = STATUS_SRC_ADDR_INVALID;
 340                 ret = -ENOMEM;
 341                 goto err;
 342         }
 343
 344         ret = pci_epc_map_addr(epc, epf->func_no, phys_addr, reg->src_addr,
 345                                reg->size);
 346         if (ret) {
 347                 dev_err(dev, "Failed to map address\n");
 348                 reg->status = STATUS_SRC_ADDR_INVALID;
 349                 goto err_addr;
 350         }
 351
 352         buf = kzalloc(reg->size, GFP_KERNEL);
 353         if (!buf) {
 354                 ret = -ENOMEM;
 355                 goto err_map_addr;
 356         }
 357
 358         use_dma = !!(reg->flags & FLAG_USE_DMA);
 359         if (use_dma) {
 360                 if (!epf_test->dma_supported) {
 361                         dev_err(dev, "Cannot transfer data using DMA\n");
 362                         ret = -EINVAL;
 363                         goto err_dma_map;
 364                 }
 365
 366                 dst_phys_addr = dma_map_single(dma_dev, buf, reg->size,
 367                                                DMA_FROM_DEVICE);
 368                 if (dma_mapping_error(dma_dev, dst_phys_addr)) {
 369                         dev_err(dev, "Failed to map destination buffer addr\n");
 370                         ret = -ENOMEM;
 371                         goto err_dma_map;
 372                 }
 373
 374                 ktime_get_ts64(&start);
 375                 ret = pci_epf_test_data_transfer(epf_test, dst_phys_addr,
 376                                                  phys_addr, reg->size);
 377                 if (ret)
 378                         dev_err(dev, "Data transfer failed\n");
 379                 ktime_get_ts64(&end);
 380
 381                 dma_unmap_single(dma_dev, dst_phys_addr, reg->size,
 382                                  DMA_FROM_DEVICE);
 383         } else {
 384                 ktime_get_ts64(&start);
 385                 memcpy_fromio(buf, src_addr, reg->size);
 386                 ktime_get_ts64(&end);
 387         }
 388
 389         pci_epf_test_print_rate("READ", reg->size, &start, &end, use_dma);
 390
 391         crc32 = crc32_le(~0, buf, reg->size);
 392         if (crc32 != reg->checksum)
 393                 ret = -EIO;
 394
 395 err_dma_map:
 396         kfree(buf);
 397
 398 err_map_addr:
 399         pci_epc_unmap_addr(epc, epf->func_no, phys_addr);
 400
 401 err_addr:
 402         pci_epc_mem_free_addr(epc, phys_addr, src_addr, reg->size);
 403
 404 err:
 405         return ret;
 406 }
 407
 408 static int pci_epf_test_write(struct pci_epf_test *epf_test)
 409 {
 410         int ret;
 411         void __iomem *dst_addr;
 412         void *buf;
 413         bool use_dma;
 414         phys_addr_t phys_addr;
 415         phys_addr_t src_phys_addr;
 416         struct timespec64 start, end;
 417         struct pci_epf *epf = epf_test->epf;
 418         struct device *dev = &epf->dev;
 419         struct pci_epc *epc = epf->epc;
 420         struct device *dma_dev = epf->epc->dev.parent;
 421         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 422         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 423
 424         dst_addr = pci_epc_mem_alloc_addr(epc, &phys_addr, reg->size);
 425         if (!dst_addr) {
 426                 dev_err(dev, "Failed to allocate address\n");
 427                 reg->status = STATUS_DST_ADDR_INVALID;
 428                 ret = -ENOMEM;
 429                 goto err;
 430         }
 431
 432         ret = pci_epc_map_addr(epc, epf->func_no, phys_addr, reg->dst_addr,
 433                                reg->size);
 434         if (ret) {
 435                 dev_err(dev, "Failed to map address\n");
 436                 reg->status = STATUS_DST_ADDR_INVALID;
 437                 goto err_addr;
 438         }
 439
 440         buf = kzalloc(reg->size, GFP_KERNEL);
 441         if (!buf) {
 442                 ret = -ENOMEM;
 443                 goto err_map_addr;
 444         }
 445
 446         get_random_bytes(buf, reg->size);
 447         reg->checksum = crc32_le(~0, buf, reg->size);
 448
 449         use_dma = !!(reg->flags & FLAG_USE_DMA);
 450         if (use_dma) {
 451                 if (!epf_test->dma_supported) {
 452                         dev_err(dev, "Cannot transfer data using DMA\n");
 453                         ret = -EINVAL;
 454                         goto err_dma_map;
 455                 }
 456
 457                 src_phys_addr = dma_map_single(dma_dev, buf, reg->size,
 458                                                DMA_TO_DEVICE);
 459                 if (dma_mapping_error(dma_dev, src_phys_addr)) {
 460                         dev_err(dev, "Failed to map source buffer addr\n");
 461                         ret = -ENOMEM;
 462                         goto err_dma_map;
 463                 }
 464
 465                 ktime_get_ts64(&start);
 466                 ret = pci_epf_test_data_transfer(epf_test, phys_addr,
 467                                                  src_phys_addr, reg->size);
 468                 if (ret)
 469                         dev_err(dev, "Data transfer failed\n");
 470                 ktime_get_ts64(&end);
 471
 472                 dma_unmap_single(dma_dev, src_phys_addr, reg->size,
 473                                  DMA_TO_DEVICE);
 474         } else {
 475                 ktime_get_ts64(&start);
 476                 memcpy_toio(dst_addr, buf, reg->size);
 477                 ktime_get_ts64(&end);
 478         }
 479
 480         pci_epf_test_print_rate("WRITE", reg->size, &start, &end, use_dma);
 481
 482         /*
 483          * wait 1ms inorder for the write to complete. Without this delay L3
 484          * error in observed in the host system.
 485          */
 486         usleep_range(1000, 2000);
 487
 488 err_dma_map:
 489         kfree(buf);
 490
 491 err_map_addr:
 492         pci_epc_unmap_addr(epc, epf->func_no, phys_addr);
 493
 494 err_addr:
 495         pci_epc_mem_free_addr(epc, phys_addr, dst_addr, reg->size);
 496
 497 err:
 498         return ret;
 499 }
 500
 501 static void pci_epf_test_raise_irq(struct pci_epf_test *epf_test, u8 irq_type,
 502                                    u16 irq)
 503 {
 504         struct pci_epf *epf = epf_test->epf;
 505         struct device *dev = &epf->dev;
 506         struct pci_epc *epc = epf->epc;
 507         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 508         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 509
 510         reg->status |= STATUS_IRQ_RAISED;
 511
 512         switch (irq_type) {
 513         case IRQ_TYPE_LEGACY:
 514                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_LEGACY, 0);
 515                 break;
 516         case IRQ_TYPE_MSI:
 517                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSI, irq);
 518                 break;
 519         case IRQ_TYPE_MSIX:
 520                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSIX, irq);
 521                 break;
 522         default:
 523                 dev_err(dev, "Failed to raise IRQ, unknown type\n");
 524                 break;
 525         }
 526 }
 527
 528 static void pci_epf_test_cmd_handler(struct work_struct *work)
 529 {
 530         int ret;
 531         int count;
 532         u32 command;
 533         struct pci_epf_test *epf_test = container_of(work, struct pci_epf_test,
 534                                                      cmd_handler.work);
 535         struct pci_epf *epf = epf_test->epf;
 536         struct device *dev = &epf->dev;
 537         struct pci_epc *epc = epf->epc;
 538         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 539         struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
 540
 541         command = reg->command;
 542         if (!command)
 543                 goto reset_handler;
 544
 545         reg->command = 0;
 546         reg->status = 0;
 547
 548         if (reg->irq_type > IRQ_TYPE_MSIX) {
 549                 dev_err(dev, "Failed to detect IRQ type\n");
 550                 goto reset_handler;
 551         }
 552
 553         if (command & COMMAND_RAISE_LEGACY_IRQ) {
 554                 reg->status = STATUS_IRQ_RAISED;
 555                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_LEGACY, 0);
 556                 goto reset_handler;
 557         }
 558
 559         if (command & COMMAND_WRITE) {
 560                 ret = pci_epf_test_write(epf_test);
 561                 if (ret)
 562                         reg->status |= STATUS_WRITE_FAIL;
 563                 else
 564                         reg->status |= STATUS_WRITE_SUCCESS;
 565                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 566                                        reg->irq_number);
 567                 goto reset_handler;
 568         }
 569
 570         if (command & COMMAND_READ) {
 571                 ret = pci_epf_test_read(epf_test);
 572                 if (!ret)
 573                         reg->status |= STATUS_READ_SUCCESS;
 574                 else
 575                         reg->status |= STATUS_READ_FAIL;
 576                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 577                                        reg->irq_number);
 578                 goto reset_handler;
 579         }
 580
 581         if (command & COMMAND_COPY) {
 582                 ret = pci_epf_test_copy(epf_test);
 583                 if (!ret)
 584                         reg->status |= STATUS_COPY_SUCCESS;
 585                 else
 586                         reg->status |= STATUS_COPY_FAIL;
 587                 pci_epf_test_raise_irq(epf_test, reg->irq_type,
 588                                        reg->irq_number);
 589                 goto reset_handler;
 590         }
 591
 592         if (command & COMMAND_RAISE_MSI_IRQ) {
 593                 count = pci_epc_get_msi(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_MSI,
 598                                   reg->irq_number);
 599                 goto reset_handler;
 600         }
 601
 602         if (command & COMMAND_RAISE_MSIX_IRQ) {
 603                 count = pci_epc_get_msix(epc, epf->func_no);
 604                 if (reg->irq_number > count || count <= 0)
 605                         goto reset_handler;
 606                 reg->status = STATUS_IRQ_RAISED;
 607                 pci_epc_raise_irq(epc, epf->func_no, PCI_EPC_IRQ_MSIX,
 608                                   reg->irq_number);
 609                 goto reset_handler;
 610         }
 611
 612 reset_handler:
 613         queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
 614                            msecs_to_jiffies(1));
 615 }
 616
 617 static void pci_epf_test_unbind(struct pci_epf *epf)
 618 {
 619         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 620         struct pci_epc *epc = epf->epc;
 621         struct pci_epf_bar *epf_bar;
 622         int bar;
 623
 624         cancel_delayed_work(&epf_test->cmd_handler);
 625         pci_epf_test_clean_dma_chan(epf_test);
 626         for (bar = 0; bar < PCI_STD_NUM_BARS; bar++) {
 627                 epf_bar = &epf->bar[bar];
 628
 629                 if (epf_test->reg[bar]) {
 630                         pci_epc_clear_bar(epc, epf->func_no, epf_bar);
 631                         pci_epf_free_space(epf, epf_test->reg[bar], bar);
 632                 }
 633         }
 634 }
 635
 636 static int pci_epf_test_set_bar(struct pci_epf *epf)
 637 {
 638         int bar, add;
 639         int ret;
 640         struct pci_epf_bar *epf_bar;
 641         struct pci_epc *epc = epf->epc;
 642         struct device *dev = &epf->dev;
 643         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 644         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 645         const struct pci_epc_features *epc_features;
 646
 647         epc_features = epf_test->epc_features;
 648
 649         for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
 650                 epf_bar = &epf->bar[bar];
 651                 /*
 652                  * pci_epc_set_bar() sets PCI_BASE_ADDRESS_MEM_TYPE_64
 653                  * if the specific implementation required a 64-bit BAR,
 654                  * even if we only requested a 32-bit BAR.
 655                  */
 656                 add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
 657
 658                 if (!!(epc_features->reserved_bar & (1 << bar)))
 659                         continue;
 660
 661                 ret = pci_epc_set_bar(epc, epf->func_no, epf_bar);
 662                 if (ret) {
 663                         pci_epf_free_space(epf, epf_test->reg[bar], bar);
 664                         dev_err(dev, "Failed to set BAR%d\n", bar);
 665                         if (bar == test_reg_bar)
 666                                 return ret;
 667                 }
 668         }
 669
 670         return 0;
 671 }
 672
 673 static int pci_epf_test_core_init(struct pci_epf *epf)
 674 {
 675         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 676         struct pci_epf_header *header = epf->header;
 677         const struct pci_epc_features *epc_features;
 678         struct pci_epc *epc = epf->epc;
 679         struct device *dev = &epf->dev;
 680         bool msix_capable = false;
 681         bool msi_capable = true;
 682         int ret;
 683
 684         epc_features = pci_epc_get_features(epc, epf->func_no);
 685         if (epc_features) {
 686                 msix_capable = epc_features->msix_capable;
 687                 msi_capable = epc_features->msi_capable;
 688         }
 689
 690         ret = pci_epc_write_header(epc, epf->func_no, header);
 691         if (ret) {
 692                 dev_err(dev, "Configuration header write failed\n");
 693                 return ret;
 694         }
 695
 696         ret = pci_epf_test_set_bar(epf);
 697         if (ret)
 698                 return ret;
 699
 700         if (msi_capable) {
 701                 ret = pci_epc_set_msi(epc, epf->func_no, epf->msi_interrupts);
 702                 if (ret) {
 703                         dev_err(dev, "MSI configuration failed\n");
 704                         return ret;
 705                 }
 706         }
 707
 708         if (msix_capable) {
 709                 ret = pci_epc_set_msix(epc, epf->func_no, epf->msix_interrupts,
 710                                        epf_test->test_reg_bar,
 711                                        epf_test->msix_table_offset);
 712                 if (ret) {
 713                         dev_err(dev, "MSI-X configuration failed\n");
 714                         return ret;
 715                 }
 716         }
 717
 718         return 0;
 719 }
 720
 721 static int pci_epf_test_notifier(struct notifier_block *nb, unsigned long val,
 722                                  void *data)
 723 {
 724         struct pci_epf *epf = container_of(nb, struct pci_epf, nb);
 725         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 726         int ret;
 727
 728         switch (val) {
 729         case CORE_INIT:
 730                 ret = pci_epf_test_core_init(epf);
 731                 if (ret)
 732                         return NOTIFY_BAD;
 733                 break;
 734
 735         case LINK_UP:
 736                 queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
 737                                    msecs_to_jiffies(1));
 738                 break;
 739
 740         default:
 741                 dev_err(&epf->dev, "Invalid EPF test notifier event\n");
 742                 return NOTIFY_BAD;
 743         }
 744
 745         return NOTIFY_OK;
 746 }
 747
 748 static int pci_epf_test_alloc_space(struct pci_epf *epf)
 749 {
 750         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 751         struct device *dev = &epf->dev;
 752         struct pci_epf_bar *epf_bar;
 753         size_t msix_table_size = 0;
 754         size_t test_reg_bar_size;
 755         size_t pba_size = 0;
 756         bool msix_capable;
 757         void *base;
 758         int bar, add;
 759         enum pci_barno test_reg_bar = epf_test->test_reg_bar;
 760         const struct pci_epc_features *epc_features;
 761         size_t test_reg_size;
 762
 763         epc_features = epf_test->epc_features;
 764
 765         test_reg_bar_size = ALIGN(sizeof(struct pci_epf_test_reg), 128);
 766
 767         msix_capable = epc_features->msix_capable;
 768         if (msix_capable) {
 769                 msix_table_size = PCI_MSIX_ENTRY_SIZE * epf->msix_interrupts;
 770                 epf_test->msix_table_offset = test_reg_bar_size;
 771                 /* Align to QWORD or 8 Bytes */
 772                 pba_size = ALIGN(DIV_ROUND_UP(epf->msix_interrupts, 8), 8);
 773         }
 774         test_reg_size = test_reg_bar_size + msix_table_size + pba_size;
 775
 776         if (epc_features->bar_fixed_size[test_reg_bar]) {
 777                 if (test_reg_size > bar_size[test_reg_bar])
 778                         return -ENOMEM;
 779                 test_reg_size = bar_size[test_reg_bar];
 780         }
 781
 782         base = pci_epf_alloc_space(epf, test_reg_size, test_reg_bar,
 783                                    epc_features->align);
 784         if (!base) {
 785                 dev_err(dev, "Failed to allocated register space\n");
 786                 return -ENOMEM;
 787         }
 788         epf_test->reg[test_reg_bar] = base;
 789
 790         for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
 791                 epf_bar = &epf->bar[bar];
 792                 add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
 793
 794                 if (bar == test_reg_bar)
 795                         continue;
 796
 797                 if (!!(epc_features->reserved_bar & (1 << bar)))
 798                         continue;
 799
 800                 base = pci_epf_alloc_space(epf, bar_size[bar], bar,
 801                                            epc_features->align);
 802                 if (!base)
 803                         dev_err(dev, "Failed to allocate space for BAR%d\n",
 804                                 bar);
 805                 epf_test->reg[bar] = base;
 806         }
 807
 808         return 0;
 809 }
 810
 811 static void pci_epf_configure_bar(struct pci_epf *epf,
 812                                   const struct pci_epc_features *epc_features)
 813 {
 814         struct pci_epf_bar *epf_bar;
 815         bool bar_fixed_64bit;
 816         int i;
 817
 818         for (i = 0; i < PCI_STD_NUM_BARS; i++) {
 819                 epf_bar = &epf->bar[i];
 820                 bar_fixed_64bit = !!(epc_features->bar_fixed_64bit & (1 << i));
 821                 if (bar_fixed_64bit)
 822                         epf_bar->flags |= PCI_BASE_ADDRESS_MEM_TYPE_64;
 823                 if (epc_features->bar_fixed_size[i])
 824                         bar_size[i] = epc_features->bar_fixed_size[i];
 825         }
 826 }
 827
 828 static int pci_epf_test_bind(struct pci_epf *epf)
 829 {
 830         int ret;
 831         struct pci_epf_test *epf_test = epf_get_drvdata(epf);
 832         const struct pci_epc_features *epc_features;
 833         enum pci_barno test_reg_bar = BAR_0;
 834         struct pci_epc *epc = epf->epc;
 835         bool linkup_notifier = false;
 836         bool core_init_notifier = false;
 837
 838         if (WARN_ON_ONCE(!epc))
 839                 return -EINVAL;
 840
 841         epc_features = pci_epc_get_features(epc, epf->func_no);
 842         if (!epc_features) {
 843                 dev_err(&epf->dev, "epc_features not implemented\n");
 844                 return -EOPNOTSUPP;
 845         }
 846
 847         linkup_notifier = epc_features->linkup_notifier;
 848         core_init_notifier = epc_features->core_init_notifier;
 849         test_reg_bar = pci_epc_get_first_free_bar(epc_features);
 850         if (test_reg_bar < 0)
 851                 return -EINVAL;
 852         pci_epf_configure_bar(epf, epc_features);
 853
 854         epf_test->test_reg_bar = test_reg_bar;
 855         epf_test->epc_features = epc_features;
 856
 857         ret = pci_epf_test_alloc_space(epf);
 858         if (ret)
 859                 return ret;
 860
 861         if (!core_init_notifier) {
 862                 ret = pci_epf_test_core_init(epf);
 863                 if (ret)
 864                         return ret;
 865         }
 866
 867         epf_test->dma_supported = true;
 868
 869         ret = pci_epf_test_init_dma_chan(epf_test);
 870         if (ret)
 871                 epf_test->dma_supported = false;
 872
 873         if (linkup_notifier) {
 874                 epf->nb.notifier_call = pci_epf_test_notifier;
 875                 pci_epc_register_notifier(epc, &epf->nb);
 876         } else {
 877                 queue_work(kpcitest_workqueue, &epf_test->cmd_handler.work);
 878         }
 879
 880         return 0;
 881 }
 882
 883 static const struct pci_epf_device_id pci_epf_test_ids[] = {
 884         {
 885                 .name = "pci_epf_test",
 886         },
 887         {},
 888 };
 889
 890 static int pci_epf_test_probe(struct pci_epf *epf)
 891 {
 892         struct pci_epf_test *epf_test;
 893         struct device *dev = &epf->dev;
 894
 895         epf_test = devm_kzalloc(dev, sizeof(*epf_test), GFP_KERNEL);
 896         if (!epf_test)
 897                 return -ENOMEM;
 898
 899         epf->header = &test_header;
 900         epf_test->epf = epf;
 901
 902         INIT_DELAYED_WORK(&epf_test->cmd_handler, pci_epf_test_cmd_handler);
 903
 904         epf_set_drvdata(epf, epf_test);
 905         return 0;
 906 }
 907
 908 static struct pci_epf_ops ops = {
 909         .unbind = pci_epf_test_unbind,
 910         .bind   = pci_epf_test_bind,
 911 };
 912
 913 static struct pci_epf_driver test_driver = {
 914         .driver.name    = "pci_epf_test",
 915         .probe          = pci_epf_test_probe,
 916         .id_table       = pci_epf_test_ids,
 917         .ops            = &ops,
 918         .owner          = THIS_MODULE,
 919 };
 920
 921 static int __init pci_epf_test_init(void)
 922 {
 923         int ret;
 924
 925         kpcitest_workqueue = alloc_workqueue("kpcitest",
 926                                              WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
 927         if (!kpcitest_workqueue) {
 928                 pr_err("Failed to allocate the kpcitest work queue\n");
 929                 return -ENOMEM;
 930         }
 931
 932         ret = pci_epf_register_driver(&test_driver);
 933         if (ret) {
 934                 destroy_workqueue(kpcitest_workqueue);
 935                 pr_err("Failed to register pci epf test driver --> %d\n", ret);
 936                 return ret;
 937         }
 938
 939         return 0;
 940 }
 941 module_init(pci_epf_test_init);
 942
 943 static void __exit pci_epf_test_exit(void)
 944 {
 945         if (kpcitest_workqueue)
 946                 destroy_workqueue(kpcitest_workqueue);
 947         pci_epf_unregister_driver(&test_driver);
 948 }
 949 module_exit(pci_epf_test_exit);
 950
 951 MODULE_DESCRIPTION("PCI EPF TEST DRIVER");
 952 MODULE_AUTHOR("Kishon Vijay Abraham I <kishon@ti.com>");
 953 MODULE_LICENSE("GPL v2");