fbtft: software screen rotation

Signed-off-by: Vincent-FK <vincent.buso@funkey-project.com>

Author: Vincent-FK

arch/arm/boot/dts/sun8i-v3s-funkey.dts

@@ -179,6 +179,7 @@
                txbuflen = <115202>;
                spi_async_mode = "true";
                rotate = <0>;
+               rotate_soft = <0>;
                fps = <100>;
                buswidth = <8>;
                reset-gpios = <&pio 1 2 GPIO_ACTIVE_HIGH>;	//PB2

drivers/staging/fbtft/fbtft-bus.c

@@ -136,6 +136,9 @@ int fbtft_write_init_cmd_data_transfers(struct fbtft_par *par)
 
 	// printk("%s\n", __func__);
 
+	/* Post process */
+	fbtft_post_process_screen(par, 0, par->info->var.yres - 1);
+
 	/* Resetting to 0 for incoming cmd init data write */
 	if (gpio_is_valid(par->gpio.dc))
 		gpio_set_value(par->gpio.dc, 0);

drivers/staging/fbtft/fbtft-core.c

@@ -463,6 +463,40 @@ static void fbtft_update_display(struct fbtft_par *par, unsigned int start_line,
 	}
 }
 
+/* Soft Matrix Rotation - Works only on 2D square matrices */
+void fbtft_rotate_soft(u16 *mat, int size, int rotation)
+{
+	int i, j;
+	u16 temp;
+	int N = size;
+
+#define AT(i, j) 	((i) * N + (j))
+
+	if (rotation == 90) {
+		/* Rotate screen 90° Clockwise */
+		for (i = 0; i < N / 2; i++) {
+			for (j = i; j < N - i - 1; j++) {
+				temp = mat[AT(i, j)];
+				mat[AT(i, j)] = mat[AT(N - 1 - j, i)];
+				mat[AT(N - 1 - j, i)] = mat[AT(N - 1 - i, N - 1 - j)];
+				mat[AT(N - 1 - i, N - 1 - j)] = mat[AT(j, N - 1 - i)];
+				mat[AT(j, N - 1 - i)] = temp;
+			}
+		}
+	} else if (rotation == 270) {
+		/* Rotate screen 270° Clockwise */
+		for (i = 0; i < N / 2; i++) {
+			for (j = i; j < N - i - 1; j++) {
+				temp = mat[AT(i, j)];
+				mat[AT(i, j)] = mat[AT(j, N - 1 - i)];
+				mat[AT(j, N-1-i)] = mat[AT(N - 1 - i, N - 1 - j)];
+				mat[AT(N - 1 - i, N - 1 - j)] = mat[AT(N - 1 - j, i)];
+				mat[AT(N - 1 - j, i)] = temp;
+			}
+		}
+	}
+}
+
 static void fbtft_mkdirty(struct fb_info *info, int y, int height)
 {
 	struct fbtft_par *par = info->par;
@@ -491,6 +525,36 @@ static void fbtft_mkdirty(struct fb_info *info, int y, int height)
 	schedule_delayed_work(&info->deferred_work, fbdefio->delay);
 }
 
+void fbtft_post_process_screen(struct fbtft_par *par, unsigned int dirty_lines_start, unsigned int dirty_lines_end)
+{
+	int x_notif = 0;
+	int y_notif = 0;
+	bool screen_post_process = false;
+
+	/* Reset default write buffer */
+	par->vmem_ptr = par->info->screen_buffer;
+
+	/* If soft rotation, mark whole screen to update to avoid data non rotated */
+	if (par->pdata->rotate_soft) {
+		dirty_lines_start = 0;
+		dirty_lines_end = par->info->var.yres - 1;
+		par->vmem_ptr = par->vmem_post_process;
+		screen_post_process = true;
+	}
+
+	/* Post process */
+	if (screen_post_process) {
+		/* Copy buffer */
+		memcpy(par->vmem_post_process + dirty_lines_start * par->info->fix.line_length,
+			par->info->screen_buffer + dirty_lines_start * par->info->fix.line_length,
+			(dirty_lines_end-dirty_lines_start+1) * par->info->fix.line_length);
+
+		/* Soft rotation */
+		if (par->pdata->rotate_soft)
+			fbtft_rotate_soft((u16*)par->vmem_post_process, par->info->var.yres, par->pdata->rotate_soft);
+	}
+}
+
 static void fbtft_deferred_io(struct fb_info *info, struct list_head *pagelist)
 {
 	struct fbtft_par *par = info->par;
@@ -525,6 +589,8 @@ static void fbtft_deferred_io(struct fb_info *info, struct list_head *pagelist)
 			dirty_lines_end = y_high;
 	}
 
+	fbtft_post_process_screen(par, dirty_lines_start, dirty_lines_end);
+
 	/* Screen upgrade */
 	par->fbtftops.update_display(par, dirty_lines_start, dirty_lines_end);
 }
@@ -1475,6 +1541,7 @@ static struct fbtft_platform_data *fbtft_probe_dt(struct device *dev)
 	pdata->display.bpp = fbtft_of_value(node, "bpp");
 	pdata->display.debug = fbtft_of_value(node, "debug");
 	pdata->rotate = fbtft_of_value(node, "rotate");
+	pdata->rotate_soft = fbtft_of_value(node, "rotate_soft");
 	pdata->bgr = of_property_read_bool(node, "bgr");
 	pdata->spi_async_mode = of_property_read_bool(node, "spi_async_mode");
 	pdata->fps = fbtft_of_value(node, "fps");

drivers/staging/fbtft/fbtft-sysfs.c

@@ -145,6 +145,64 @@ static struct device_attribute gamma_device_attrs[] = {
 	__ATTR(gamma, 0660, show_gamma_curve, store_gamma_curve),
 };
 
+void fbtft_expand_rotate_soft_value(unsigned long *rotate_soft)
+{
+	switch (*rotate_soft) {
+	case 0:
+	case 90:
+	case 180:
+	case 270:
+		break;
+	case 1:
+		*rotate_soft = 90;
+		break;
+	case 2:
+		*rotate_soft = 180;
+		break;
+	case 3:
+		*rotate_soft = 270;
+	default:
+		printk("Wrong Rotate soft value: %lu\n", *rotate_soft);
+		 *rotate_soft = 0;
+		break;
+	}
+}
+
+static ssize_t store_rotate_soft(struct device *device,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &par->pdata->rotate_soft);
+	if (ret)
+		return ret;
+	fbtft_expand_rotate_soft_value(&par->pdata->rotate_soft);
+
+	/* Schedule deferred_io to update display (no-op if already on queue)*/
+	if (!par->spi_async_mode){
+		par->dirty_lines_start = 0;
+		par->dirty_lines_end = par->info->var.yres - 1;
+		schedule_delayed_work(&par->info->deferred_work, par->info->fbdefio->delay);
+	}
+
+	return count;
+}
+
+static ssize_t show_rotate_soft(struct device *device,
+			  struct device_attribute *attr, char *buf)
+{
+	struct fb_info *fb_info = dev_get_drvdata(device);
+	struct fbtft_par *par = fb_info->par;
+
+	return snprintf(buf, PAGE_SIZE, "%lu\n", par->pdata->rotate_soft);
+}
+
+static struct device_attribute rotate_soft_device_attr =
+	__ATTR(rotate_soft, 0660, show_rotate_soft, store_rotate_soft);
+
 void fbtft_expand_debug_value(unsigned long *debug)
 {
 	switch (*debug & 0x7) {
@@ -206,13 +264,15 @@ static struct device_attribute debug_device_attr =
 void fbtft_sysfs_init(struct fbtft_par *par)
 {
 	device_create_file(par->info->dev, &debug_device_attr);
+	device_create_file(par->info->dev, &rotate_soft_device_attr);
 	if (par->gamma.curves && par->fbtftops.set_gamma)
 		device_create_file(par->info->dev, &gamma_device_attrs[0]);
 }
 
 void fbtft_sysfs_exit(struct fbtft_par *par)
 {
 	device_remove_file(par->info->dev, &debug_device_attr);
+	device_remove_file(par->info->dev, &rotate_soft_device_attr);
 	if (par->gamma.curves && par->fbtftops.set_gamma)
 		device_remove_file(par->info->dev, &gamma_device_attrs[0]);
 }

drivers/staging/fbtft/fbtft.h

@@ -151,6 +151,7 @@ struct fbtft_platform_data {
 	struct fbtft_display display;
 	const struct fbtft_gpio *gpios;
 	unsigned int rotate;
+	unsigned long rotate_soft;
 	bool bgr;
 	bool spi_async_mode;
 	unsigned int fps;
@@ -273,6 +274,9 @@ int fbtft_init_display(struct fbtft_par *par);
 int fbtft_probe_common(struct fbtft_display *display, struct spi_device *sdev,
 		       struct platform_device *pdev);
 int fbtft_remove_common(struct device *dev, struct fb_info *info);
+void fbtft_rotate_soft(u16 *mat, int size, int rotation);
+void fbtft_post_process_screen(struct fbtft_par *par,
+			unsigned int dirty_lines_start, unsigned int dirty_lines_end);
 
 /* fbtft-io.c */
 int fbtft_write_spi(struct fbtft_par *par, void *buf, size_t len);