video: rockchip: vpu: alloc & mmap iova inside drm allocator
authorJung Zhao <jung.zhao@rock-chips.com>
Tue, 21 Mar 2017 01:14:55 +0000 (09:14 +0800)
committerHuang, Tao <huangtao@rock-chips.com>
Fri, 24 Mar 2017 06:50:42 +0000 (14:50 +0800)
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
drivers/video/rockchip/vcodec/vcodec_service.c

index e8f2042..aa79a98 100644 (file)
@@ -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);
 
index 47531bd..55f39a4 100644 (file)
@@ -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;
 }