b2r2: Add optimized path for rot+rescale

This patch adds an optimized implementation of combined rotate+rescale
operations.

ST-Ericsson ID: 322771

Change-Id: Ifca03ce411297a5641e6382de57ad6c854e4e7b0
Signed-off-by: Fredrik Allansson <fredrik.allansson@stericsson.com>
Signed-off-by: Robert Fekete <robert.fekete@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/16862

3 files changed, 433 insertions, 107 deletions

diff --git a/drivers/video/b2r2/b2r2_blt_main.c b/drivers/video/b2r2/b2r2_blt_main.c
index f2bca6a1d5f..1f5be3d0d95 100644
--- a/drivers/video/b2r2/b2r2_blt_main.c
+++ b/drivers/video/b2r2/b2r2_blt_main.c
@@ -46,7 +46,7 @@
 #include "b2r2_input_validation.h"
 
 #define B2R2_HEAP_SIZE (4 * PAGE_SIZE)
-#define MAX_TMP_BUF_SIZE (31 * PAGE_SIZE)
+#define MAX_TMP_BUF_SIZE (128 * PAGE_SIZE)
 
 /*
  * TODO:
diff --git a/drivers/video/b2r2/b2r2_debug.c b/drivers/video/b2r2/b2r2_debug.c
index 170cc899900..d4711cd3e28 100644
--- a/drivers/video/b2r2/b2r2_debug.c
+++ b/drivers/video/b2r2/b2r2_debug.c
@@ -245,18 +245,18 @@ static ssize_t last_job_read(struct file *filep, char __user *buf,
 	size_t count;
 	loff_t offs = *off;
 
-	while (node != NULL) {
+	for (; node != NULL; node = node->next)
 		node_count++;
-		node = node->next;
-	}
+
+	size = node_count * dumped_node_size;
 
 	if (node_count != prev_node_count) {
 		kfree(last_job_chars);
 
-		last_job_chars = kzalloc(node_count * dumped_node_size,
-				GFP_KERNEL);
+		last_job_chars = kzalloc(size, GFP_KERNEL);
 		if (!last_job_chars)
 			return 0;
+		prev_node_count = node_count;
 	}
 
 	mutex_lock(&last_job_lock);
@@ -269,17 +269,13 @@ static ssize_t last_job_read(struct file *filep, char __user *buf,
 	}
 	mutex_unlock(&last_job_lock);
 
-	prev_node_count = node_count;
-
-	size = node_count * dumped_node_size;
-
-	count = (size > bytes) ? bytes : size;
-
-	if (offs > count)
+	if (offs > size)
 		return 0;
 
-	if ((offs + count) > size)
+	if (offs + bytes > size)
 		count = size - offs;
+	else
+		count = bytes;
 
 	if (copy_to_user(buf, last_job_chars + offs, count))
 		return -EFAULT;
@@ -330,7 +326,7 @@ int b2r2_debug_init(struct device *log_dev)
 #endif
 
 	stats_dir = debugfs_create_dir("stats", root_dir);
-	(void)debugfs_create_file("last_job", 0644, stats_dir, NULL,
+	(void)debugfs_create_file("last_job", 0444, stats_dir, NULL,
 			&last_job_fops);
 
 	mutex_init(&last_job_lock);
diff --git a/drivers/video/b2r2/b2r2_node_split.c b/drivers/video/b2r2/b2r2_node_split.c
index 853323e9de9..c7ce0572d11 100644
--- a/drivers/video/b2r2/b2r2_node_split.c
+++ b/drivers/video/b2r2/b2r2_node_split.c
@@ -113,8 +113,9 @@ static const u32 vmx_yvu_to_yuv[] = {
  * Forward declaration of private functions
  */
 
-static int analyze_fmt_conv(struct b2r2_node_split_job *this,
-		const struct b2r2_blt_request *req, u32 *node_count);
+static int analyze_fmt_conv(struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count);
 static int analyze_color_fill(struct b2r2_node_split_job *this,
 		const struct b2r2_blt_request *req, u32 *node_count);
 static int analyze_copy(struct b2r2_node_split_job *this,
@@ -123,7 +124,7 @@ static int analyze_copy(struct b2r2_node_split_job *this,
 static int analyze_scaling(struct b2r2_node_split_job *this,
 		const struct b2r2_blt_request *req, u32 *node_count,
 		u32 *buf_count);
-static int analyze_rotation(struct b2r2_node_split_job *this,
+static int analyze_rotate(struct b2r2_node_split_job *this,
 		const struct b2r2_blt_request *req, u32 *node_count,
 		u32 *buf_count);
 static int analyze_transform(struct b2r2_node_split_job *this,
@@ -189,6 +190,7 @@ static bool fmt_has_alpha(enum b2r2_blt_fmt fmt);
 static bool is_rgb_fmt(enum b2r2_blt_fmt fmt);
 static bool is_bgr_fmt(enum b2r2_blt_fmt fmt);
 static bool is_yuv_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yvu_fmt(enum b2r2_blt_fmt fmt);
 static bool is_yuv420_fmt(enum b2r2_blt_fmt fmt);
 static bool is_yuv422_fmt(enum b2r2_blt_fmt fmt);
 static bool is_yuv444_fmt(enum b2r2_blt_fmt fmt);
@@ -308,8 +310,8 @@ int b2r2_node_split_analyze(const struct b2r2_blt_request *req,
 			(this->global_alpha != 255))
 		this->blend = true;
 	else if (this->flags & B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)
-		this->blend = fmt_has_alpha(this->src.fmt) ||
-				fmt_has_alpha(this->dst.fmt);
+		this->blend = (color_fill && fmt_has_alpha(this->dst.fmt)) ||
+				fmt_has_alpha(this->src.fmt);
 
 	if (this->blend && this->src.type == B2R2_FMT_TYPE_PLANAR) {
 		b2r2_log_warn("%s: Unsupported: blend with planar source\n",
@@ -425,6 +427,8 @@ int b2r2_node_split_analyze(const struct b2r2_blt_request *req,
 			__func__, this->dst.win.x, this->dst.win.y,
 			this->dst.win.width, this->dst.win.height, this->dst.dx,
 			this->dst.dy);
+	b2r2_log_info("%s: buf_count=%d, buf_size=%d, node_count=%d\n",
+			__func__, *buf_count, bufs[0]->size, *node_count);
 
 	return 0;
 
@@ -440,6 +444,8 @@ unsupported:
 int b2r2_node_split_configure(struct b2r2_node_split_job *this,
 		struct b2r2_node *first)
 {
+	int ret;
+
 	struct b2r2_node_split_buf *dst = &this->dst;
 	struct b2r2_node *node = first;
 
@@ -462,10 +468,13 @@ int b2r2_node_split_configure(struct b2r2_node_split_job *this,
 			if (dst_w > dst->rect.width - x_pixels)
 				dst->win.width = dst->rect.width - x_pixels;
 
-			configure_tile(this, node, &node);
+			ret = configure_tile(this, node, &node);
+			if (ret < 0)
+				goto error;
 
 			dst->win.x += dst->dx;
 			x_pixels += max(dst->dx, -dst->dx);
+			b2r2_log_info("%s: x_pixels=%d\n", __func__, x_pixels);
 		}
 
 		dst->win.y += dst->dy;
@@ -479,6 +488,10 @@ int b2r2_node_split_configure(struct b2r2_node_split_job *this,
 	}
 
 	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
 }
 
 /**
@@ -626,50 +639,46 @@ error:
 /**
  * analyze_fmt_conv() - analyze the format conversions needed for a job
  */
-static int analyze_fmt_conv(struct b2r2_node_split_job *this,
-		const struct b2r2_blt_request *req, u32 *node_count)
+static int analyze_fmt_conv(struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count)
 {
-	if (is_rgb_fmt(this->src.fmt)) {
-		if (is_yuv_fmt(this->dst.fmt)) {
-			if (this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR)
-				this->ivmx = &vmx_rgb_to_yvu[0];
-			else
-				this->ivmx = &vmx_rgb_to_yuv[0];
-		} else if (is_bgr_fmt(this->dst.fmt)) {
-			this->ivmx = &vmx_rgb_to_bgr[0];
-		}
-	} else if (is_yuv_fmt(this->src.fmt)) {
-		if (this->src.fmt == B2R2_BLT_FMT_Y_CB_Y_CR) {
-			if (is_rgb_fmt(this->dst.fmt))
-				this->ivmx = &vmx_yvu_to_rgb[0];
-			else if (is_bgr_fmt(this->dst.fmt))
-				this->ivmx = &vmx_yvu_to_bgr[0];
-			else if (this->dst.fmt != B2R2_BLT_FMT_Y_CB_Y_CR)
-				this->ivmx = &vmx_yvu_to_yuv[0];
-		} else {
-			if (is_rgb_fmt(this->dst.fmt))
-				this->ivmx = &vmx_yuv_to_rgb[0];
-			else if (is_bgr_fmt(this->dst.fmt))
-				this->ivmx = &vmx_yuv_to_bgr[0];
-			else if (this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR)
-				this->ivmx = &vmx_yvu_to_yuv[0];
-		}
-	} else if (is_bgr_fmt(this->src.fmt)) {
-		if (is_rgb_fmt(this->dst.fmt))
-			this->ivmx = &vmx_rgb_to_bgr[0];
-		else if (is_yuv_fmt(this->dst.fmt)) {
-			if (this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR)
-				this->ivmx = &vmx_bgr_to_yvu[0];
-			else
-				this->ivmx = &vmx_bgr_to_yuv[0];
-		}
-	}
-
-	if (this->dst.type == B2R2_FMT_TYPE_RASTER) {
+	if (is_rgb_fmt(src->fmt)) {
+		if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_yvu[0];
+		else if (is_yuv_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_yuv[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_bgr[0];
+	} else if (is_yvu_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_rgb[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_bgr[0];
+		else if (is_yuv_fmt(dst->fmt) &&
+				!is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_yuv[0];
+	} else if (is_yuv_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_yuv_to_rgb[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_yuv_to_bgr[0];
+		else if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_yuv[0];
+	} else if (is_bgr_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_bgr[0];
+		else if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_bgr_to_yvu[0];
+		else if (is_yuv_fmt(dst->fmt))
+			*vmx = &vmx_bgr_to_yuv[0];
+	}
+
+	if (dst->type == B2R2_FMT_TYPE_RASTER) {
 		*node_count = 1;
-	} else if (this->dst.type == B2R2_FMT_TYPE_SEMI_PLANAR) {
+	} else if (dst->type == B2R2_FMT_TYPE_SEMI_PLANAR) {
 		*node_count = 2;
-	} else if (this->dst.type == B2R2_FMT_TYPE_PLANAR) {
+	} else if (dst->type == B2R2_FMT_TYPE_PLANAR) {
 		*node_count = 3;
 	} else {
 		/* That's strange... */
@@ -756,7 +765,7 @@ static int analyze_color_fill(struct b2r2_node_split_job *this,
 						this->src.color);
 		}
 
-		ret = analyze_fmt_conv(this, req, node_count);
+		ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx, node_count);
 		if (ret < 0)
 			goto error;
 	}
@@ -788,6 +797,17 @@ static int analyze_transform(struct b2r2_node_split_job *this,
 	 */
 	this->rotation = (this->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0;
 
+	/* B2R2 cannot do rotations if the destination is not raster, or 422R */
+	if (this->rotation && (this->dst.type != B2R2_FMT_TYPE_RASTER ||
+				this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR ||
+				this->dst.fmt == B2R2_BLT_FMT_CB_Y_CR_Y)) {
+		b2r2_log_warn("%s: Unsupported operation "
+				"(rot && (!dst_raster || dst==422R))",
+				__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
 	/* Flip the image by changing the scan order of the destination */
 	if (this->transform & B2R2_BLT_TRANSFORM_FLIP_H)
 		this->dst.hso = B2R2_TY_HSO_RIGHT_TO_LEFT;
@@ -810,14 +830,6 @@ static int analyze_transform(struct b2r2_node_split_job *this,
 			(this->dst.type == B2R2_FMT_TYPE_SEMI_PLANAR) ||
 			(this->dst.type == B2R2_FMT_TYPE_PLANAR);
 
-	if (is_scaling && this->rotation) {
-		/* TODO: Unsupported for now */
-		b2r2_log_info("%s: Unsupported operation (rot+rescale)\n",
-				__func__);
-		ret = -ENOSYS;
-		goto unsupported;
-	}
-
 	if (is_scaling && this->rotation && this->blend) {
 		/* TODO: This is unsupported. Fix it! */
 		b2r2_log_info("%s: Unsupported operation (rot+rescale+blend)\n",
@@ -836,7 +848,7 @@ static int analyze_transform(struct b2r2_node_split_job *this,
 		if (ret < 0)
 			goto error;
 	} else if (this->rotation) {
-		ret = analyze_rotation(this, req, node_count, buf_count);
+		ret = analyze_rotate(this, req, node_count, buf_count);
 		if (ret < 0)
 			goto error;
 	} else {
@@ -847,12 +859,10 @@ static int analyze_transform(struct b2r2_node_split_job *this,
 		this->type = B2R2_FLIP;
 	}
 
-	b2r2_log_info("%s exit\n", __func__);
-
 	return 0;
 
 error:
-	b2r2_log_warn("%s: Exit...\n", __func__);
+	b2r2_log_warn("%s: error!\n", __func__);
 unsupported:
 	return ret;
 }
@@ -885,7 +895,7 @@ static int analyze_copy(struct b2r2_node_split_job *this,
 
 		this->type = B2R2_COPY;
 
-		ret = analyze_fmt_conv(this, req, &copy_count);
+		ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx, &copy_count);
 		if (ret < 0)
 			goto error;
 
@@ -899,6 +909,146 @@ error:
 	return ret;
 }
 
+static int calc_rot_count(u32 width, u32 height)
+{
+	int count;
+
+	count = width / B2R2_ROTATE_MAX_WIDTH;
+	if (width % B2R2_ROTATE_MAX_WIDTH)
+		count++;
+	if (height > B2R2_ROTATE_MAX_WIDTH &&
+			height % B2R2_ROTATE_MAX_WIDTH)
+		count *= 2;
+
+	return count;
+}
+
+static int analyze_rot_scale_downscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	u32 num_rows;
+	u32 num_cols;
+
+	u32 rot_count;
+	u32 rescale_count;
+
+	u32 nodes_per_rot;
+	u32 nodes_per_rescale;
+
+	u32 right_width;
+	u32 bottom_height;
+
+	const u32 *dummy_vmx;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Calculate the desired tmp buffer size */
+	tmp->win.width = rescale(B2R2_RESCALE_MAX_WIDTH - 1, this->h_rsf);
+	tmp->win.width >>= 10;
+	tmp->win.width = min(tmp->win.width, dst->rect.height);
+	tmp->win.height = dst->rect.width;
+
+	setup_tmp_buf(tmp, this->max_buf_size, dst->fmt, tmp->win.width, tmp->win.height);
+	tmp->tmp_buf_index = 1;
+	this->work_bufs[0].size = tmp->pitch * tmp->height;
+
+	tmp->win.width = tmp->rect.width;
+	tmp->win.height = tmp->rect.height;
+
+	tmp->dither = dst->dither;
+	dst->dither = 0;
+
+	/* Update the dst window with the actual tmp buffer dimensions */
+	dst->win.width = tmp->win.height;
+	dst->win.height = tmp->win.width;
+
+	/* The rotated stripes are written to the destination bottom-up */
+	if (this->dst.vso == B2R2_TY_VSO_TOP_TO_BOTTOM)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+	else
+		this->dst.vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	/*
+	 * Calculate how many nodes are required to copy to and from the tmp
+	 * buffer
+	 */
+	ret = analyze_fmt_conv(src, tmp, &this->ivmx, &nodes_per_rescale);
+	if (ret < 0)
+		goto error;
+
+	/* We will not do any format conversion in the rotation stage */
+	ret = analyze_fmt_conv(tmp, dst, &dummy_vmx, &nodes_per_rot);
+	if (ret < 0)
+		goto error;
+
+	/* Calculate node count for the inner tiles */
+	num_cols = dst->rect.width / dst->win.width;
+	num_rows = dst->rect.height / dst->win.height;
+
+	rescale_count = num_cols * num_rows;
+	rot_count = calc_rot_count(dst->win.height, dst->win.width) *
+		num_cols * num_rows;
+
+	right_width = dst->rect.width % dst->win.width;
+	bottom_height = dst->rect.height % dst->win.height;
+
+	/* Calculate node count for the rightmost tiles */
+	if (right_width) {
+		u32 count = calc_rot_count(dst->win.height, right_width);
+
+		rot_count += count * num_rows;
+		rescale_count += num_rows;
+		b2r2_log_info("%s: rightmost: %d nodes\n", __func__,
+				count*num_rows);
+	}
+
+	/* Calculate node count for the bottom tiles */
+	if (bottom_height) {
+		u32 count = calc_rot_count(bottom_height, dst->win.width);
+
+		rot_count += count * num_cols;
+		rescale_count += num_cols;
+		b2r2_log_info("%s: bottom: %d nodes\n", __func__,
+				count * num_cols);
+
+	}
+
+	/* And finally for the bottom right corner */
+	if (right_width && bottom_height) {
+		u32 count = calc_rot_count(bottom_height, right_width);
+
+		rot_count += count;
+		rescale_count++;
+		b2r2_log_info("%s: bottom right: %d nodes\n", __func__, count);
+
+	}
+
+	*node_count = rot_count * nodes_per_rot;
+	*node_count += rescale_count * nodes_per_rescale;
+	*buf_count = 1;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+static int analyze_rot_scale_upscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	/* TODO: When upscaling we should optimally to the rotation first... */
+	return analyze_rot_scale_downscale(this, req, node_count, buf_count);
+}
+
 /**
  * analyze_rot_scaling() - analyzes a combined rotation and scaling op
  */
@@ -906,11 +1056,30 @@ static int analyze_rot_scale(struct b2r2_node_split_job *this,
 		const struct b2r2_blt_request *req, u32 *node_count,
 		u32 *buf_count)
 {
-	/*
-	 * TODO: This code was very error prone and has been removed. It needs
-	 *       to be reimplemented in a better way...
-	 */
-	return -ENOSYS;
+	int ret;
+
+	bool upscale;
+
+	ret = analyze_scale_factors(this);
+	if (ret < 0)
+		goto error;
+
+	upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 20);
+
+	if (upscale)
+		ret = analyze_rot_scale_upscale(this, req, node_count, buf_count);
+	else
+		ret = analyze_rot_scale_downscale(this, req, node_count, buf_count);
+
+	if (ret < 0)
+		goto error;
+
+	this->type = B2R2_SCALE_AND_ROTATE;
+
+	return 0;
+
+error:
+	return ret;
 }
 
 /**
@@ -921,6 +1090,7 @@ static int analyze_scaling(struct b2r2_node_split_job *this,
 		u32 *buf_count)
 {
 	int ret;
+
 	u32 copy_count;
 	u32 nbr_cols;
 
@@ -933,7 +1103,7 @@ static int analyze_scaling(struct b2r2_node_split_job *this,
 		goto error;
 
 	/* Find out how many nodes a simple copy would require */
-	ret = analyze_fmt_conv(this, req, &copy_count);
+	ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx, &copy_count);
 	if (ret < 0)
 		goto error;
 
@@ -982,20 +1152,19 @@ error:
 }
 
 /**
- * analyze_rotation() - analyze a rotate operation
+ * analyze_rotate() - analyze a rotate operation
  */
-static int analyze_rotation(struct b2r2_node_split_job *this,
+static int analyze_rotate(struct b2r2_node_split_job *this,
 		const struct b2r2_blt_request *req, u32 *node_count,
 		u32 *buf_count)
 {
 	int ret;
 
 	u32 nodes_per_tile;
-	u32 nbr_rows;
-	u32 nbr_cols;
 
 	/* Find out how many nodes a simple copy would require */
-	ret = analyze_fmt_conv(this, req, &nodes_per_tile);
+	ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx,
+			&nodes_per_tile);
 	if (ret < 0)
 		goto error;
 
@@ -1052,17 +1221,9 @@ static int analyze_rotation(struct b2r2_node_split_job *this,
 		nodes_per_tile++;
 	}
 
-	/* Calculate how many tiles this will result in */
-	nbr_cols = this->dst.rect.width / this->dst.win.width;
-	if (this->dst.rect.width % this->dst.win.width)
-		nbr_cols++;
-
-	nbr_rows = this->dst.rect.height / this->dst.win.height;
-	if (this->dst.rect.height % this->dst.win.height)
-		nbr_rows++;
-
 	/* Finally, calculate the node count */
-	*node_count = nodes_per_tile * nbr_rows * nbr_cols;
+	*node_count = nodes_per_tile *
+		calc_rot_count(this->dst.rect.width, this->dst.rect.height);
 
 	return 0;
 
@@ -1082,13 +1243,13 @@ static int analyze_scale_factors(struct b2r2_node_split_job *this)
 	u16 vsf;
 
 	if (this->rotation) {
-		ret = calculate_scale_factor(this->src.rect.width,
-					this->dst.rect.height, &hsf);
+		ret = calculate_scale_factor(this->src.rect.height,
+					this->dst.rect.width, &hsf);
 		if (ret < 0)
 			goto error;
 
-		ret = calculate_scale_factor(this->src.rect.height,
-					this->dst.rect.width, &vsf);
+		ret = calculate_scale_factor(this->src.rect.width,
+					this->dst.rect.height, &vsf);
 		if (ret < 0)
 			goto error;
 	} else {
@@ -1109,6 +1270,9 @@ static int analyze_scale_factors(struct b2r2_node_split_job *this)
 	this->h_rsf = hsf;
 	this->v_rsf = vsf;
 
+	b2r2_log_info("%s: h_rsf=%.4x\n", __func__, this->h_rsf);
+	b2r2_log_info("%s: v_rsf=%.4x\n", __func__, this->v_rsf);
+
 	return 0;
 error:
 	b2r2_log_warn("%s: Exit...\n", __func__);
@@ -1269,17 +1433,168 @@ error:
 	return ret;
 }
 
+/*
+ * configure_sub_rot() - configure a sub-rotation
+ *
+ * This functions configures a set of nodes for rotation using the destination
+ * window instead of the rectangle for calculating tiles.
+ */
+static int configure_sub_rot(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *job)
+{
+	int ret;
+
+	struct b2r2_blt_rect src_win;
+	struct b2r2_blt_rect dst_win;
+
+	u32 y_pixels;
+	u32 x_pixels;
+
+	memcpy(&src_win, &src->win, sizeof(src_win));
+	memcpy(&dst_win, &dst->win, sizeof(dst_win));
+
+	b2r2_log_info("%s: src_win=(%d, %d, %d, %d) "
+			"dst_win=(%d, %d, %d, %d)\n", __func__,
+			src_win.x, src_win.y, src_win.width, src_win.height,
+			dst_win.x, dst_win.y, dst_win.width, dst_win.height);
+
+	dst->win.height = B2R2_ROTATE_MAX_WIDTH;
+	if (dst->win.width % B2R2_ROTATE_MAX_WIDTH)
+		dst->win.width -= dst->win.width % B2R2_ROTATE_MAX_WIDTH;
+
+	while (x_pixels < dst_win.width) {
+		u32 src_x = src->win.x;
+		u32 src_w = src->win.width;
+		u32 dst_y = dst->win.y;
+		u32 dst_h = dst->win.height;
+
+		dst->win.width = min(dst->win.width,
+				dst_win.width - (int)x_pixels);
+		src->win.height = dst->win.width;
+
+		b2r2_log_info("%s: x_pixels=%d\n", __func__, x_pixels);
+
+		while (y_pixels < dst_win.height) {
+			dst->win.height = min(dst->win.height,
+					dst_win.height - (int)y_pixels);
+			src->win.width = dst->win.height;
+
+			b2r2_log_info("%s: y_pixels=%d\n", __func__, y_pixels);
+
+			ret = configure_rotate(node, src, dst, ivmx, &node, job);
+			if (ret < 0)
+				goto error;
+
+			src->win.x += (src->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+					src->win.width : -src->win.width;
+			dst->win.y += (dst->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+					dst->win.height : -dst->win.height;
+
+			y_pixels += dst->win.height;
+		}
+
+		src->win.x = src_x;
+		src->win.y += (src->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+				src->win.height : -src->win.height;
+		src->win.width = src_w;
+
+		dst->win.x += (dst->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+				dst->win.width : -dst->win.width;
+		dst->win.y = dst_y;
+		dst->win.height = dst_h;
+
+		x_pixels += dst->win.width;
+		y_pixels = 0;
+
+	}
+
+	memcpy(&src->win, &src_win, sizeof(src->win));
+	memcpy(&dst->win, &dst_win, sizeof(dst->win));
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_downscale() - configures a combined rotate and downscale
+ *
+ * When doing a downscale it is better to do the rotation last.
+ */
+static int configure_rot_downscale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	tmp->win.x = 0;
+	tmp->win.y = 0;
+	tmp->win.width = dst->win.height;
+	tmp->win.height = dst->win.width;
+
+	ret = configure_scale(node, src, tmp, this->h_rsf, this->v_rsf,
+			this->ivmx, &node, this);
+	if (ret < 0)
+		goto error;
+
+	ret = configure_sub_rot(node, tmp, dst, NULL, &node, this);
+	if (ret < 0)
+		goto error;
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_info("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_upscale() - configures a combined rotate and upscale
+ *
+ * When doing an upscale it is better to do the rotation first.
+ */
+static int configure_rot_upscale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	/* TODO: Implement a optimal upscale (rotation first) */
+	return configure_rot_downscale(this, node, next);
+}
+
 /**
  * configure_rot_scale() - configures a combined rotation and scaling op
  */
 static int configure_rot_scale(struct b2r2_node_split_job *this,
 		struct b2r2_node *node, struct b2r2_node **next)
 {
-	/*
-	 * TODO: This code was very error prone and has been removed. It should
-	 *       be reimplemented in a better way.
-	 */
-	return -ENOSYS;
+	int ret;
+
+	bool upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 10);
+
+	if (upscale)
+		ret = configure_rot_upscale(this, node, next);
+	else
+		ret = configure_rot_downscale(this, node, next);
+
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
 }
 
 /**
@@ -1569,7 +1884,7 @@ static int configure_rotate(struct b2r2_node *node,
 
 	return 0;
 error:
-	b2r2_log_warn("%s: Exit...\n", __func__);
+	b2r2_log_warn("%s: error!\n", __func__);
 	return ret;
 }
 
@@ -2240,6 +2555,8 @@ static int setup_tmp_buf(struct b2r2_node_split_buf *tmp, u32 max_size,
 		fmt = B2R2_BLT_FMT_32_BIT_ARGB8888;
 	} else if (is_bgr_fmt(pref_fmt)) {
 		fmt = B2R2_BLT_FMT_32_BIT_ABGR8888;
+	} else if (is_yvu_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_CB_Y_CR_Y;
 	} else if (is_yuv_fmt(pref_fmt)) {
 		fmt = B2R2_BLT_FMT_32_BIT_AYUV8888;
 	} else {
@@ -2447,6 +2764,19 @@ static bool is_yuv_fmt(enum b2r2_blt_fmt fmt)
 }
 
 /**
+ * is_yvu_fmt() - returns whether the given format is a yvu format
+ */
+static bool is_yvu_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
  * is_yuv420_fmt() - returns whether the given format is a yuv420 format
  */
 static bool is_yuv420_fmt(enum b2r2_blt_fmt fmt)