forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-09 b22da3d8526a935aa31e086e63f60ff3246cb61c 
 kernel/samples/vfio-mdev/mbochs.c
....@@ -71,11 +71,19 @@
7171 #define MBOCHS_NAME		  "mbochs"
7272 #define MBOCHS_CLASS_NAME	  "mbochs"
7373 
74
+#define MBOCHS_EDID_REGION_INDEX  VFIO_PCI_NUM_REGIONS
75
+#define MBOCHS_NUM_REGIONS        (MBOCHS_EDID_REGION_INDEX+1)
76
+
7477 #define MBOCHS_CONFIG_SPACE_SIZE  0xff
7578 #define MBOCHS_MMIO_BAR_OFFSET	  PAGE_SIZE
7679 #define MBOCHS_MMIO_BAR_SIZE	  PAGE_SIZE
77
-#define MBOCHS_MEMORY_BAR_OFFSET  (MBOCHS_MMIO_BAR_OFFSET + \
80
+#define MBOCHS_EDID_OFFSET	  (MBOCHS_MMIO_BAR_OFFSET +	\
7881 				   MBOCHS_MMIO_BAR_SIZE)
82
+#define MBOCHS_EDID_SIZE	  PAGE_SIZE
83
+#define MBOCHS_MEMORY_BAR_OFFSET  (MBOCHS_EDID_OFFSET + \
84
+				   MBOCHS_EDID_SIZE)
85
+
86
+#define MBOCHS_EDID_BLOB_OFFSET   (MBOCHS_EDID_SIZE/2)
7987 
8088 #define STORE_LE16(addr, val)	(*(u16 *)addr = val)
8189 #define STORE_LE32(addr, val)	(*(u32 *)addr = val)
....@@ -95,16 +103,24 @@
95103 static const struct mbochs_type {
96104 	const char *name;
97105 	u32 mbytes;
106
+	u32 max_x;
107
+	u32 max_y;
98108 } mbochs_types[] = {
99109 	{
100110 		.name	= MBOCHS_CLASS_NAME "-" MBOCHS_TYPE_1,
101111 		.mbytes = 4,
112
+		.max_x  = 800,
113
+		.max_y  = 600,
102114 	}, {
103115 		.name	= MBOCHS_CLASS_NAME "-" MBOCHS_TYPE_2,
104116 		.mbytes = 16,
117
+		.max_x  = 1920,
118
+		.max_y  = 1440,
105119 	}, {
106120 		.name	= MBOCHS_CLASS_NAME "-" MBOCHS_TYPE_3,
107121 		.mbytes = 64,
122
+		.max_x  = 0,
123
+		.max_y  = 0,
108124 	},
109125 };
110126 
....@@ -114,6 +130,11 @@
114130 static struct cdev	mbochs_cdev;
115131 static struct device	mbochs_dev;
116132 static int		mbochs_used_mbytes;
133
+
134
+struct vfio_region_info_ext {
135
+	struct vfio_region_info          base;
136
+	struct vfio_region_info_cap_type type;
137
+};
117138 
118139 struct mbochs_mode {
119140 	u32 drm_format;
....@@ -144,13 +165,14 @@
144165 	u32 memory_bar_mask;
145166 	struct mutex ops_lock;
146167 	struct mdev_device *mdev;
147
-	struct vfio_device_info dev_info;
148168 
149169 	const struct mbochs_type *type;
150170 	u16 vbe[VBE_DISPI_INDEX_COUNT];
151171 	u64 memsize;
152172 	struct page **pages;
153173 	pgoff_t pagecount;
174
+	struct vfio_region_gfx_edid edid_regs;
175
+	u8 edid_blob[0x400];
154176 
155177 	struct list_head dmabufs;
156178 	u32 active_id;
....@@ -342,10 +364,20 @@
342364 			     char *buf, u32 count)
343365 {
344366 	struct device *dev = mdev_dev(mdev_state->mdev);
367
+	struct vfio_region_gfx_edid *edid;
345368 	u16 reg16 = 0;
346369 	int index;
347370 
348371 	switch (offset) {
372
+	case 0x000 ... 0x3ff: /* edid block */
373
+		edid = &mdev_state->edid_regs;
374
+		if (edid->link_state != VFIO_DEVICE_GFX_LINK_STATE_UP ||
375
+		    offset >= edid->edid_size) {
376
+			memset(buf, 0, count);
377
+			break;
378
+		}
379
+		memcpy(buf, mdev_state->edid_blob + offset, count);
380
+		break;
349381 	case 0x500 ... 0x515: /* bochs dispi interface */
350382 		if (count != 2)
351383 			goto unhandled;
....@@ -363,6 +395,44 @@
363395 		memset(buf, 0, count);
364396 		break;
365397 	}
398
+}
399
+
400
+static void handle_edid_regs(struct mdev_state *mdev_state, u16 offset,
401
+			     char *buf, u32 count, bool is_write)
402
+{
403
+	char *regs = (void *)&mdev_state->edid_regs;
404
+
405
+	if (offset + count > sizeof(mdev_state->edid_regs))
406
+		return;
407
+	if (count != 4)
408
+		return;
409
+	if (offset % 4)
410
+		return;
411
+
412
+	if (is_write) {
413
+		switch (offset) {
414
+		case offsetof(struct vfio_region_gfx_edid, link_state):
415
+		case offsetof(struct vfio_region_gfx_edid, edid_size):
416
+			memcpy(regs + offset, buf, count);
417
+			break;
418
+		default:
419
+			/* read-only regs */
420
+			break;
421
+		}
422
+	} else {
423
+		memcpy(buf, regs + offset, count);
424
+	}
425
+}
426
+
427
+static void handle_edid_blob(struct mdev_state *mdev_state, u16 offset,
428
+			     char *buf, u32 count, bool is_write)
429
+{
430
+	if (offset + count > mdev_state->edid_regs.edid_max_size)
431
+		return;
432
+	if (is_write)
433
+		memcpy(mdev_state->edid_blob + offset, buf, count);
434
+	else
435
+		memcpy(buf, mdev_state->edid_blob + offset, count);
366436 }
367437 
368438 static ssize_t mdev_access(struct mdev_device *mdev, char *buf, size_t count,
....@@ -384,12 +454,24 @@
384454 			memcpy(buf, (mdev_state->vconfig + pos), count);
385455 
386456 	} else if (pos >= MBOCHS_MMIO_BAR_OFFSET &&
387
-		   pos + count <= MBOCHS_MEMORY_BAR_OFFSET) {
457
+		   pos + count <= (MBOCHS_MMIO_BAR_OFFSET +
458
+				   MBOCHS_MMIO_BAR_SIZE)) {
388459 		pos -= MBOCHS_MMIO_BAR_OFFSET;
389460 		if (is_write)
390461 			handle_mmio_write(mdev_state, pos, buf, count);
391462 		else
392463 			handle_mmio_read(mdev_state, pos, buf, count);
464
+
465
+	} else if (pos >= MBOCHS_EDID_OFFSET &&
466
+		   pos + count <= (MBOCHS_EDID_OFFSET +
467
+				   MBOCHS_EDID_SIZE)) {
468
+		pos -= MBOCHS_EDID_OFFSET;
469
+		if (pos < MBOCHS_EDID_BLOB_OFFSET) {
470
+			handle_edid_regs(mdev_state, pos, buf, count, is_write);
471
+		} else {
472
+			pos -= MBOCHS_EDID_BLOB_OFFSET;
473
+			handle_edid_blob(mdev_state, pos, buf, count, is_write);
474
+		}
393475 
394476 	} else if (pos >= MBOCHS_MEMORY_BAR_OFFSET &&
395477 		   pos + count <=
....@@ -471,6 +553,10 @@
471553 	mdev_state->next_id = 1;
472554 
473555 	mdev_state->type = type;
556
+	mdev_state->edid_regs.max_xres = type->max_x;
557
+	mdev_state->edid_regs.max_yres = type->max_y;
558
+	mdev_state->edid_regs.edid_offset = MBOCHS_EDID_BLOB_OFFSET;
559
+	mdev_state->edid_regs.edid_max_size = sizeof(mdev_state->edid_blob);
474560 	mbochs_create_config_space(mdev_state);
475561 	mbochs_reset(mdev);
476562 
....@@ -760,7 +846,7 @@
760846 	if (sg_alloc_table_from_pages(sg, dmabuf->pages, dmabuf->pagecount,
761847 				      0, dmabuf->mode.size, GFP_KERNEL) < 0)
762848 		goto err2;
763
-	if (!dma_map_sg(at->dev, sg->sgl, sg->nents, direction))
849
+	if (dma_map_sgtable(at->dev, sg, direction, 0))
764850 		goto err3;
765851 
766852 	return sg;
....@@ -782,6 +868,7 @@
782868 
783869 	dev_dbg(dev, "%s: %d\n", __func__, dmabuf->id);
784870 
871
+	dma_unmap_sgtable(at->dev, sg, direction, 0);
785872 	sg_free_table(sg);
786873 	kfree(sg);
787874 }
....@@ -805,26 +892,10 @@
805892 	mutex_unlock(&mdev_state->ops_lock);
806893 }
807894 
808
-static void *mbochs_kmap_dmabuf(struct dma_buf *buf, unsigned long page_num)
809
-{
810
-	struct mbochs_dmabuf *dmabuf = buf->priv;
811
-	struct page *page = dmabuf->pages[page_num];
812
-
813
-	return kmap(page);
814
-}
815
-
816
-static void mbochs_kunmap_dmabuf(struct dma_buf *buf, unsigned long page_num,
817
-				 void *vaddr)
818
-{
819
-	kunmap(vaddr);
820
-}
821
-
822895 static struct dma_buf_ops mbochs_dmabuf_ops = {
823896 	.map_dma_buf	  = mbochs_map_dmabuf,
824897 	.unmap_dma_buf	  = mbochs_unmap_dmabuf,
825898 	.release	  = mbochs_release_dmabuf,
826
-	.map		  = mbochs_kmap_dmabuf,
827
-	.unmap		  = mbochs_kunmap_dmabuf,
828899 	.mmap		  = mbochs_mmap_dmabuf,
829900 };
830901 
....@@ -932,16 +1003,16 @@
9321003 }
9331004 
9341005 static int mbochs_get_region_info(struct mdev_device *mdev,
935
-				  struct vfio_region_info *region_info,
936
-				  u16 *cap_type_id, void **cap_type)
1006
+				  struct vfio_region_info_ext *ext)
9371007 {
1008
+	struct vfio_region_info *region_info = &ext->base;
9381009 	struct mdev_state *mdev_state;
9391010 
9401011 	mdev_state = mdev_get_drvdata(mdev);
9411012 	if (!mdev_state)
9421013 		return -EINVAL;
9431014 
944
-	if (region_info->index >= VFIO_PCI_NUM_REGIONS)
1015
+	if (region_info->index >= MBOCHS_NUM_REGIONS)
9451016 		return -EINVAL;
9461017 
9471018 	switch (region_info->index) {
....@@ -964,6 +1035,20 @@
9641035 		region_info->flags  = (VFIO_REGION_INFO_FLAG_READ  |
9651036 				       VFIO_REGION_INFO_FLAG_WRITE);
9661037 		break;
1038
+	case MBOCHS_EDID_REGION_INDEX:
1039
+		ext->base.argsz = sizeof(*ext);
1040
+		ext->base.offset = MBOCHS_EDID_OFFSET;
1041
+		ext->base.size = MBOCHS_EDID_SIZE;
1042
+		ext->base.flags = (VFIO_REGION_INFO_FLAG_READ  |
1043
+				   VFIO_REGION_INFO_FLAG_WRITE |
1044
+				   VFIO_REGION_INFO_FLAG_CAPS);
1045
+		ext->base.cap_offset = offsetof(typeof(*ext), type);
1046
+		ext->type.header.id = VFIO_REGION_INFO_CAP_TYPE;
1047
+		ext->type.header.version = 1;
1048
+		ext->type.header.next = 0;
1049
+		ext->type.type = VFIO_REGION_TYPE_GFX;
1050
+		ext->type.subtype = VFIO_REGION_SUBTYPE_GFX_EDID;
1051
+		break;
9671052 	default:
9681053 		region_info->size   = 0;
9691054 		region_info->offset = 0;
....@@ -984,7 +1069,7 @@
9841069 				  struct vfio_device_info *dev_info)
9851070 {
9861071 	dev_info->flags = VFIO_DEVICE_FLAGS_PCI;
987
-	dev_info->num_regions = VFIO_PCI_NUM_REGIONS;
1072
+	dev_info->num_regions = MBOCHS_NUM_REGIONS;
9881073 	dev_info->num_irqs = VFIO_PCI_NUM_IRQS;
9891074 	return 0;
9901075 }
....@@ -1084,10 +1169,7 @@
10841169 			unsigned long arg)
10851170 {
10861171 	int ret = 0;
1087
-	unsigned long minsz;
1088
-	struct mdev_state *mdev_state;
1089
-
1090
-	mdev_state = mdev_get_drvdata(mdev);
1172
+	unsigned long minsz, outsz;
10911173 
10921174 	switch (cmd) {
10931175 	case VFIO_DEVICE_GET_INFO:
....@@ -1106,8 +1188,6 @@
11061188 		if (ret)
11071189 			return ret;
11081190 
1109
-		memcpy(&mdev_state->dev_info, &info, sizeof(info));
1110
-
11111191 		if (copy_to_user((void __user *)arg, &info, minsz))
11121192 			return -EFAULT;
11131193 
....@@ -1115,24 +1195,24 @@
11151195 	}
11161196 	case VFIO_DEVICE_GET_REGION_INFO:
11171197 	{
1118
-		struct vfio_region_info info;
1119
-		u16 cap_type_id = 0;
1120
-		void *cap_type = NULL;
1198
+		struct vfio_region_info_ext info;
11211199 
1122
-		minsz = offsetofend(struct vfio_region_info, offset);
1200
+		minsz = offsetofend(typeof(info), base.offset);
11231201 
11241202 		if (copy_from_user(&info, (void __user *)arg, minsz))
11251203 			return -EFAULT;
11261204 
1127
-		if (info.argsz < minsz)
1205
+		outsz = info.base.argsz;
1206
+		if (outsz < minsz)
1207
+			return -EINVAL;
1208
+		if (outsz > sizeof(info))
11281209 			return -EINVAL;
11291210 
1130
-		ret = mbochs_get_region_info(mdev, &info, &cap_type_id,
1131
-					   &cap_type);
1211
+		ret = mbochs_get_region_info(mdev, &info);
11321212 		if (ret)
11331213 			return ret;
11341214 
1135
-		if (copy_to_user((void __user *)arg, &info, minsz))
1215
+		if (copy_to_user((void __user *)arg, &info, outsz))
11361216 			return -EFAULT;
11371217 
11381218 		return 0;
....@@ -1148,7 +1228,7 @@
11481228 			return -EFAULT;
11491229 
11501230 		if ((info.argsz < minsz) ||
1151
-		    (info.index >= mdev_state->dev_info.num_irqs))
1231
+		    (info.index >= VFIO_PCI_NUM_IRQS))
11521232 			return -EINVAL;
11531233 
11541234 		ret = mbochs_get_irq_info(mdev, &info);
....@@ -1350,13 +1430,13 @@
13501430 {
13511431 	int ret = 0;
13521432 
1353
-	ret = alloc_chrdev_region(&mbochs_devt, 0, MINORMASK, MBOCHS_NAME);
1433
+	ret = alloc_chrdev_region(&mbochs_devt, 0, MINORMASK + 1, MBOCHS_NAME);
13541434 	if (ret < 0) {
13551435 		pr_err("Error: failed to register mbochs_dev, err: %d\n", ret);
13561436 		return ret;
13571437 	}
13581438 	cdev_init(&mbochs_cdev, &vd_fops);
1359
-	cdev_add(&mbochs_cdev, mbochs_devt, MINORMASK);
1439
+	cdev_add(&mbochs_cdev, mbochs_devt, MINORMASK + 1);
13601440 	pr_info("%s: major %d\n", __func__, MAJOR(mbochs_devt));
13611441 
13621442 	mbochs_class = class_create(THIS_MODULE, MBOCHS_CLASS_NAME);
....@@ -1385,7 +1465,7 @@
13851465 	class_destroy(mbochs_class);
13861466 failed1:
13871467 	cdev_del(&mbochs_cdev);
1388
-	unregister_chrdev_region(mbochs_devt, MINORMASK);
1468
+	unregister_chrdev_region(mbochs_devt, MINORMASK + 1);
13891469 	return ret;
13901470 }
13911471 
....@@ -1396,7 +1476,7 @@
13961476 
13971477 	device_unregister(&mbochs_dev);
13981478 	cdev_del(&mbochs_cdev);
1399
-	unregister_chrdev_region(mbochs_devt, MINORMASK);
1479
+	unregister_chrdev_region(mbochs_devt, MINORMASK + 1);
14001480 	class_destroy(mbochs_class);
14011481 	mbochs_class = NULL;
14021482 }