Add support for changing mode via fbset on non-shared fullscreen framebuffers

Signed-off-by: Philipp Zabel <p.zabel@pengutronix.de>

3 files changed, 164 insertions, 4 deletions

diff --git a/src/bgi.c b/src/bgi.c
index 2166c1b..a190f5c 100644
--- a/src/bgi.c
+++ b/src/bgi.c
@@ -124,6 +124,83 @@ static int wait_vblank(struct kms_fb *fb)
 	return -EINVAL;
 }
 
+#define PICOSECONDS_PER_MILLISECOND	1000000000ULL
+
+static void fb_var_screeninfo_to_drm_mode(struct fb_var_screeninfo *fb_var,
+					  struct drm_mode_modeinfo *mode)
+{
+	memset(mode, 0, sizeof(*mode));
+
+	/* Convert pixclock in ps into clock in kHz */
+	mode->clock = PICOSECONDS_PER_MILLISECOND / fb_var->pixclock;
+	mode->hdisplay = fb_var->xres;
+	mode->vdisplay = fb_var->yres;
+	mode->hsync_start = mode->hdisplay + fb_var->right_margin;
+	mode->vsync_start = mode->vdisplay + fb_var->lower_margin;
+	mode->hsync_end = mode->hsync_start + fb_var->hsync_len;
+	mode->vsync_end = mode->vsync_start + fb_var->vsync_len;
+	mode->htotal = mode->hsync_end + fb_var->left_margin;
+	mode->vtotal = mode->vsync_end + fb_var->upper_margin;
+	mode->vrefresh = mode->clock * 1000 / (mode->htotal * mode->vtotal);
+
+	if (fb_var->sync & FB_SYNC_HOR_HIGH_ACT)
+		mode->flags |= DRM_MODE_FLAG_PHSYNC;
+	else
+		mode->flags |= DRM_MODE_FLAG_NHSYNC;
+	if (fb_var->sync & FB_SYNC_VERT_HIGH_ACT)
+		mode->flags |= DRM_MODE_FLAG_PVSYNC;
+	else
+		mode->flags |= DRM_MODE_FLAG_NVSYNC;
+}
+
+static void drm_mode_to_fb_var_screeninfo(struct drm_mode_modeinfo *mode,
+					  struct fb_var_screeninfo *fb_var)
+{
+	/* Convert clock in kHz into pixclock in ps */
+	fb_var->pixclock = PICOSECONDS_PER_MILLISECOND / mode->clock;
+	fb_var->xres = mode->hdisplay;
+	fb_var->yres = mode->vdisplay;
+	fb_var->left_margin = mode->htotal - mode->hsync_end;
+	fb_var->right_margin = mode->hsync_start - mode->hdisplay;
+	fb_var->upper_margin = mode->vtotal - mode->vsync_end;
+	fb_var->lower_margin = mode->vsync_start - mode->vdisplay;
+	fb_var->hsync_len = mode->hsync_end - mode->hsync_start;
+	fb_var->vsync_len = mode->vsync_end - mode->vsync_start;
+
+	if (mode->flags & DRM_MODE_FLAG_PHSYNC)
+		fb_var->sync |= FB_SYNC_HOR_HIGH_ACT;
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		fb_var->sync &= ~FB_SYNC_HOR_HIGH_ACT;
+	if (mode->flags & DRM_MODE_FLAG_PVSYNC)
+		fb_var->sync |= FB_SYNC_VERT_HIGH_ACT;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		fb_var->sync &= ~FB_SYNC_VERT_HIGH_ACT;
+}
+
+static int do_set_mode(struct kms_fb *fb, const struct fb_var_screeninfo *v)
+{
+	struct drm_mode_modeinfo mode;
+	int ret;
+
+	if (fb->xres != v->xres || fb->yres != v->yres) {
+		pr_err("can't change framebuffer size\n");
+		return -EINVAL;
+	}
+
+	fb_var_screeninfo_to_drm_mode(v, &mode);
+
+	ret = kms_fb_set_mode(fb, &mode);
+	if (ret < 0) {
+		pr_err("failed to set mode %dx%d@%d\n",
+		       mode.hdisplay, mode.vdisplay, mode.vrefresh);
+		return ret;
+	}
+
+	drm_mode_to_fb_var_screeninfo(&mode, &fb->fb_var);
+
+	return 0;
+}
+
 static void do_pageflip(struct kms_fb *fb, unsigned int offset)
 {
 	struct drm_mode_crtc_page_flip flip = {};
@@ -196,6 +273,7 @@ static void fbdev_ioctl(fuse_req_t req, int cmd, void *arg,
 			  const void *in_buf, size_t in_bufsz, size_t out_bufsz)
 {
 	struct kms_fb *fb = fuse_req_userdata(req);
+	int ret;
 
 	switch (cmd) {
 	case FBIOGET_VSCREENINFO:
@@ -209,13 +287,21 @@ static void fbdev_ioctl(fuse_req_t req, int cmd, void *arg,
 		}
 		break;
 	case FBIOPUT_VSCREENINFO:
-		pr_err("%s: FBIOPUT_VSCREENINFO: not implemented\n", __func__);
-		if (!out_bufsz) {
+		if (!in_bufsz || !out_bufsz) {
 			struct iovec iov = { arg, sizeof(fb->fb_var) };
 
-			fuse_reply_ioctl_retry(req, NULL, 0, &iov, 1);
+			fuse_reply_ioctl_retry(req, &iov, 1, &iov, 1);
 		} else {
-			fuse_reply_ioctl(req, 0, &fb->fb_var,
+			const struct fb_var_screeninfo *v = in_buf;
+
+			if (!fb->con) {
+				fuse_reply_ioctl(req, -EINVAL, &fb->fb_var,
+						 sizeof(fb->fb_var));
+				break;
+			}
+
+			ret = do_set_mode(fb, v);
+			fuse_reply_ioctl(req, ret, &fb->fb_var,
 					 sizeof(fb->fb_var));
 		}
 		break;
@@ -338,3 +424,30 @@ int bgi_init(struct kms_fb *fb)
 	ret = pthread_create(&threadid, NULL, bgi_thread, fb);
 	return ret;
 }
+
+int bgi_set_connector_crtc_mode(struct kms_fb *fb, struct drm_connector *con,
+				uint32_t crtc, struct drm_mode_modeinfo *mode)
+{
+	if (mode) {
+		if (fb->xres != mode->hdisplay || fb->yres != mode->vdisplay) {
+			printf("can't set connector on non-fullscreen FB\n");
+			return -1;
+		}
+
+		drm_mode_to_fb_var_screeninfo(mode, &fb->fb_var);
+	} else {
+		fb->fb_var.pixclock = 0;
+		fb->fb_var.left_margin = 0;
+		fb->fb_var.right_margin = 0;
+		fb->fb_var.upper_margin = 0;
+		fb->fb_var.lower_margin = 0;
+		fb->fb_var.hsync_len = 0;
+		fb->fb_var.vsync_len = 0;
+		fb->fb_var.sync = 0;
+	}
+
+	fb->crtc = crtc;
+	fb->con = con;
+
+	return 0;
+}
diff --git a/src/kmsfb-manage.c b/src/kmsfb-manage.c
index 03f55eb..b2b39de 100644
--- a/src/kmsfb-manage.c
+++ b/src/kmsfb-manage.c
@@ -748,6 +748,27 @@ static int drm_mode_set_plane(int fd, uint32_t plane_id, uint32_t crtc_id,
 	return 0;
 }
 
+int kms_fb_set_mode(struct kms_fb *fb, struct drm_mode_modeinfo *mode)
+{
+	struct drm_mode_modeinfo *mode_adjust;
+
+	mode_adjust = drm_find_mode(fb->con, fb->xres, fb->yres, mode->vrefresh);
+	if (!mode_adjust) {
+		printf("could not find mode %dx%d@%d\n",
+		       fb->xres, fb->yres, mode->vrefresh);
+	} else {
+		mode_adjust = drm_find_mode(fb->con, fb->xres, fb->yres, 0);
+	}
+	if (!mode_adjust) {
+		printf("could not find any mode %dx%d, forcing %dx%d@%d\n",
+		       fb->xres, fb->yres, mode->hdisplay, mode->vdisplay,
+		       mode->vrefresh);
+	}
+
+	return drm_mode_set_crtc(drmfd, fb->crtc, fb->slices[0].fb_id,
+				 0, 0, &fb->con->id, 1, mode);
+}
+
 static struct option long_options[] = {
 	{
 		.name = "crtc",
@@ -835,6 +856,7 @@ static int create_window(struct drm_resource *res, const char *in)
 	struct drm_connector *con;
 	struct drm_mode_modeinfo *mode;
 	struct kms_data kd = {};
+	int singular_slices;
 	int ret, i, p;
 
 	ret = parse_key_value(in, &kd);
@@ -903,11 +925,26 @@ static int create_window(struct drm_resource *res, const char *in)
 	if (ret)
 		perror("drm_mode_set_crtc");
 
+	singular_slices = 0;
 	for (i = 0; i < kd.kms_fb->num_slices; i++) {
 		kd.kms_fb->slices[i].windows[kd.kms_fb->slices[i].num_windows].crtc_id = kd.crtc;
 		kd.kms_fb->slices[i].windows[kd.kms_fb->slices[i].num_windows].plane_id = 0;
 		kd.kms_fb->slices[i].num_windows++;
+		if (kd.kms_fb->slices[i].num_windows == 1)
+			singular_slices++;
+	}
+
+	/*
+	 * allow to set display dimings on singular full-screen windows,
+	 * allow to read display timings on all full-screen windows.
+	 */
+	if (kd.xres != kd.kms_fb->xres || kd.yres != kd.kms_fb->yres) {
+		con = NULL;
+		mode = NULL;
+	} else if (singular_slices != kd.kms_fb->num_slices) {
+		con = NULL;
 	}
+	bgi_set_connector_crtc_mode(kd.kms_fb, con, kd.crtc, mode);
 
 	return ret;
 }
diff --git a/src/kmsfb.h b/src/kmsfb.h
index a7b45ea..9a36b57 100644
--- a/src/kmsfb.h
+++ b/src/kmsfb.h
@@ -12,6 +12,9 @@
 				fprintf(stderr, fmt, ##arg);	\
 		} while (0);
 
+struct drm_connector;
+struct drm_mode_modeinfo;
+
 extern int drmfd;
 extern int debug;
 
@@ -70,6 +73,9 @@ struct kms_fb {
 	struct fb_fix_screeninfo fb_fix;
 	struct fb_var_screeninfo fb_var;
 	unsigned long pos;
+
+	struct drm_connector *con;
+	uint32_t crtc;
 };
 
 struct kms_data {
@@ -103,5 +109,9 @@ struct splash {
 int png_load(char *name, struct splash *splash, unsigned char **alpha);
 
 int bgi_init(struct kms_fb *fb);
+int bgi_set_connector_crtc_mode(struct kms_fb *fb, struct drm_connector *con,
+				uint32_t crtc, struct drm_mode_modeinfo *mode);
+
+int kms_fb_set_mode(struct kms_fb *fb, struct drm_mode_modeinfo *mode);
 
 #endif /* __KMSFB_H */