1 files changed, 75 insertions, 35 deletions

diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv04.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv04.c
index f1f3bd15a5c0..6dba53df35fc 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv04.c
@@ -24,60 +24,100 @@
 
 #include "priv.h"
 
+#include <core/client.h>
 #include <core/event.h>
 #include <core/class.h>
+#include <nvif/unpack.h>
+#include <nvif/class.h>
 
 struct nv04_disp_priv {
 	struct nouveau_disp base;
 };
 
 static int
-nv04_disp_scanoutpos(struct nouveau_object *object, u32 mthd,
-		     void *data, u32 size)
+nv04_disp_scanoutpos(struct nouveau_object *object, struct nv04_disp_priv *priv,
+		     void *data, u32 size, int head)
 {
-	struct nv04_disp_priv *priv = (void *)object->engine;
-	struct nv04_display_scanoutpos *args = data;
-	const int head = (mthd & NV04_DISP_MTHD_HEAD);
+	const u32 hoff = head * 0x2000;
+	union {
+		struct nv04_disp_scanoutpos_v0 v0;
+	} *args = data;
 	u32 line;
+	int ret;
+
+	nv_ioctl(object, "disp scanoutpos size %d\n", size);
+	if (nvif_unpack(args->v0, 0, 0, false)) {
+		nv_ioctl(object, "disp scanoutpos vers %d\n", args->v0.version);
+		args->v0.vblanks = nv_rd32(priv, 0x680800 + hoff) & 0xffff;
+		args->v0.vtotal  = nv_rd32(priv, 0x680804 + hoff) & 0xffff;
+		args->v0.vblanke = args->v0.vtotal - 1;
+
+		args->v0.hblanks = nv_rd32(priv, 0x680820 + hoff) & 0xffff;
+		args->v0.htotal  = nv_rd32(priv, 0x680824 + hoff) & 0xffff;
+		args->v0.hblanke = args->v0.htotal - 1;
+
+		/*
+		 * If output is vga instead of digital then vtotal/htotal is
+		 * invalid so we have to give up and trigger the timestamping
+		 * fallback in the drm core.
+		 */
+		if (!args->v0.vtotal || !args->v0.htotal)
+			return -ENOTSUPP;
+
+		args->v0.time[0] = ktime_to_ns(ktime_get());
+		line = nv_rd32(priv, 0x600868 + hoff);
+		args->v0.time[1] = ktime_to_ns(ktime_get());
+		args->v0.hline = (line & 0xffff0000) >> 16;
+		args->v0.vline = (line & 0x0000ffff);
+	} else
+		return ret;
 
-	if (size < sizeof(*args))
-		return -EINVAL;
-
-	args->vblanks = nv_rd32(priv, 0x680800 + (head * 0x2000)) & 0xffff;
-	args->vtotal  = nv_rd32(priv, 0x680804 + (head * 0x2000)) & 0xffff;
-	args->vblanke = args->vtotal - 1;
-
-	args->hblanks = nv_rd32(priv, 0x680820 + (head * 0x2000)) & 0xffff;
-	args->htotal  = nv_rd32(priv, 0x680824 + (head * 0x2000)) & 0xffff;
-	args->hblanke = args->htotal - 1;
-
-	/*
-	 * If output is vga instead of digital then vtotal/htotal is invalid
-	 * so we have to give up and trigger the timestamping fallback in the
-	 * drm core.
-	 */
-	if (!args->vtotal || !args->htotal)
-		return -ENOTSUPP;
-
-	args->time[0] = ktime_to_ns(ktime_get());
-	line = nv_rd32(priv, 0x600868 + (head * 0x2000));
-	args->time[1] = ktime_to_ns(ktime_get());
-	args->hline = (line & 0xffff0000) >> 16;
-	args->vline = (line & 0x0000ffff);
 	return 0;
 }
 
-#define HEAD_MTHD(n) (n), (n) + 0x01
+static int
+nv04_disp_mthd(struct nouveau_object *object, u32 mthd, void *data, u32 size)
+{
+	union {
+		struct nv04_disp_mthd_v0 v0;
+	} *args = data;
+	struct nv04_disp_priv *priv = (void *)object->engine;
+	int head, ret;
+
+	nv_ioctl(object, "disp mthd size %d\n", size);
+	if (nvif_unpack(args->v0, 0, 0, true)) {
+		nv_ioctl(object, "disp mthd vers %d mthd %02x head %d\n",
+			 args->v0.version, args->v0.method, args->v0.head);
+		mthd = args->v0.method;
+		head = args->v0.head;
+	} else
+		return ret;
+
+	if (head < 0 || head >= 2)
+		return -ENXIO;
+
+	switch (mthd) {
+	case NV04_DISP_SCANOUTPOS:
+		return nv04_disp_scanoutpos(object, priv, data, size, head);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
 
-static struct nouveau_omthds
-nv04_disp_omthds[] = {
-	{ HEAD_MTHD(NV04_DISP_SCANOUTPOS), nv04_disp_scanoutpos },
-	{}
+static struct nouveau_ofuncs
+nv04_disp_ofuncs = {
+	.ctor = _nouveau_object_ctor,
+	.dtor = nouveau_object_destroy,
+	.init = nouveau_object_init,
+	.fini = nouveau_object_fini,
+	.mthd = nv04_disp_mthd,
 };
 
 static struct nouveau_oclass
 nv04_disp_sclass[] = {
-	{ NV04_DISP_CLASS, &nouveau_object_ofuncs, nv04_disp_omthds },
+	{ NV04_DISP_CLASS, &nv04_disp_ofuncs },
 	{},
 };