video: rockchip: vpu: alloc & mmap iova inside drm allocator

since on 3368 platform, hevc & vpu share the same hardware
resource, but have two independent mmu. and only one mmu can
be attached at the same time. I have to alloc and mmap iova
outside mmu attached status, otherwise it will cause mmu reset
error.

fix bugs:
1. hevc & vpu can work at the same time now. on 3368 platform,
vpu_mmu and hevc_mmu share the same irq with their respective
master device, we can not detach mmu during irq context.
2. fix copy_sgt offset and lenght invalide value

Change-Id: I9d02aa0b85a6d0690832c7869a260953f5a5baab
Signed-off-by: Jung Zhao <jung.zhao@rock-chips.com>

drivers/video/rockchip/vcodec/vcodec_iommu_drm.c

@@ -35,6 +35,7 @@
 #include <linux/kref.h>
 #include <linux/fdtable.h>
 #include <linux/ktime.h>
+#include <linux/iova.h>
 
 #include "vcodec_iommu_ops.h"
 
@@ -50,6 +51,7 @@ struct vcodec_drm_buffer {
 	int index;
 	struct dma_buf_attachment *attach;
 	struct sg_table *sgt;
+	struct sg_table *copy_sgt;
 	struct page **pages;
 	struct kref ref;
 	struct vcodec_iommu_session_info *session_info;
@@ -138,12 +140,6 @@ static int vcodec_drm_attach_unlock(struct vcodec_iommu_info *iommu_info)
 		return ret;
 	}
 
-	if (!common_iommu_setup_dma_ops(dev, 0x10000000, SZ_2G, domain->ops)) {
-		dev_err(dev, "Failed to set dma_ops\n");
-		iommu_detach_device(domain, dev);
-		ret = -ENODEV;
-	}
-
 	return ret;
 }
 
@@ -208,6 +204,164 @@ static void vcodec_drm_sgt_unmap_kernel(struct vcodec_drm_buffer *drm_buffer)
 	kfree(drm_buffer->pages);
 }
 
+static int vcodec_finalise_sg(struct scatterlist *sg,
+			      int nents,
+			      dma_addr_t dma_addr)
+{
+	struct scatterlist *s, *cur = sg;
+	unsigned long seg_mask = DMA_BIT_MASK(32);
+	unsigned int cur_len = 0, max_len = DMA_BIT_MASK(32);
+	int i, count = 0;
+
+	for_each_sg(sg, s, nents, i) {
+		/* Restore this segment's original unaligned fields first */
+		unsigned int s_iova_off = sg_dma_address(s);
+		unsigned int s_length = sg_dma_len(s);
+		unsigned int s_iova_len = s->length;
+
+		s->offset += s_iova_off;
+		s->length = s_length;
+		sg_dma_address(s) = DMA_ERROR_CODE;
+		sg_dma_len(s) = 0;
+
+		/*
+		 * Now fill in the real DMA data. If...
+		 * - there is a valid output segment to append to
+		 * - and this segment starts on an IOVA page boundary
+		 * - but doesn't fall at a segment boundary
+		 * - and wouldn't make the resulting output segment too long
+		 */
+		if (cur_len && !s_iova_off && (dma_addr & seg_mask) &&
+		    (cur_len + s_length <= max_len)) {
+			/* ...then concatenate it with the previous one */
+			cur_len += s_length;
+		} else {
+			/* Otherwise start the next output segment */
+			if (i > 0)
+				cur = sg_next(cur);
+			cur_len = s_length;
+			count++;
+
+			sg_dma_address(cur) = dma_addr + s_iova_off;
+		}
+
+		sg_dma_len(cur) = cur_len;
+		dma_addr += s_iova_len;
+
+		if (s_length + s_iova_off < s_iova_len)
+			cur_len = 0;
+	}
+	return count;
+}
+
+static void vcodec_invalidate_sg(struct scatterlist *sg, int nents)
+{
+	struct scatterlist *s;
+	int i;
+
+	for_each_sg(sg, s, nents, i) {
+		if (sg_dma_address(s) != DMA_ERROR_CODE)
+			s->offset += sg_dma_address(s);
+		if (sg_dma_len(s))
+			s->length = sg_dma_len(s);
+		sg_dma_address(s) = DMA_ERROR_CODE;
+		sg_dma_len(s) = 0;
+	}
+}
+
+static dma_addr_t vcodec_dma_map_sg(struct iommu_domain *domain,
+				    struct scatterlist *sg,
+				    int nents, int prot)
+{
+	struct iova_domain *iovad = domain->iova_cookie;
+	struct iova *iova;
+	struct scatterlist *s, *prev = NULL;
+	dma_addr_t dma_addr;
+	size_t iova_len = 0;
+	unsigned long mask = DMA_BIT_MASK(32);
+	unsigned long shift = iova_shift(iovad);
+	int i;
+
+	/*
+	 * Work out how much IOVA space we need, and align the segments to
+	 * IOVA granules for the IOMMU driver to handle. With some clever
+	 * trickery we can modify the list in-place, but reversibly, by
+	 * stashing the unaligned parts in the as-yet-unused DMA fields.
+	 */
+	for_each_sg(sg, s, nents, i) {
+		size_t s_iova_off = iova_offset(iovad, s->offset);
+		size_t s_length = s->length;
+		size_t pad_len = (mask - iova_len + 1) & mask;
+
+		sg_dma_address(s) = s_iova_off;
+		sg_dma_len(s) = s_length;
+		s->offset -= s_iova_off;
+		s_length = iova_align(iovad, s_length + s_iova_off);
+		s->length = s_length;
+
+		/*
+		 * Due to the alignment of our single IOVA allocation, we can
+		 * depend on these assumptions about the segment boundary mask:
+		 * - If mask size >= IOVA size, then the IOVA range cannot
+		 *   possibly fall across a boundary, so we don't care.
+		 * - If mask size < IOVA size, then the IOVA range must start
+		 *   exactly on a boundary, therefore we can lay things out
+		 *   based purely on segment lengths without needing to know
+		 *   the actual addresses beforehand.
+		 * - The mask must be a power of 2, so pad_len == 0 if
+		 *   iova_len == 0, thus we cannot dereference prev the first
+		 *   time through here (i.e. before it has a meaningful value).
+		 */
+		if (pad_len && pad_len < s_length - 1) {
+			prev->length += pad_len;
+			iova_len += pad_len;
+		}
+
+		iova_len += s_length;
+		prev = s;
+	}
+
+	iova = alloc_iova(iovad, iova_align(iovad, iova_len) >> shift,
+					  mask >> shift, true);
+	if (!iova)
+		goto out_restore_sg;
+
+	/*
+	 * We'll leave any physical concatenation to the IOMMU driver's
+	 * implementation - it knows better than we do.
+	 */
+	dma_addr = iova_dma_addr(iovad, iova);
+	if (iommu_map_sg(domain, dma_addr, sg, nents, prot) < iova_len)
+		goto out_free_iova;
+
+	return vcodec_finalise_sg(sg, nents, dma_addr);
+
+out_free_iova:
+	__free_iova(iovad, iova);
+out_restore_sg:
+	vcodec_invalidate_sg(sg, nents);
+	return 0;
+}
+
+static void vcodec_dma_unmap_sg(struct iommu_domain *domain,
+				dma_addr_t dma_addr)
+{
+	struct iova_domain *iovad = domain->iova_cookie;
+	unsigned long shift = iova_shift(iovad);
+	unsigned long pfn = dma_addr >> shift;
+	struct iova *iova = find_iova(iovad, pfn);
+	size_t size;
+
+	if (WARN_ON(!iova))
+		return;
+
+	size = iova_size(iova) << shift;
+	size -= iommu_unmap(domain, pfn << shift, size);
+	/* ...and if we can't, then something is horribly, horribly wrong */
+	WARN_ON(size > 0);
+	__free_iova(iovad, iova);
+}
+
 static void vcodec_drm_clear_map(struct kref *ref)
 {
 	struct vcodec_drm_buffer *drm_buffer =
@@ -216,15 +370,9 @@ static void vcodec_drm_clear_map(struct kref *ref)
 		drm_buffer->session_info;
 	struct vcodec_iommu_info *iommu_info = session_info->iommu_info;
 	struct vcodec_iommu_drm_info *drm_info = iommu_info->private;
-	struct device *dev = session_info->dev;
-	struct iommu_domain *domain = drm_info->domain;
 
 	mutex_lock(&iommu_info->iommu_mutex);
 	drm_info = session_info->iommu_info->private;
-	if (!drm_info->attached) {
-		if (vcodec_drm_attach_unlock(session_info->iommu_info))
-			dev_err(dev, "can't clea map, attach iommu failed.\n");
-	}
 
 	if (drm_buffer->cpu_addr) {
 		vcodec_drm_sgt_unmap_kernel(drm_buffer);
@@ -232,6 +380,9 @@ static void vcodec_drm_clear_map(struct kref *ref)
 	}
 
 	if (drm_buffer->attach) {
+		vcodec_dma_unmap_sg(drm_info->domain, drm_buffer->iova);
+		sg_free_table(drm_buffer->copy_sgt);
+		kfree(drm_buffer->copy_sgt);
 		dma_buf_unmap_attachment(drm_buffer->attach, drm_buffer->sgt,
 					 DMA_BIDIRECTIONAL);
 		dma_buf_detach(drm_buffer->dma_buf, drm_buffer->attach);
@@ -239,9 +390,6 @@ static void vcodec_drm_clear_map(struct kref *ref)
 		drm_buffer->attach = NULL;
 	}
 
-	if (!drm_info->attached)
-		iommu_detach_device(domain, dev);
-
 	mutex_unlock(&iommu_info->iommu_mutex);
 }
 
@@ -446,12 +594,13 @@ static int vcodec_drm_import(struct vcodec_iommu_session_info *session_info,
 	struct vcodec_drm_buffer *oldest_buffer = NULL, *loop_buffer = NULL;
 	struct vcodec_iommu_info *iommu_info = session_info->iommu_info;
 	struct vcodec_iommu_drm_info *drm_info = iommu_info->private;
-	struct iommu_domain *domain = drm_info->domain;
 	struct device *dev = session_info->dev;
 	struct dma_buf_attachment *attach;
 	struct sg_table *sgt;
 	struct dma_buf *dma_buf;
 	ktime_t oldest_time = ktime_set(0, 0);
+	struct scatterlist *sg, *s;
+	int i;
 	int ret = 0;
 
 	dma_buf = dma_buf_get(fd);
@@ -483,11 +632,6 @@ static int vcodec_drm_import(struct vcodec_iommu_session_info *session_info,
 
 	mutex_lock(&iommu_info->iommu_mutex);
 	drm_info = session_info->iommu_info->private;
-	if (!drm_info->attached) {
-		ret = vcodec_drm_attach_unlock(session_info->iommu_info);
-		if (ret)
-			goto fail_out;
-	}
 
 	attach = dma_buf_attach(drm_buffer->dma_buf, dev);
 	if (IS_ERR(attach)) {
@@ -503,15 +647,39 @@ static int vcodec_drm_import(struct vcodec_iommu_session_info *session_info,
 		goto fail_detach;
 	}
 
-	drm_buffer->iova = sg_dma_address(sgt->sgl);
+	/*
+	 * Since we call dma_buf_map_attachment outside attach/detach, this
+	 * will cause incorrectly map. we have to re-build map table native
+	 * and for avoiding destroy their origin map table, we need use a
+	 * copy one sg_table.
+	 */
+	drm_buffer->copy_sgt = kmalloc(sizeof(*drm_buffer->copy_sgt),
+				       GFP_KERNEL);
+	if (!drm_buffer->copy_sgt) {
+		ret = -ENOMEM;
+		goto fail_detach;
+	}
+
+	ret = sg_alloc_table(drm_buffer->copy_sgt, sgt->nents, GFP_KERNEL);
+	s = drm_buffer->copy_sgt->sgl;
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		sg_set_page(s, sg_page(sg),
+			    PAGE_SIZE << compound_order(sg_page(sg)), 0);
+		sg_dma_address(s) = page_to_phys(sg_page(sg));
+		s->offset = sg->offset;
+		s->length = sg->length;
+		s = sg_next(s);
+	}
+
+	vcodec_dma_map_sg(drm_info->domain, drm_buffer->copy_sgt->sgl,
+			  drm_buffer->copy_sgt->nents,
+			  IOMMU_READ | IOMMU_WRITE);
+	drm_buffer->iova = sg_dma_address(drm_buffer->copy_sgt->sgl);
 	drm_buffer->size = drm_buffer->dma_buf->size;
 
 	drm_buffer->attach = attach;
 	drm_buffer->sgt = sgt;
 
-	if (!drm_info->attached)
-		iommu_detach_device(domain, dev);
-
 	mutex_unlock(&iommu_info->iommu_mutex);
 
 	INIT_LIST_HEAD(&drm_buffer->list);
@@ -558,6 +726,7 @@ fail_out:
 static int vcodec_drm_create(struct vcodec_iommu_info *iommu_info)
 {
 	struct vcodec_iommu_drm_info *drm_info;
+	struct iommu_group *group;
 	int ret;
 
 	iommu_info->private = kzalloc(sizeof(*drm_info),
@@ -575,10 +744,28 @@ static int vcodec_drm_create(struct vcodec_iommu_info *iommu_info)
 	if (ret)
 		goto err_free_domain;
 
-	vcodec_drm_attach(iommu_info);
+	group = iommu_group_get(iommu_info->dev);
+	if (!group) {
+		group = iommu_group_alloc();
+		if (IS_ERR(group)) {
+			dev_err(iommu_info->dev,
+				"Failed to allocate IOMMU group\n");
+			goto err_put_cookie;
+		}
+		ret = iommu_group_add_device(group, iommu_info->dev);
+		if (ret) {
+			dev_err(iommu_info->dev,
+				"failed to add device to IOMMU group\n");
+			goto err_put_cookie;
+		}
+	}
+	iommu_dma_init_domain(drm_info->domain, 0x10000000, SZ_2G);
+	iommu_group_put(group);
 
 	return 0;
 
+err_put_cookie:
+	iommu_put_dma_cookie(drm_info->domain);
 err_free_domain:
 	iommu_domain_free(drm_info->domain);
 
@@ -589,7 +776,6 @@ static int vcodec_drm_destroy(struct vcodec_iommu_info *iommu_info)
 {
 	struct vcodec_iommu_drm_info *drm_info = iommu_info->private;
 
-	vcodec_drm_detach(iommu_info);
 	iommu_put_dma_cookie(drm_info->domain);
 	iommu_domain_free(drm_info->domain);
 

drivers/video/rockchip/vcodec/vcodec_service.c

@@ -354,11 +354,14 @@ struct vpu_subdev_data {
 
 	struct device *mmu_dev;
 	struct vcodec_iommu_info *iommu_info;
+	struct work_struct set_work;
 };
 
 struct vpu_service_info {
 	struct wake_lock wake_lock;
 	struct delayed_work power_off_work;
+	struct wake_lock set_wake_lock;
+	struct workqueue_struct *set_workq;
 	ktime_t last; /* record previous power-on time */
 	/* vpu service structure global lock */
 	struct mutex lock;
@@ -477,8 +480,23 @@ static void vcodec_enter_mode(struct vpu_subdev_data *data)
 	struct vpu_service_info *pservice = data->pservice;
 	struct vpu_subdev_data *subdata, *n;
 
-	if (pservice->subcnt < 2)
+	if (pservice->subcnt < 2) {
+		if (data->mmu_dev && !test_bit(MMU_ACTIVATED, &data->state)) {
+			set_bit(MMU_ACTIVATED, &data->state);
+
+			if (atomic_read(&pservice->enabled)) {
+				if (vcodec_iommu_attach(data->iommu_info))
+					dev_err(data->dev,
+						"vcodec service attach failed\n"
+						);
+				else
+					BUG_ON(
+					       !atomic_read(&pservice->enabled)
+					       );
+			}
+		}
 		return;
+	}
 
 	if (pservice->curr_mode == data->mode)
 		return;
@@ -489,6 +507,7 @@ static void vcodec_enter_mode(struct vpu_subdev_data *data)
 		if (data != subdata && subdata->mmu_dev &&
 		    test_bit(MMU_ACTIVATED, &subdata->state)) {
 			clear_bit(MMU_ACTIVATED, &subdata->state);
+			vcodec_iommu_detach(subdata->iommu_info);
 		}
 	}
 	bits = 1 << pservice->mode_bit;
@@ -534,7 +553,9 @@ static void vcodec_enter_mode(struct vpu_subdev_data *data)
 #endif
 	if (data->mmu_dev && !test_bit(MMU_ACTIVATED, &data->state)) {
 		set_bit(MMU_ACTIVATED, &data->state);
-		if (!atomic_read(&pservice->enabled))
+		if (atomic_read(&pservice->enabled))
+			vcodec_iommu_attach(data->iommu_info);
+		else
 			/* FIXME BUG_ON should not be used in mass produce */
 			BUG_ON(!atomic_read(&pservice->enabled));
 	}
@@ -664,7 +685,6 @@ static void vpu_reset(struct vpu_subdev_data *data)
 		if (atomic_read(&pservice->enabled)) {
 			/* Need to reset iommu */
 			vcodec_iommu_detach(data->iommu_info);
-			vcodec_iommu_attach(data->iommu_info);
 		} else {
 			/* FIXME BUG_ON should not be used in mass produce */
 			BUG_ON(!atomic_read(&pservice->enabled));
@@ -799,13 +819,8 @@ static void vpu_service_power_on(struct vpu_subdev_data *data,
 		pservice->last = now;
 	}
 	ret = atomic_add_unless(&pservice->enabled, 1, 1);
-	if (!ret) {
-		if (data->mmu_dev && !test_bit(MMU_ACTIVATED, &data->state)) {
-			set_bit(MMU_ACTIVATED, &data->state);
-			vcodec_iommu_attach(data->iommu_info);
-		}
+	if (!ret)
 		return;
-	}
 
 	dev_dbg(pservice->dev, "power on\n");
 
@@ -829,18 +844,6 @@ static void vpu_service_power_on(struct vpu_subdev_data *data,
 #endif
 	pm_runtime_get_sync(pservice->dev);
 
-	if (data->mmu_dev && !test_bit(MMU_ACTIVATED, &data->state)) {
-		set_bit(MMU_ACTIVATED, &data->state);
-		if (atomic_read(&pservice->enabled))
-			vcodec_iommu_attach(data->iommu_info);
-		else
-			/*
-			 * FIXME BUG_ON should not be used in mass
-			 * produce.
-			 */
-			BUG_ON(!atomic_read(&pservice->enabled));
-	}
-
 	udelay(5);
 	atomic_add(1, &pservice->power_on_cnt);
 	wake_lock(&pservice->wake_lock);
@@ -1075,8 +1078,6 @@ static int vcodec_bufid_to_iova(struct vpu_subdev_data *data,
 			if (pps_info_count) {
 				u8 *pps;
 
-				mutex_lock(&pservice->lock);
-
 				pps = vcodec_iommu_map_kernel
 					(data->iommu_info, session, hdl);
 
@@ -1091,7 +1092,6 @@ static int vcodec_bufid_to_iova(struct vpu_subdev_data *data,
 
 				vcodec_iommu_unmap_kernel
 					(data->iommu_info, session, hdl);
-				mutex_unlock(&pservice->lock);
 			}
 		}
 
@@ -1628,6 +1628,8 @@ static void try_set_reg(struct vpu_subdev_data *data)
 		struct vpu_reg *reg = list_entry(pservice->waiting.next,
 				struct vpu_reg, status_link);
 
+		vpu_service_power_on(data, pservice);
+
 		if (change_able || !reset_request) {
 			switch (reg->type) {
 			case VPU_ENC: {
@@ -1688,6 +1690,18 @@ static void try_set_reg(struct vpu_subdev_data *data)
 	vpu_debug_leave();
 }
 
+static void vpu_set_register_work(struct work_struct *work_s)
+{
+	struct vpu_subdev_data *data = container_of(work_s,
+						    struct vpu_subdev_data,
+						    set_work);
+	struct vpu_service_info *pservice = data->pservice;
+
+	mutex_lock(&pservice->lock);
+	try_set_reg(data);
+	mutex_unlock(&pservice->lock);
+}
+
 static int return_reg(struct vpu_subdev_data *data,
 		      struct vpu_reg *reg, u32 __user *dst)
 {
@@ -1772,8 +1786,6 @@ static long vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		struct vpu_request req;
 		struct vpu_reg *reg;
 
-		vpu_service_power_on(data, pservice);
-
 		vpu_debug(DEBUG_IOCTL, "pid %d set reg type %d\n",
 			  session->pid, session->type);
 		if (copy_from_user(&req, (void __user *)arg,
@@ -1786,9 +1798,7 @@ static long vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		if (NULL == reg) {
 			return -EFAULT;
 		} else {
-			mutex_lock(&pservice->lock);
-			try_set_reg(data);
-			mutex_unlock(&pservice->lock);
+			queue_work(pservice->set_workq, &data->set_work);
 		}
 	} break;
 	case VPU_IOC_GET_REG: {
@@ -1796,8 +1806,6 @@ static long vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		struct vpu_reg *reg;
 		int ret;
 
-		vpu_service_power_on(data, pservice);
-
 		vpu_debug(DEBUG_IOCTL, "pid %d get reg type %d\n",
 			  session->pid, session->type);
 		if (copy_from_user(&req, (void __user *)arg,
@@ -1844,9 +1852,9 @@ static long vpu_service_ioctl(struct file *filp, unsigned int cmd,
 			}
 			vpu_service_session_clear(data, session);
 			mutex_unlock(&pservice->lock);
+
 			return ret;
 		}
-
 		mutex_lock(&pservice->lock);
 		reg = list_entry(session->done.next,
 				 struct vpu_reg, session_link);
@@ -1923,8 +1931,6 @@ static long compat_vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		struct compat_vpu_request req;
 		struct vpu_reg *reg;
 
-		vpu_service_power_on(data, pservice);
-
 		vpu_debug(DEBUG_IOCTL, "compat set reg type %d\n",
 			  session->type);
 		if (copy_from_user(&req, compat_ptr((compat_uptr_t)arg),
@@ -1937,9 +1943,7 @@ static long compat_vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		if (NULL == reg) {
 			return -EFAULT;
 		} else {
-			mutex_lock(&pservice->lock);
-			try_set_reg(data);
-			mutex_unlock(&pservice->lock);
+			queue_work(pservice->set_workq, &data->set_work);
 		}
 	} break;
 	case COMPAT_VPU_IOC_GET_REG: {
@@ -1947,8 +1951,6 @@ static long compat_vpu_service_ioctl(struct file *filp, unsigned int cmd,
 		struct vpu_reg *reg;
 		int ret;
 
-		vpu_service_power_on(data, pservice);
-
 		vpu_debug(DEBUG_IOCTL, "compat get reg type %d\n",
 			  session->type);
 		if (copy_from_user(&req, compat_ptr((compat_uptr_t)arg),
@@ -2105,7 +2107,6 @@ static int vpu_service_release(struct inode *inode, struct file *filp)
 	}
 	wake_up(&session->wait);
 
-	vpu_service_power_on(data, pservice);
 	mutex_lock(&pservice->lock);
 	/* remove this filp from the asynchronusly notified filp's */
 	list_del_init(&session->list_session);
@@ -2266,6 +2267,8 @@ static int vcodec_subdev_probe(struct platform_device *pdev,
 
 	data->pservice = pservice;
 	data->dev = dev;
+
+	INIT_WORK(&data->set_work, vpu_set_register_work);
 	of_property_read_u32(np, "dev_mode", (u32 *)&data->mode);
 
 	if (pservice->reg_base == 0) {
@@ -2319,12 +2322,18 @@ static int vcodec_subdev_probe(struct platform_device *pdev,
 	clear_bit(MMU_ACTIVATED, &data->state);
 	vpu_service_power_on(data, pservice);
 
+	of_property_read_u32(np, "allocator", (u32 *)&pservice->alloc_type);
+	data->iommu_info = vcodec_iommu_info_create(dev, data->mmu_dev,
+						    pservice->alloc_type);
+	dev_info(dev, "allocator is %s\n", pservice->alloc_type == 1 ? "drm" :
+		(pservice->alloc_type == 2 ? "ion" : "null"));
 	vcodec_enter_mode(data);
 	ret = vpu_service_check_hw(data);
 	if (ret < 0) {
 		vpu_err("error: hw info check faild\n");
 		goto err;
 	}
+	vcodec_exit_mode(data);
 
 	hw_info = data->hw_info;
 	regs = (u8 *)data->regs;
@@ -2370,15 +2379,9 @@ static int vcodec_subdev_probe(struct platform_device *pdev,
 	atomic_set(&data->enc_dev.irq_count_codec, 0);
 	atomic_set(&data->enc_dev.irq_count_pp, 0);
 
-	of_property_read_u32(np, "allocator", (u32 *)&pservice->alloc_type);
-	data->iommu_info = vcodec_iommu_info_create(dev, data->mmu_dev,
-						    pservice->alloc_type);
-	dev_info(dev, "allocator is %s\n", pservice->alloc_type == 1 ? "drm" :
-		(pservice->alloc_type == 2 ? "ion" : "null"));
 	get_hw_info(data);
 	pservice->auto_freq = true;
 
-	vcodec_exit_mode(data);
 	/* create device node */
 	ret = alloc_chrdev_region(&data->dev_t, 0, 1, name);
 	if (ret) {
@@ -2553,6 +2556,12 @@ static int vcodec_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	pservice->dev = dev;
 
+	pservice->set_workq = create_singlethread_workqueue("vcodec");
+	if (!pservice->set_workq) {
+		dev_err(dev, "failed to create workqueue\n");
+		return -ENOMEM;
+	}
+
 	driver_data = vcodec_get_drv_data(pdev);
 	if (!driver_data)
 		return -EINVAL;
@@ -2630,6 +2639,7 @@ static int vcodec_probe(struct platform_device *pdev)
 err:
 	dev_info(dev, "init failed\n");
 	vpu_service_power_off(pservice);
+	destroy_workqueue(pservice->set_workq);
 	wake_lock_destroy(&pservice->wake_lock);
 
 	return ret;
@@ -2668,7 +2678,6 @@ static void vcodec_shutdown(struct platform_device *pdev)
 		dev_err(&pdev->dev, "wait total running time out\n");
 
 	vcodec_exit_mode(data);
-
 	vpu_service_clear(data);
 	if (of_property_read_bool(np, "subcnt")) {
 		for (i = 0; i < pservice->subcnt; i++) {
@@ -2677,7 +2686,6 @@ static void vcodec_shutdown(struct platform_device *pdev)
 
 			sub_np = of_parse_phandle(np, "rockchip,sub", i);
 			sub_pdev = of_find_device_by_node(sub_np);
-
 			vcodec_subdev_remove(platform_get_drvdata(sub_pdev));
 		}
 
@@ -2903,7 +2911,8 @@ static irqreturn_t vdpu_isr(int irq, void *dev_id)
 		else
 			reg_from_run_to_done(data, pservice->reg_pproc);
 	}
-	try_set_reg(data);
+
+	queue_work(pservice->set_workq, &data->set_work);
 	mutex_unlock(&pservice->lock);
 	return IRQ_HANDLED;
 }
@@ -2961,8 +2970,9 @@ static irqreturn_t vepu_isr(int irq, void *dev_id)
 		else
 			reg_from_run_to_done(data, pservice->reg_codec);
 	}
-	try_set_reg(data);
+	queue_work(pservice->set_workq, &data->set_work);
 	mutex_unlock(&pservice->lock);
+
 	return IRQ_HANDLED;
 }