From c5d00ceef2977a9376fddcaba5be49d371a7b840 Mon Sep 17 00:00:00 2001
From: Johnson Ding <johnson.ding@rock-chips.com>
Date: Tue, 7 May 2019 11:44:42 +0800
Subject: [PATCH] video: rockchip: vcodec: adjust system status base on decoder
 tasks

Change-Id: Ibcd5789e92d4e80f6d3e51e335c75d5aefa01206
Signed-off-by: Johnson Ding <johnson.ding@rock-chips.com>
---
 .../video/rockchip/vcodec/vcodec_service.c    | 258 +++++++++++++++++-
 .../video/rockchip/vcodec/vcodec_service.h    |   2 +
 2 files changed, 259 insertions(+), 1 deletion(-)

diff --git a/drivers/video/rockchip/vcodec/vcodec_service.c b/drivers/video/rockchip/vcodec/vcodec_service.c
index a387016d5e93..c072e48d84fa 100644
--- a/drivers/video/rockchip/vcodec/vcodec_service.c
+++ b/drivers/video/rockchip/vcodec/vcodec_service.c
@@ -16,7 +16,7 @@
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
+#include <dt-bindings/soc/rockchip-system-status.h>
 #include <linux/clk.h>
 #include <linux/compat.h>
 #include <linux/delay.h>
@@ -55,6 +55,7 @@
 #include <video/rk_vpu_service.h>
 #include <soc/rockchip/pm_domains.h>
 #include <soc/rockchip/rockchip_opp_select.h>
+#include <soc/rockchip/rockchip-system-status.h>
 
 #include "vcodec_hw_info.h"
 #include "vcodec_hw_vpu.h"
@@ -479,6 +480,225 @@ struct compat_vpu_request {
 #define VPU_POWER_OFF_DELAY		(4 * HZ) /* 4s */
 #define VPU_TIMEOUT_DELAY		(2 * HZ) /* 2s */
 
+#define VIDEO_INFO 1
+
+#if VIDEO_INFO
+
+struct video_info {
+	unsigned int width;
+	u8 bit_depth;
+	pid_t pid;
+	struct list_head node;
+};
+
+static DEFINE_MUTEX(video_info_mutex);
+static LIST_HEAD(video_info_list);
+
+static struct video_info *find_video_info(pid_t pid)
+{
+	struct video_info *info;
+
+	mutex_lock(&video_info_mutex);
+	list_for_each_entry(info, &video_info_list, node) {
+		if (info->pid == pid) {
+			mutex_unlock(&video_info_mutex);
+			return info;
+		}
+	}
+	mutex_unlock(&video_info_mutex);
+	return NULL;
+}
+
+static int add_video_info(pid_t pid, unsigned int width, u8 bit_depth)
+{
+	struct video_info *info = find_video_info(pid);
+
+	if (info) {
+		info->width = width;
+		info->bit_depth = bit_depth;
+	} else {
+		info = kzalloc(sizeof(*info), GFP_KERNEL);
+		if (!info)
+			return -ENOMEM;
+		info->pid = pid;
+		info->width = width;
+		info->bit_depth = bit_depth;
+		mutex_lock(&video_info_mutex);
+		list_add(&info->node, &video_info_list);
+		mutex_unlock(&video_info_mutex);
+	}
+	return 0;
+}
+
+static int del_video_info(pid_t pid)
+{
+	struct video_info *info = find_video_info(pid);
+
+	if (!info)
+		return -1;
+
+	mutex_lock(&video_info_mutex);
+	list_del(&info->node);
+	mutex_unlock(&video_info_mutex);
+	kfree(info);
+	return 0;
+}
+
+static bool has_4k_in_list(void)
+{
+	struct video_info *info;
+
+	mutex_lock(&video_info_mutex);
+	list_for_each_entry(info, &video_info_list, node) {
+		if (info->width > 1920 && info->bit_depth != 10) {
+			mutex_unlock(&video_info_mutex);
+			return true;
+		}
+	}
+	mutex_unlock(&video_info_mutex);
+	return false;
+}
+
+static bool has_1080p_in_list(void)
+{
+	struct video_info *info;
+
+	mutex_lock(&video_info_mutex);
+	list_for_each_entry(info, &video_info_list, node) {
+		if (info->width <= 1920 && info->width > 0) {
+			mutex_unlock(&video_info_mutex);
+			return true;
+		}
+	}
+	mutex_unlock(&video_info_mutex);
+	return false;
+}
+
+static bool has_4k_10bit_in_list(void)
+{
+	struct video_info *info;
+
+	mutex_lock(&video_info_mutex);
+	list_for_each_entry(info, &video_info_list, node) {
+		if (info->bit_depth == 10 && info->width > 1920) {
+			mutex_unlock(&video_info_mutex);
+			return true;
+		}
+	}
+	mutex_unlock(&video_info_mutex);
+	return false;
+}
+
+static int update_system_status(void)
+{
+	unsigned long previous_status = rockchip_get_system_status();
+	bool is_previous_4k = previous_status & (SYS_STATUS_VIDEO_4K);
+	bool is_previous_1080P = previous_status & (SYS_STATUS_VIDEO_1080P);
+	bool is_previous_4k_10bit = previous_status & (SYS_STATUS_VIDEO_4K_10B);
+	bool has_4k = has_4k_in_list();
+	bool has_1080p = has_1080p_in_list();
+	bool has_4k_10bit = has_4k_10bit_in_list();
+
+	if (has_4k) {
+		if (!is_previous_4k)
+			rockchip_set_system_status(SYS_STATUS_VIDEO_4K);
+	} else {
+		if (is_previous_4k)
+			rockchip_clear_system_status(SYS_STATUS_VIDEO_4K);
+	}
+
+	if (has_1080p) {
+		if (!is_previous_1080P)
+			rockchip_set_system_status(SYS_STATUS_VIDEO_1080P);
+	} else {
+		if (is_previous_1080P)
+			rockchip_clear_system_status(SYS_STATUS_VIDEO_1080P);
+	}
+
+	if (has_4k_10bit) {
+		if (!is_previous_4k_10bit)
+			rockchip_set_system_status(SYS_STATUS_VIDEO_4K_10B);
+	} else {
+		if (is_previous_4k_10bit)
+			rockchip_clear_system_status(SYS_STATUS_VIDEO_4K_10B);
+	}
+
+	return 0;
+}
+
+static int probe_width(struct vpu_reg *reg)
+{
+	int width = 0;
+	int type = -1;
+
+	if (reg->type != VPU_DEC && reg->type != VPU_DEC_PP) {
+		pr_info("type is not dec or pp, return\n");
+		return -1;
+	}
+	if (reg->task->get_fmt) {
+		type = reg->task->get_fmt(reg->reg);
+	} else {
+		pr_info("invalid task with NULL get_fmt\n");
+		return -EINVAL;
+	}
+
+	switch (reg->data->hw_id) {
+	case VPU_DEC_ID_9190:
+	case VPU_ID_8270:
+	case VPU_ID_4831:
+
+		switch (type) {
+		case FMT_JPEGD:
+		case FMT_H263D:
+		case FMT_H264D:
+		case FMT_MPEG2D:
+		case FMT_MPEG4D:
+		case FMT_VP6D:
+		case FMT_VP8D:
+		case FMT_VC1D:
+		case FMT_AVSD:
+			width = (reg->reg[04] & 0xff800000) >> 19;
+			break;
+		default: //MPEG1D, VP7D, H265D, VP9D, FMT_PP, JPEGE, H264E, VP8E
+			pr_err("unsupported FORMAT_TYPE %d\n", type);
+			break;
+		}
+		break;
+
+	case HEVC_ID:
+	case RKV_DEC_ID:
+	case RKV_DEC_ID2:
+		if (type == FMT_H264D || type == FMT_H265D || type == FMT_VP9D)
+			width = (reg->reg[3] & 0x3ff) << 4;
+		break;
+
+	case VPU2_ID:
+		switch (type) {
+		case FMT_H264D:
+			width = (reg->reg[110] & 0x1ff) << 4;
+			break;
+		case FMT_JPEGD:
+		case FMT_H263D:
+		case FMT_MPEG2D:
+		case FMT_MPEG4D:
+		case FMT_VP6D:
+		case FMT_VP8D:
+		case FMT_VC1D:
+		case FMT_AVSD:
+			width = (reg->reg[120] & 0xff800000) >> 19;
+			break;
+		default: //MPEG1D, VP7D, H265D, VP9D, FMT_PP, JPEGE, H264E, VP8E
+			pr_err("unsupported FORMAT_TYPE %d\n", type);
+			break;
+		}
+		break;
+	}
+
+	return width;
+}
+
+#endif
+
 static void vcodec_reduce_freq_rk3328(struct vpu_service_info *pservice);
 
 static void vpu_service_power_on(struct vpu_subdev_data *data,
@@ -1011,6 +1231,21 @@ static int fill_scaling_list_pps(struct vpu_subdev_data *data,
 	}
 	pps = vaddr + offset;
 
+#if VIDEO_INFO
+	{
+		u8 bit_depth = 0;
+
+		if (pps_info_size == 32) {	//FMT_H264D
+			bit_depth = ((((*(pps + 2)) & 0x03) << 1) |
+					(((*(pps + 1)) & 0x80) >> 7)) + 8;
+		} else if (pps_info_size == 80) {	//FMT_H265D
+			bit_depth = ((*(pps + 4)) & 0xf0) >> 4;
+		}
+		if (bit_depth != reg->session->bit_depth)
+			reg->session->bit_depth = bit_depth;
+	}
+#endif
+
 	memcpy(&scaling_offset, pps + base, sizeof(scaling_offset));
 	scaling_offset = le32_to_cpu(scaling_offset);
 
@@ -1268,6 +1503,21 @@ static struct vpu_reg *reg_init(struct vpu_subdev_data *data,
 	if (pservice->auto_freq && pservice->hw_ops->get_freq)
 		pservice->hw_ops->get_freq(data, reg);
 
+#if VIDEO_INFO
+	{
+		unsigned int current_width = probe_width(reg);
+
+		if (current_width > 0 &&
+				reg->session->width != current_width) {
+			reg->session->width = current_width;
+			add_video_info(reg->session->pid, reg->session->width,
+					reg->session->bit_depth);
+			update_system_status();
+		}
+	}
+
+#endif
+
 	vpu_debug_leave();
 
 	return reg;
@@ -2026,6 +2276,7 @@ static int vpu_service_open(struct inode *inode, struct file *filp)
 
 	session->type	= VPU_TYPE_BUTT;
 	session->pid	= current->pid;
+	session->width	= 0;
 	INIT_LIST_HEAD(&session->waiting);
 	INIT_LIST_HEAD(&session->running);
 	INIT_LIST_HEAD(&session->done);
@@ -2076,6 +2327,10 @@ static int vpu_service_release(struct inode *inode, struct file *filp)
 	list_del_init(&session->list_session);
 	vpu_service_session_clear(data, session);
 	vcodec_iommu_clear(data->iommu_info, session);
+#if VIDEO_INFO
+	del_video_info(session->pid);
+	update_system_status();
+#endif
 	kfree(session);
 	filp->private_data = NULL;
 	mutex_unlock(&pservice->lock);
@@ -3529,6 +3784,7 @@ static int vcodec_probe(struct platform_device *pdev)
 
 	session->type	= VPU_TYPE_BUTT;
 	session->pid	= current->pid;
+	session->width	= 0;
 	INIT_LIST_HEAD(&session->waiting);
 	INIT_LIST_HEAD(&session->running);
 	INIT_LIST_HEAD(&session->done);
diff --git a/drivers/video/rockchip/vcodec/vcodec_service.h b/drivers/video/rockchip/vcodec/vcodec_service.h
index 2410471d0b8f..4e6a80a5e0b3 100644
--- a/drivers/video/rockchip/vcodec/vcodec_service.h
+++ b/drivers/video/rockchip/vcodec/vcodec_service.h
@@ -63,6 +63,8 @@ struct vpu_session {
 	wait_queue_head_t wait;
 	pid_t pid;
 	atomic_t task_running;
+	unsigned int width;
+	u8 bit_depth;
 };
 
 #endif