add driver 5G

hc

2024-01-31 f9004dbfff8a3fbbd7e2a88c8a4327c7f2f8e5b2

 kernel/drivers/virtio/virtio_pci_modern.c
..
..
@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
1
2
 /*
2
3
  * Virtio PCI driver - modern (virtio 1.0) device support
3
4
  *
..
..
@@ -11,14 +12,11 @@
11
12
  *  Anthony Liguori  <aliguori@us.ibm.com>
12
13
  *  Rusty Russell <rusty@rustcorp.com.au>
13
14
  *  Michael S. Tsirkin <mst@redhat.com>
14
- *
15
- * This work is licensed under the terms of the GNU GPL, version 2 or later.
16
- * See the COPYING file in the top-level directory.
17
- *
18
15
  */
19
16
 
20
17
 #include <linux/delay.h>
21
18
 #define VIRTIO_PCI_NO_LEGACY
19
+#define VIRTIO_RING_NO_LEGACY
22
20
 #include "virtio_pci_common.h"
23
21
 
24
22
 /*
..
..
@@ -29,16 +27,16 @@
29
27
  * method, i.e. 32-bit accesses for 32-bit fields, 16-bit accesses
30
28
  * for 16-bit fields and 8-bit accesses for 8-bit fields.
31
29
  */
32
-static inline u8 vp_ioread8(u8 __iomem *addr)
30
+static inline u8 vp_ioread8(const u8 __iomem *addr)
33
31
 {
34
32
 	return ioread8(addr);
35
33
 }
36
-static inline u16 vp_ioread16 (__le16 __iomem *addr)
34
+static inline u16 vp_ioread16 (const __le16 __iomem *addr)
37
35
 {
38
36
 	return ioread16(addr);
39
37
 }
40
38
 
41
-static inline u32 vp_ioread32(__le32 __iomem *addr)
39
+static inline u32 vp_ioread32(const __le32 __iomem *addr)
42
40
 {
43
41
 	return ioread32(addr);
44
42
 }
..
..
@@ -446,6 +444,99 @@
446
444
 	vring_del_virtqueue(vq);
447
445
 }
448
446
 
447
+static int virtio_pci_find_shm_cap(struct pci_dev *dev, u8 required_id,
448
+				   u8 *bar, u64 *offset, u64 *len)
449
+{
450
+	int pos;
451
+
452
+	for (pos = pci_find_capability(dev, PCI_CAP_ID_VNDR); pos > 0;
453
+	     pos = pci_find_next_capability(dev, pos, PCI_CAP_ID_VNDR)) {
454
+		u8 type, cap_len, id;
455
+		u32 tmp32;
456
+		u64 res_offset, res_length;
457
+
458
+		pci_read_config_byte(dev, pos + offsetof(struct virtio_pci_cap,
459
+							 cfg_type), &type);
460
+		if (type != VIRTIO_PCI_CAP_SHARED_MEMORY_CFG)
461
+			continue;
462
+
463
+		pci_read_config_byte(dev, pos + offsetof(struct virtio_pci_cap,
464
+							 cap_len), &cap_len);
465
+		if (cap_len != sizeof(struct virtio_pci_cap64)) {
466
+			dev_err(&dev->dev, "%s: shm cap with bad size offset:"
467
+				" %d size: %d\n", __func__, pos, cap_len);
468
+			continue;
469
+		}
470
+
471
+		pci_read_config_byte(dev, pos + offsetof(struct virtio_pci_cap,
472
+							 id), &id);
473
+		if (id != required_id)
474
+			continue;
475
+
476
+		/* Type, and ID match, looks good */
477
+		pci_read_config_byte(dev, pos + offsetof(struct virtio_pci_cap,
478
+							 bar), bar);
479
+
480
+		/* Read the lower 32bit of length and offset */
481
+		pci_read_config_dword(dev, pos + offsetof(struct virtio_pci_cap,
482
+							  offset), &tmp32);
483
+		res_offset = tmp32;
484
+		pci_read_config_dword(dev, pos + offsetof(struct virtio_pci_cap,
485
+							  length), &tmp32);
486
+		res_length = tmp32;
487
+
488
+		/* and now the top half */
489
+		pci_read_config_dword(dev,
490
+				      pos + offsetof(struct virtio_pci_cap64,
491
+						     offset_hi), &tmp32);
492
+		res_offset |= ((u64)tmp32) << 32;
493
+		pci_read_config_dword(dev,
494
+				      pos + offsetof(struct virtio_pci_cap64,
495
+						     length_hi), &tmp32);
496
+		res_length |= ((u64)tmp32) << 32;
497
+
498
+		*offset = res_offset;
499
+		*len = res_length;
500
+
501
+		return pos;
502
+	}
503
+	return 0;
504
+}
505
+
506
+static bool vp_get_shm_region(struct virtio_device *vdev,
507
+			      struct virtio_shm_region *region, u8 id)
508
+{
509
+	struct virtio_pci_device *vp_dev = to_vp_device(vdev);
510
+	struct pci_dev *pci_dev = vp_dev->pci_dev;
511
+	u8 bar;
512
+	u64 offset, len;
513
+	phys_addr_t phys_addr;
514
+	size_t bar_len;
515
+
516
+	if (!virtio_pci_find_shm_cap(pci_dev, id, &bar, &offset, &len))
517
+		return false;
518
+
519
+	phys_addr = pci_resource_start(pci_dev, bar);
520
+	bar_len = pci_resource_len(pci_dev, bar);
521
+
522
+	if ((offset + len) < offset) {
523
+		dev_err(&pci_dev->dev, "%s: cap offset+len overflow detected\n",
524
+			__func__);
525
+		return false;
526
+	}
527
+
528
+	if (offset + len > bar_len) {
529
+		dev_err(&pci_dev->dev, "%s: bar shorter than cap offset+len\n",
530
+			__func__);
531
+		return false;
532
+	}
533
+
534
+	region->len = len;
535
+	region->addr = (u64) phys_addr + offset;
536
+
537
+	return true;
538
+}
539
+
449
540
 static const struct virtio_config_ops virtio_pci_config_nodev_ops = {
450
541
 	.get		= NULL,
451
542
 	.set		= NULL,
..
..
@@ -460,6 +551,7 @@
460
551
 	.bus_name	= vp_bus_name,
461
552
 	.set_vq_affinity = vp_set_vq_affinity,
462
553
 	.get_vq_affinity = vp_get_vq_affinity,
554
+	.get_shm_region  = vp_get_shm_region,
463
555
 };
464
556
 
465
557
 static const struct virtio_config_ops virtio_pci_config_ops = {
..
..
@@ -476,6 +568,7 @@
476
568
 	.bus_name	= vp_bus_name,
477
569
 	.set_vq_affinity = vp_set_vq_affinity,
478
570
 	.get_vq_affinity = vp_get_vq_affinity,
571
+	.get_shm_region  = vp_get_shm_region,
479
572
 };
480
573
 
481
574
 /**
..
..
@@ -483,6 +576,7 @@
483
576
  * @dev: the pci device
484
577
  * @cfg_type: the VIRTIO_PCI_CAP_* value we seek
485
578
  * @ioresource_types: IORESOURCE_MEM and/or IORESOURCE_IO.
579
+ * @bars: the bitmask of BARs
486
580
  *
487
581
  * Returns offset of the capability, or 0.
488
582
  */