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, epf->vfunc_no, src_phys_addr,
251 			       reg->src_addr, 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, epf->vfunc_no, dst_phys_addr,
267 			       reg->dst_addr, 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, epf->vfunc_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, epf->vfunc_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, epf->vfunc_no, phys_addr,
335 			       reg->src_addr, 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, epf->vfunc_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, epf->vfunc_no, phys_addr,
423 			       reg->dst_addr, 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, epf->vfunc_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, epf->vfunc_no,
505 				  PCI_EPC_IRQ_LEGACY, 0);
506 		break;
507 	case IRQ_TYPE_MSI:
508 		pci_epc_raise_irq(epc, epf->func_no, epf->vfunc_no,
509 				  PCI_EPC_IRQ_MSI, irq);
510 		break;
511 	case IRQ_TYPE_MSIX:
512 		pci_epc_raise_irq(epc, epf->func_no, epf->vfunc_no,
513 				  PCI_EPC_IRQ_MSIX, irq);
514 		break;
515 	default:
516 		dev_err(dev, "Failed to raise IRQ, unknown type\n");
517 		break;
518 	}
519 }
520 
521 static void pci_epf_test_cmd_handler(struct work_struct *work)
522 {
523 	int ret;
524 	int count;
525 	u32 command;
526 	struct pci_epf_test *epf_test = container_of(work, struct pci_epf_test,
527 						     cmd_handler.work);
528 	struct pci_epf *epf = epf_test->epf;
529 	struct device *dev = &epf->dev;
530 	struct pci_epc *epc = epf->epc;
531 	enum pci_barno test_reg_bar = epf_test->test_reg_bar;
532 	struct pci_epf_test_reg *reg = epf_test->reg[test_reg_bar];
533 
534 	command = reg->command;
535 	if (!command)
536 		goto reset_handler;
537 
538 	reg->command = 0;
539 	reg->status = 0;
540 
541 	if (reg->irq_type > IRQ_TYPE_MSIX) {
542 		dev_err(dev, "Failed to detect IRQ type\n");
543 		goto reset_handler;
544 	}
545 
546 	if (command & COMMAND_RAISE_LEGACY_IRQ) {
547 		reg->status = STATUS_IRQ_RAISED;
548 		pci_epc_raise_irq(epc, epf->func_no, epf->vfunc_no,
549 				  PCI_EPC_IRQ_LEGACY, 0);
550 		goto reset_handler;
551 	}
552 
553 	if (command & COMMAND_WRITE) {
554 		ret = pci_epf_test_write(epf_test);
555 		if (ret)
556 			reg->status |= STATUS_WRITE_FAIL;
557 		else
558 			reg->status |= STATUS_WRITE_SUCCESS;
559 		pci_epf_test_raise_irq(epf_test, reg->irq_type,
560 				       reg->irq_number);
561 		goto reset_handler;
562 	}
563 
564 	if (command & COMMAND_READ) {
565 		ret = pci_epf_test_read(epf_test);
566 		if (!ret)
567 			reg->status |= STATUS_READ_SUCCESS;
568 		else
569 			reg->status |= STATUS_READ_FAIL;
570 		pci_epf_test_raise_irq(epf_test, reg->irq_type,
571 				       reg->irq_number);
572 		goto reset_handler;
573 	}
574 
575 	if (command & COMMAND_COPY) {
576 		ret = pci_epf_test_copy(epf_test);
577 		if (!ret)
578 			reg->status |= STATUS_COPY_SUCCESS;
579 		else
580 			reg->status |= STATUS_COPY_FAIL;
581 		pci_epf_test_raise_irq(epf_test, reg->irq_type,
582 				       reg->irq_number);
583 		goto reset_handler;
584 	}
585 
586 	if (command & COMMAND_RAISE_MSI_IRQ) {
587 		count = pci_epc_get_msi(epc, epf->func_no, epf->vfunc_no);
588 		if (reg->irq_number > count || count <= 0)
589 			goto reset_handler;
590 		reg->status = STATUS_IRQ_RAISED;
591 		pci_epc_raise_irq(epc, epf->func_no, epf->vfunc_no,
592 				  PCI_EPC_IRQ_MSI, reg->irq_number);
593 		goto reset_handler;
594 	}
595 
596 	if (command & COMMAND_RAISE_MSIX_IRQ) {
597 		count = pci_epc_get_msix(epc, epf->func_no, epf->vfunc_no);
598 		if (reg->irq_number > count || count <= 0)
599 			goto reset_handler;
600 		reg->status = STATUS_IRQ_RAISED;
601 		pci_epc_raise_irq(epc, epf->func_no, epf->vfunc_no,
602 				  PCI_EPC_IRQ_MSIX, reg->irq_number);
603 		goto reset_handler;
604 	}
605 
606 reset_handler:
607 	queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
608 			   msecs_to_jiffies(1));
609 }
610 
611 static void pci_epf_test_unbind(struct pci_epf *epf)
612 {
613 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
614 	struct pci_epc *epc = epf->epc;
615 	struct pci_epf_bar *epf_bar;
616 	int bar;
617 
618 	cancel_delayed_work(&epf_test->cmd_handler);
619 	pci_epf_test_clean_dma_chan(epf_test);
620 	pci_epc_stop(epc);
621 	for (bar = 0; bar < PCI_STD_NUM_BARS; bar++) {
622 		epf_bar = &epf->bar[bar];
623 
624 		if (epf_test->reg[bar]) {
625 			pci_epc_clear_bar(epc, epf->func_no, epf->vfunc_no,
626 					  epf_bar);
627 			pci_epf_free_space(epf, epf_test->reg[bar], bar,
628 					   PRIMARY_INTERFACE);
629 		}
630 	}
631 }
632 
633 static int pci_epf_test_set_bar(struct pci_epf *epf)
634 {
635 	int bar, add;
636 	int ret;
637 	struct pci_epf_bar *epf_bar;
638 	struct pci_epc *epc = epf->epc;
639 	struct device *dev = &epf->dev;
640 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
641 	enum pci_barno test_reg_bar = epf_test->test_reg_bar;
642 	const struct pci_epc_features *epc_features;
643 
644 	epc_features = epf_test->epc_features;
645 
646 	for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
647 		epf_bar = &epf->bar[bar];
648 		/*
649 		 * pci_epc_set_bar() sets PCI_BASE_ADDRESS_MEM_TYPE_64
650 		 * if the specific implementation required a 64-bit BAR,
651 		 * even if we only requested a 32-bit BAR.
652 		 */
653 		add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
654 
655 		if (!!(epc_features->reserved_bar & (1 << bar)))
656 			continue;
657 
658 		ret = pci_epc_set_bar(epc, epf->func_no, epf->vfunc_no,
659 				      epf_bar);
660 		if (ret) {
661 			pci_epf_free_space(epf, epf_test->reg[bar], bar,
662 					   PRIMARY_INTERFACE);
663 			dev_err(dev, "Failed to set BAR%d\n", bar);
664 			if (bar == test_reg_bar)
665 				return ret;
666 		}
667 	}
668 
669 	return 0;
670 }
671 
672 static int pci_epf_test_core_init(struct pci_epf *epf)
673 {
674 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
675 	struct pci_epf_header *header = epf->header;
676 	const struct pci_epc_features *epc_features;
677 	struct pci_epc *epc = epf->epc;
678 	struct device *dev = &epf->dev;
679 	bool msix_capable = false;
680 	bool msi_capable = true;
681 	int ret;
682 
683 	epc_features = pci_epc_get_features(epc, epf->func_no, epf->vfunc_no);
684 	if (epc_features) {
685 		msix_capable = epc_features->msix_capable;
686 		msi_capable = epc_features->msi_capable;
687 	}
688 
689 	if (epf->vfunc_no <= 1) {
690 		ret = pci_epc_write_header(epc, epf->func_no, epf->vfunc_no, header);
691 		if (ret) {
692 			dev_err(dev, "Configuration header write failed\n");
693 			return ret;
694 		}
695 	}
696 
697 	ret = pci_epf_test_set_bar(epf);
698 	if (ret)
699 		return ret;
700 
701 	if (msi_capable) {
702 		ret = pci_epc_set_msi(epc, epf->func_no, epf->vfunc_no,
703 				      epf->msi_interrupts);
704 		if (ret) {
705 			dev_err(dev, "MSI configuration failed\n");
706 			return ret;
707 		}
708 	}
709 
710 	if (msix_capable) {
711 		ret = pci_epc_set_msix(epc, epf->func_no, epf->vfunc_no,
712 				       epf->msix_interrupts,
713 				       epf_test->test_reg_bar,
714 				       epf_test->msix_table_offset);
715 		if (ret) {
716 			dev_err(dev, "MSI-X configuration failed\n");
717 			return ret;
718 		}
719 	}
720 
721 	return 0;
722 }
723 
724 static int pci_epf_test_notifier(struct notifier_block *nb, unsigned long val,
725 				 void *data)
726 {
727 	struct pci_epf *epf = container_of(nb, struct pci_epf, nb);
728 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
729 	int ret;
730 
731 	switch (val) {
732 	case CORE_INIT:
733 		ret = pci_epf_test_core_init(epf);
734 		if (ret)
735 			return NOTIFY_BAD;
736 		break;
737 
738 	case LINK_UP:
739 		queue_delayed_work(kpcitest_workqueue, &epf_test->cmd_handler,
740 				   msecs_to_jiffies(1));
741 		break;
742 
743 	default:
744 		dev_err(&epf->dev, "Invalid EPF test notifier event\n");
745 		return NOTIFY_BAD;
746 	}
747 
748 	return NOTIFY_OK;
749 }
750 
751 static int pci_epf_test_alloc_space(struct pci_epf *epf)
752 {
753 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
754 	struct device *dev = &epf->dev;
755 	struct pci_epf_bar *epf_bar;
756 	size_t msix_table_size = 0;
757 	size_t test_reg_bar_size;
758 	size_t pba_size = 0;
759 	bool msix_capable;
760 	void *base;
761 	int bar, add;
762 	enum pci_barno test_reg_bar = epf_test->test_reg_bar;
763 	const struct pci_epc_features *epc_features;
764 	size_t test_reg_size;
765 
766 	epc_features = epf_test->epc_features;
767 
768 	test_reg_bar_size = ALIGN(sizeof(struct pci_epf_test_reg), 128);
769 
770 	msix_capable = epc_features->msix_capable;
771 	if (msix_capable) {
772 		msix_table_size = PCI_MSIX_ENTRY_SIZE * epf->msix_interrupts;
773 		epf_test->msix_table_offset = test_reg_bar_size;
774 		/* Align to QWORD or 8 Bytes */
775 		pba_size = ALIGN(DIV_ROUND_UP(epf->msix_interrupts, 8), 8);
776 	}
777 	test_reg_size = test_reg_bar_size + msix_table_size + pba_size;
778 
779 	if (epc_features->bar_fixed_size[test_reg_bar]) {
780 		if (test_reg_size > bar_size[test_reg_bar])
781 			return -ENOMEM;
782 		test_reg_size = bar_size[test_reg_bar];
783 	}
784 
785 	base = pci_epf_alloc_space(epf, test_reg_size, test_reg_bar,
786 				   epc_features->align, PRIMARY_INTERFACE);
787 	if (!base) {
788 		dev_err(dev, "Failed to allocated register space\n");
789 		return -ENOMEM;
790 	}
791 	epf_test->reg[test_reg_bar] = base;
792 
793 	for (bar = 0; bar < PCI_STD_NUM_BARS; bar += add) {
794 		epf_bar = &epf->bar[bar];
795 		add = (epf_bar->flags & PCI_BASE_ADDRESS_MEM_TYPE_64) ? 2 : 1;
796 
797 		if (bar == test_reg_bar)
798 			continue;
799 
800 		if (!!(epc_features->reserved_bar & (1 << bar)))
801 			continue;
802 
803 		base = pci_epf_alloc_space(epf, bar_size[bar], bar,
804 					   epc_features->align,
805 					   PRIMARY_INTERFACE);
806 		if (!base)
807 			dev_err(dev, "Failed to allocate space for BAR%d\n",
808 				bar);
809 		epf_test->reg[bar] = base;
810 	}
811 
812 	return 0;
813 }
814 
815 static void pci_epf_configure_bar(struct pci_epf *epf,
816 				  const struct pci_epc_features *epc_features)
817 {
818 	struct pci_epf_bar *epf_bar;
819 	bool bar_fixed_64bit;
820 	int i;
821 
822 	for (i = 0; i < PCI_STD_NUM_BARS; i++) {
823 		epf_bar = &epf->bar[i];
824 		bar_fixed_64bit = !!(epc_features->bar_fixed_64bit & (1 << i));
825 		if (bar_fixed_64bit)
826 			epf_bar->flags |= PCI_BASE_ADDRESS_MEM_TYPE_64;
827 		if (epc_features->bar_fixed_size[i])
828 			bar_size[i] = epc_features->bar_fixed_size[i];
829 	}
830 }
831 
832 static int pci_epf_test_bind(struct pci_epf *epf)
833 {
834 	int ret;
835 	struct pci_epf_test *epf_test = epf_get_drvdata(epf);
836 	const struct pci_epc_features *epc_features;
837 	enum pci_barno test_reg_bar = BAR_0;
838 	struct pci_epc *epc = epf->epc;
839 	bool linkup_notifier = false;
840 	bool core_init_notifier = false;
841 
842 	if (WARN_ON_ONCE(!epc))
843 		return -EINVAL;
844 
845 	epc_features = pci_epc_get_features(epc, epf->func_no, epf->vfunc_no);
846 	if (!epc_features) {
847 		dev_err(&epf->dev, "epc_features not implemented\n");
848 		return -EOPNOTSUPP;
849 	}
850 
851 	linkup_notifier = epc_features->linkup_notifier;
852 	core_init_notifier = epc_features->core_init_notifier;
853 	test_reg_bar = pci_epc_get_first_free_bar(epc_features);
854 	if (test_reg_bar < 0)
855 		return -EINVAL;
856 	pci_epf_configure_bar(epf, epc_features);
857 
858 	epf_test->test_reg_bar = test_reg_bar;
859 	epf_test->epc_features = epc_features;
860 
861 	ret = pci_epf_test_alloc_space(epf);
862 	if (ret)
863 		return ret;
864 
865 	if (!core_init_notifier) {
866 		ret = pci_epf_test_core_init(epf);
867 		if (ret)
868 			return ret;
869 	}
870 
871 	epf_test->dma_supported = true;
872 
873 	ret = pci_epf_test_init_dma_chan(epf_test);
874 	if (ret)
875 		epf_test->dma_supported = false;
876 
877 	if (linkup_notifier) {
878 		epf->nb.notifier_call = pci_epf_test_notifier;
879 		pci_epc_register_notifier(epc, &epf->nb);
880 	} else {
881 		queue_work(kpcitest_workqueue, &epf_test->cmd_handler.work);
882 	}
883 
884 	return 0;
885 }
886 
887 static const struct pci_epf_device_id pci_epf_test_ids[] = {
888 	{
889 		.name = "pci_epf_test",
890 	},
891 	{},
892 };
893 
894 static int pci_epf_test_probe(struct pci_epf *epf)
895 {
896 	struct pci_epf_test *epf_test;
897 	struct device *dev = &epf->dev;
898 
899 	epf_test = devm_kzalloc(dev, sizeof(*epf_test), GFP_KERNEL);
900 	if (!epf_test)
901 		return -ENOMEM;
902 
903 	epf->header = &test_header;
904 	epf_test->epf = epf;
905 
906 	INIT_DELAYED_WORK(&epf_test->cmd_handler, pci_epf_test_cmd_handler);
907 
908 	epf_set_drvdata(epf, epf_test);
909 	return 0;
910 }
911 
912 static struct pci_epf_ops ops = {
913 	.unbind	= pci_epf_test_unbind,
914 	.bind	= pci_epf_test_bind,
915 };
916 
917 static struct pci_epf_driver test_driver = {
918 	.driver.name	= "pci_epf_test",
919 	.probe		= pci_epf_test_probe,
920 	.id_table	= pci_epf_test_ids,
921 	.ops		= &ops,
922 	.owner		= THIS_MODULE,
923 };
924 
925 static int __init pci_epf_test_init(void)
926 {
927 	int ret;
928 
929 	kpcitest_workqueue = alloc_workqueue("kpcitest",
930 					     WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
931 	if (!kpcitest_workqueue) {
932 		pr_err("Failed to allocate the kpcitest work queue\n");
933 		return -ENOMEM;
934 	}
935 
936 	ret = pci_epf_register_driver(&test_driver);
937 	if (ret) {
938 		destroy_workqueue(kpcitest_workqueue);
939 		pr_err("Failed to register pci epf test driver --> %d\n", ret);
940 		return ret;
941 	}
942 
943 	return 0;
944 }
945 module_init(pci_epf_test_init);
946 
947 static void __exit pci_epf_test_exit(void)
948 {
949 	if (kpcitest_workqueue)
950 		destroy_workqueue(kpcitest_workqueue);
951 	pci_epf_unregister_driver(&test_driver);
952 }
953 module_exit(pci_epf_test_exit);
954 
955 MODULE_DESCRIPTION("PCI EPF TEST DRIVER");
956 MODULE_AUTHOR("Kishon Vijay Abraham I <kishon@ti.com>");
957 MODULE_LICENSE("GPL v2");
958