media: rockchip: vpss: support suspend and resume

Signed-off-by: Mingwei Yan <mingwei.yan@rock-chips.com>
Change-Id: I30a15c5952853b99968acbe732adcc83301e1d62

drivers/media/platform/rockchip/vpss/dev.c

@@ -347,6 +347,8 @@ static int rkvpss_plat_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	vpss_dev->is_probe_end = true;
 	init_waitqueue_head(&vpss_dev->stop_done);
+	vpss_dev->is_suspend = false;
+	vpss_dev->is_idle = true;
 	return 0;
 
 err_unreg_media_dev:
@@ -375,7 +377,7 @@ static int rkvpss_plat_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static int __maybe_unused rkvpss_runtime_suspend(struct device *dev)
+static int __maybe_unused rkvpss_dev_runtime_suspend(struct device *dev)
 {
 	struct rkvpss_device *vpss_dev = dev_get_drvdata(dev);
 	int ret;
@@ -386,7 +388,7 @@ static int __maybe_unused rkvpss_runtime_suspend(struct device *dev)
 	return (ret > 0) ? 0 : ret;
 }
 
-static int __maybe_unused rkvpss_runtime_resume(struct device *dev)
+static int __maybe_unused rkvpss_dev_runtime_resume(struct device *dev)
 {
 	struct rkvpss_device *vpss_dev = dev_get_drvdata(dev);
 	int ret;
@@ -397,9 +399,46 @@ static int __maybe_unused rkvpss_runtime_resume(struct device *dev)
 	return (ret > 0) ? 0 : ret;
 }
 
+static int rkvpss_pm_suspend(struct device *dev)
+{
+	struct rkvpss_device *vpss_dev = dev_get_drvdata(dev);
+	u32 ret;
+
+	v4l2_dbg(4, rkvpss_debug, &vpss_dev->v4l2_dev, "%s enter\n", __func__);
+	if (vpss_dev->vpss_sdev.state == VPSS_STOP)
+		return 0;
+
+	vpss_dev->is_suspend = true;
+	/* wait fe*/
+	if (!vpss_dev->is_idle) {
+		ret = wait_for_completion_timeout(&vpss_dev->pm_suspend_wait_fe,
+						  msecs_to_jiffies(200));
+		if (!ret)
+			v4l2_info(&vpss_dev->v4l2_dev, "%s wait fe timeout\n", __func__);
+	}
+	v4l2_dbg(4, rkvpss_debug, &vpss_dev->v4l2_dev, "%s exit\n", __func__);
+	return 0;
+}
+
+static int rkvpss_pm_resume(struct device *dev)
+{
+	struct rkvpss_device *vpss_dev = dev_get_drvdata(dev);
+
+	v4l2_dbg(4, rkvpss_debug, &vpss_dev->v4l2_dev, "%s enter\n", __func__);
+	if (vpss_dev->vpss_sdev.state == VPSS_STOP)
+		return 0;
+
+	vpss_dev->is_suspend = false;
+	v4l2_dbg(4, rkvpss_debug, &vpss_dev->v4l2_dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
 static const struct dev_pm_ops rkvpss_plat_pm_ops = {
-	SET_RUNTIME_PM_OPS(rkvpss_runtime_suspend,
-			   rkvpss_runtime_resume, NULL)
+	.suspend = rkvpss_pm_suspend,
+	.resume = rkvpss_pm_resume,
+	SET_RUNTIME_PM_OPS(rkvpss_dev_runtime_suspend,
+			   rkvpss_dev_runtime_resume, NULL)
 };
 
 struct platform_driver rkvpss_plat_drv = {

drivers/media/platform/rockchip/vpss/dev.h

@@ -79,6 +79,9 @@ struct rkvpss_device {
 	unsigned int irq_ends_mask;
 
 	bool is_probe_end;
+	bool is_suspend;
+	bool is_idle;
+	struct completion pm_suspend_wait_fe;
 };
 
 void rkvpss_pipeline_default_fmt(struct rkvpss_device *dev);

drivers/media/platform/rockchip/vpss/hw.c

@@ -702,6 +702,107 @@ static const struct of_device_id rkvpss_hw_of_match[] = {
 	{},
 };
 
+void rkvpss_hw_reg_save(struct rkvpss_hw_dev *dev)
+{
+	void *buf = dev->sw_reg;
+
+	dev_info(dev->dev, "%s\n", __func__);
+	memcpy_fromio(buf, dev->base_addr, RKVPSS_SW_REG_SIZE);
+}
+
+void rkvpss_hw_reg_restore(struct rkvpss_hw_dev *dev)
+{
+	void __iomem *base = dev->base_addr;
+	void *reg_buf = dev->sw_reg;
+	u32 val, *reg, i;
+
+	dev_info(dev->dev, "%s\n", __func__);
+
+	/* cmsc */
+	for (i = RKVPSS_CMSC_BASE; i <= RKVPSS_CMSC_WIN7_L3_SLP; i += 4) {
+		if (i == RKVPSS_CMSC_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_CMSC_GEN_UPD | RKVPSS_CMSC_UPDATE;
+	writel(val, base + RKVPSS_CMSC_UPDATE);
+
+	/* crop1 */
+	for (i = RKVPSS_CROP1_CTRL; i <= RKVPSS_CROP1_3_V_SIZE; i += 4) {
+		if (i == RKVPSS_CROP1_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_CROP_GEN_UPD | RKVPSS_CROP_FORCE_UPD;
+	writel(val, base + RKVPSS_CROP1_UPDATE);
+	writel(val, base + RKVPSS_CROP1_UPDATE);
+
+	/* scale */
+	for (i = RKVPSS_ZME_BASE; i <= RKVPSS_ZME_UV_YSCL_FACTOR; i += 4) {
+		if (i == RKVPSS_ZME_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_ZME_GEN_UPD | RKVPSS_ZME_FORCE_UPD;
+	writel(val, base + RKVPSS_ZME_UPDATE);
+
+	for (i = RKVPSS_SCALE1_BASE; i <= RKVPSS_SCALE1_IN_CROP_OFFSET; i += 4) {
+		if (i == RKVPSS_SCALE1_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_SCL_GEN_UPD | RKVPSS_SCL_FORCE_UPD;
+	writel(val, base + RKVPSS_SCALE1_UPDATE);
+
+	for (i = RKVPSS_SCALE2_BASE; i <= RKVPSS_SCALE2_IN_CROP_OFFSET; i += 4) {
+		if (i == RKVPSS_SCALE2_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_SCL_GEN_UPD | RKVPSS_SCL_FORCE_UPD;
+	writel(val, base + RKVPSS_SCALE2_UPDATE);
+
+	for (i = RKVPSS_SCALE3_BASE; i <= RKVPSS_SCALE3_IN_CROP_OFFSET; i += 4) {
+		if (i == RKVPSS_SCALE3_UPDATE)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_SCL_GEN_UPD | RKVPSS_SCL_FORCE_UPD;
+	writel(val, base + RKVPSS_SCALE3_UPDATE);
+
+	/* mi */
+	for (i = RKVPSS_MI_BASE; i <= RKVPSS_MI_CHN3_WR_LINE_CNT; i += 4) {
+		if (i >= RKVPSS_MI_RD_CTRL && i <= RKVPSS_MI_RD_Y_HEIGHT_SHD)
+			continue;
+		if (i == RKVPSS_MI_WR_INIT)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_MI_FORCE_UPD;
+	writel(val, base + RKVPSS_MI_WR_INIT);
+	writel(val, base + RKVPSS_MI_WR_INIT);
+
+	/* vpss ctrl */
+	for (i = RKVPSS_VPSS_BASE; i <= RKVPSS_VPSS_Y2R_OFF2; i += 4) {
+		if (i == RKVPSS_VPSS_UPDATE || i == RKVPSS_VPSS_RESET)
+			continue;
+		reg = reg_buf + i;
+		writel(*reg, base + i);
+	}
+	val = RKVPSS_CFG_FORCE_UPD | RKVPSS_CFG_GEN_UPD | RKVPSS_MIR_GEN_UPD |
+	      RKVPSS_ONLINE2_CHN_FORCE_UPD;
+	if (!dev->is_ofl_cmsc)
+		val |= RKVPSS_MIR_FORCE_UPD;
+	writel(val, base + RKVPSS_VPSS_UPDATE);
+}
+
 static int rkvpss_hw_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match;
@@ -721,6 +822,9 @@ static int rkvpss_hw_probe(struct platform_device *pdev)
 	if (!hw_dev)
 		return -ENOMEM;
 
+	hw_dev->sw_reg = devm_kzalloc(dev, RKVPSS_SW_REG_SIZE, GFP_KERNEL);
+	if (!hw_dev->sw_reg)
+		return -ENOMEM;
 	dev_set_drvdata(dev, hw_dev);
 	hw_dev->dev = dev;
 	match_data = match->data;
@@ -749,8 +853,8 @@ static int rkvpss_hw_probe(struct platform_device *pdev)
 		irq = platform_get_irq_byname(pdev, match_data->irqs[i].name);
 		if (irq < 0) {
 			dev_err(dev, "no irq %s in dts\n", match_data->irqs[i].name);
-				ret = irq;
-				goto err;
+			ret = irq;
+			goto err;
 		}
 		ret = devm_request_irq(dev, irq,
 				       match_data->irqs[i].irq_hdl,
@@ -798,6 +902,7 @@ static int rkvpss_hw_probe(struct platform_device *pdev)
 	hw_dev->is_dma_contig = true;
 	hw_dev->is_shutdown = false;
 	hw_dev->is_mmu = is_iommu_enable(dev);
+	hw_dev->is_suspend = false;
 	ret = of_reserved_mem_device_init(dev);
 	if (ret) {
 		is_mem_reserved = false;
@@ -839,40 +944,72 @@ static void rkvpss_hw_shutdown(struct platform_device *pdev)
 	}
 }
 
-static int __maybe_unused rkvpss_runtime_suspend(struct device *dev)
+static int __maybe_unused rkvpss_hw_runtime_suspend(struct device *dev)
 {
 	struct rkvpss_hw_dev *hw_dev = dev_get_drvdata(dev);
+	u32 ret;
+
+	if (rkvpss_debug >= 4)
+		dev_info(dev, "%s enter\n", __func__);
+
+	if (dev->power.runtime_status) {
+		rkvpss_hw_write(hw_dev, RKVPSS_MI_IMSC, 0);
+		rkvpss_hw_write(hw_dev, RKVPSS_VPSS_IMSC, 0);
+		hw_dev->ofl_dev.mode_sel_en = true;
+	} else {
+		rkvpss_hw_reg_save(hw_dev);
+		hw_dev->is_suspend = true;
+		if (hw_dev->ofl_dev.pm_need_wait) {
+			ret = wait_for_completion_timeout(&hw_dev->ofl_dev.pm_cmpl,
+							  msecs_to_jiffies(200));
+			if (!ret)
+				dev_info(dev, "%s pm wait offline timeout\n", __func__);
+		}
+	}
 
-	rkvpss_hw_write(hw_dev, RKVPSS_MI_IMSC, 0);
-	rkvpss_hw_write(hw_dev, RKVPSS_VPSS_IMSC, 0);
 	disable_sys_clk(hw_dev);
-	hw_dev->ofl_dev.mode_sel_en = true;
+
+	if (rkvpss_debug >= 4)
+		dev_info(dev, "%s exit\n", __func__);
+
 	return 0;
 }
 
-static int __maybe_unused rkvpss_runtime_resume(struct device *dev)
+static int __maybe_unused rkvpss_hw_runtime_resume(struct device *dev)
 {
 	struct rkvpss_hw_dev *hw_dev = dev_get_drvdata(dev);
 	void __iomem *base = hw_dev->base_addr;
 	int i;
 
+	if (rkvpss_debug >= 4)
+		dev_info(dev, "%s enter\n", __func__);
+
 	enable_sys_clk(hw_dev);
 	rkvpss_soft_reset(hw_dev);
 
-	for (i = 0; i < hw_dev->dev_num; i++) {
-		void *buf = hw_dev->vpss[i]->sw_base_addr;
+	if (dev->power.runtime_status) {
+		for (i = 0; i < hw_dev->dev_num; i++) {
+			void *buf = hw_dev->vpss[i]->sw_base_addr;
 
-		memset(buf, 0, RKVPSS_SW_REG_SIZE_MAX);
-		memcpy_fromio(buf, base, RKVPSS_SW_REG_SIZE);
+			memset(buf, 0, RKVPSS_SW_REG_SIZE_MAX);
+			memcpy_fromio(buf, base, RKVPSS_SW_REG_SIZE);
+		}
+	} else {
+		rkvpss_hw_reg_restore(hw_dev);
+		hw_dev->is_suspend = false;
 	}
+
+	if (rkvpss_debug >= 4)
+		dev_info(dev, "%s exit\n", __func__);
+
 	return 0;
 }
 
 static const struct dev_pm_ops rkvpss_hw_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+	LATE_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
 				pm_runtime_force_resume)
-	SET_RUNTIME_PM_OPS(rkvpss_runtime_suspend,
-			   rkvpss_runtime_resume, NULL)
+	SET_RUNTIME_PM_OPS(rkvpss_hw_runtime_suspend,
+			   rkvpss_hw_runtime_resume, NULL)
 };
 
 static struct platform_driver rkvpss_hw_drv = {

drivers/media/platform/rockchip/vpss/hw.h

@@ -33,6 +33,7 @@ struct rkvpss_hw_dev {
 	struct rkvpss_device *vpss[VPSS_MAX_DEV];
 	struct rkvpss_offline_dev ofl_dev;
 	struct list_head list;
+	void *sw_reg;
 	int clk_rate_tbl_num;
 	int clks_num;
 	int dev_num;
@@ -52,6 +53,7 @@ struct rkvpss_hw_dev {
 	bool is_single;
 	bool is_dma_contig;
 	bool is_shutdown;
+	bool is_suspend;
 };
 
 #define RKVPSS_ZME_TAP_COE(x, y) (((x) & 0x3ff) | (((y) & 0x3ff) << 16))
@@ -67,4 +69,6 @@ void rkvpss_hw_write(struct rkvpss_hw_dev *hw_dev, u32 reg, u32 val);
 u32 rkvpss_hw_read(struct rkvpss_hw_dev *hw_dev, u32 reg);
 void rkvpss_hw_set_bits(struct rkvpss_hw_dev *hw, u32 reg, u32 mask, u32 val);
 void rkvpss_hw_clear_bits(struct rkvpss_hw_dev *hw, u32 reg, u32 mask);
+void rkvpss_hw_reg_save(struct rkvpss_hw_dev *dev);
+void rkvpss_hw_reg_restore(struct rkvpss_hw_dev *dev);
 #endif

drivers/media/platform/rockchip/vpss/stream.c

@@ -779,8 +779,9 @@ static void rkvpss_buf_done_task(unsigned long arg)
 		buf = list_first_entry(&local_list, struct rkvpss_buffer, queue);
 		list_del(&buf->queue);
 		v4l2_dbg(2, rkvpss_debug, &stream->dev->v4l2_dev,
-			 "%s id:%d seq:%d buf:0x%x done\n",
-			 node->vdev.name, stream->id, buf->vb.sequence, buf->dma[0]);
+			 "%s stream:%d index:%d seq:%d buf:0x%x done\n",
+			 node->vdev.name, stream->id, buf->vb.vb2_buf.index,
+			 buf->vb.sequence, buf->dma[0]);
 		vb2_buffer_done(&buf->vb.vb2_buf,
 				stream->streaming ? VB2_BUF_STATE_DONE : VB2_BUF_STATE_ERROR);
 	}
@@ -794,6 +795,9 @@ static void rkvpss_stream_buf_done(struct rkvpss_stream *stream,
 	if (!stream || !buf)
 		return;
 
+	v4l2_dbg(3, rkvpss_debug, &stream->dev->v4l2_dev,
+		 "stream:%d\n", stream->id);
+
 	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
 	list_add_tail(&buf->queue, &stream->buf_done_list);
 	spin_unlock_irqrestore(&stream->vbq_lock, lock_flags);
@@ -807,6 +811,8 @@ static void rkvpss_frame_end(struct rkvpss_stream *stream)
 	struct rkvpss_buffer *buf = NULL;
 	unsigned long lock_flags = 0;
 
+	v4l2_dbg(3, rkvpss_debug, &dev->v4l2_dev,
+		 "stream:%d\n", stream->id);
 	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
 	if (stream->curr_buf) {
 		buf = stream->curr_buf;
@@ -906,8 +912,8 @@ static void rkvpss_buf_queue(struct vb2_buffer *vb)
 	}
 
 	v4l2_dbg(2, rkvpss_debug, &dev->v4l2_dev,
-		 "%s stream:%d buf:0x%x\n", __func__,
-		 stream->id, vpssbuf->dma[0]);
+		 "%s stream:%d index:%d buf:0x%x\n", __func__,
+		 stream->id, vb->index, vpssbuf->dma[0]);
 
 	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
 	list_add_tail(&vpssbuf->queue, &stream->buf_queue);

drivers/media/platform/rockchip/vpss/vpss.c

@@ -246,10 +246,11 @@ static int rkvpss_sof(struct rkvpss_subdev *sdev, struct rkisp_vpss_sof *info)
 	sdev->frame_seq = info->seq;
 	sdev->frame_timestamp = info->timestamp;
 	dev->unite_index = info->unite_index;
+	dev->is_idle = false;
 
 	v4l2_dbg(3, rkvpss_debug, &dev->v4l2_dev,
-		 "%s unite_mode:%u, unite_indev:%u\n", __func__,
-		 dev->unite_mode, dev->unite_index);
+		 "%s unite_mode:%u, unite_indev:%u seq:%d\n", __func__,
+		 dev->unite_mode, dev->unite_index, info->seq);
 
 	rkvpss_cmsc_config(dev, !info->irq);
 	for (i = 0; i < RKVPSS_OUTPUT_MAX; i++) {
@@ -418,6 +419,10 @@ void rkvpss_check_idle(struct rkvpss_device *dev, u32 irq)
 
 	dev->irq_ends = 0;
 	rkvpss_end_notify_isp(dev);
+	dev->is_idle = true;
+
+	if (dev->is_suspend)
+		complete(&dev->pm_suspend_wait_fe);
 }
 
 int rkvpss_register_subdev(struct rkvpss_device *dev,
@@ -451,6 +456,7 @@ int rkvpss_register_subdev(struct rkvpss_device *dev,
 	if (ret < 0)
 		goto free_media;
 	rkvpss_subdev_default_fmt(vpss_sdev);
+	init_completion(&dev->pm_suspend_wait_fe);
 	return ret;
 free_media:
 	media_entity_cleanup(&sd->entity);

drivers/media/platform/rockchip/vpss/vpss_offline.c

@@ -2069,11 +2069,16 @@ end:
 static long rkvpss_ofl_ioctl(struct file *file, void *fh,
 			     bool valid_prio, unsigned int cmd, void *arg)
 {
+	struct rkvpss_offline_dev *ofl = video_drvdata(file);
 	long ret = 0;
 	bool unite;
 
-	if (!arg)
-		return -EINVAL;
+	ofl->pm_need_wait = true;
+
+	if (!arg) {
+		ret = -EINVAL;
+		goto out;
+	}
 
 	switch (cmd) {
 	case RKVPSS_CMD_MODULE_SEL:
@@ -2098,6 +2103,12 @@ static long rkvpss_ofl_ioctl(struct file *file, void *fh,
 		ret = -EFAULT;
 	}
 
+out:
+	/* notify hw suspend */
+	if (ofl->hw->is_suspend)
+		complete(&ofl->pm_cmpl);
+
+	ofl->pm_need_wait = false;
 	return ret;
 }
 
@@ -2201,6 +2212,8 @@ int rkvpss_register_offline(struct rkvpss_hw_dev *hw)
 	INIT_LIST_HEAD(&ofl->cfginfo_list);
 	mutex_init(&ofl->ofl_lock);
 	rkvpss_offline_proc_init(ofl);
+	ofl->pm_need_wait = false;
+	init_completion(&ofl->pm_cmpl);
 	return 0;
 unreg_v4l2:
 	mutex_destroy(&ofl->apilock);

drivers/media/platform/rockchip/vpss/vpss_offline.h

@@ -73,8 +73,10 @@ struct rkvpss_offline_dev {
 	struct mutex ofl_lock;
 	struct rkvpss_dev_rate dev_rate[DEV_NUM_MAX];
 	struct rkvpss_unite_scl_params unite_params[RKVPSS_OUTPUT_MAX];
+	struct completion pm_cmpl;
 	u32 unite_right_enlarge;
 	bool mode_sel_en;
+	bool pm_need_wait;
 };
 
 int rkvpss_register_offline(struct rkvpss_hw_dev *hw);