From 23bbd27fb4d191c05f3112e85ecd0838487baa7b Mon Sep 17 00:00:00 2001 From: Cai YiWei Date: Tue, 12 Jan 2021 15:37:26 +0800 Subject: [PATCH] media: rockchip: isp/ispp add vb2_rdma_sg_memops to support contiguous page Change-Id: Ic33bfe6fe5d6e095fc1943fd2933376c42360f17 Signed-off-by: Cai YiWei --- drivers/media/platform/rockchip/isp/Makefile | 3 +- drivers/media/platform/rockchip/isp/hw.c | 10 +- .../media/platform/rockchip/isp/isp_ispp.h | 3 + .../platform/rockchip/isp/videobuf2-rdma-sg.c | 620 ++++++++++++++++++ drivers/media/platform/rockchip/ispp/hw.c | 10 +- 5 files changed, 637 insertions(+), 9 deletions(-) create mode 100644 drivers/media/platform/rockchip/isp/videobuf2-rdma-sg.c diff --git a/drivers/media/platform/rockchip/isp/Makefile b/drivers/media/platform/rockchip/isp/Makefile index c184386a3c4b..e49e73b2cc32 100644 --- a/drivers/media/platform/rockchip/isp/Makefile +++ b/drivers/media/platform/rockchip/isp/Makefile @@ -22,7 +22,8 @@ video_rkisp-objs += rkisp.o \ csi.o \ bridge.o \ isp_mipi_luma.o \ - procfs.o + procfs.o \ + videobuf2-rdma-sg.o ifdef CONFIG_VIDEO_ROCKCHIP_THUNDER_BOOT_ISP video_rkisp-objs += rkisp_tb_helper.o diff --git a/drivers/media/platform/rockchip/isp/hw.c b/drivers/media/platform/rockchip/isp/hw.c index b2378f663522..f10aed42e71b 100644 --- a/drivers/media/platform/rockchip/isp/hw.c +++ b/drivers/media/platform/rockchip/isp/hw.c @@ -718,10 +718,6 @@ static int rkisp_hw_probe(struct platform_device *pdev) hw_dev->is_dma_contig = true; hw_dev->is_buf_init = false; hw_dev->is_mmu = is_iommu_enable(dev); - if (!hw_dev->is_mmu) - hw_dev->mem_ops = &vb2_dma_contig_memops; - else - hw_dev->mem_ops = &vb2_dma_sg_memops; ret = of_reserved_mem_device_init(dev); if (ret) { if (!hw_dev->is_mmu) @@ -729,6 +725,12 @@ static int rkisp_hw_probe(struct platform_device *pdev) else hw_dev->is_dma_contig = false; } + if (!hw_dev->is_mmu) + hw_dev->mem_ops = &vb2_dma_contig_memops; + else if (!hw_dev->is_dma_contig) + hw_dev->mem_ops = &vb2_dma_sg_memops; + else + hw_dev->mem_ops = &vb2_rdma_sg_memops; pm_runtime_enable(dev); diff --git a/drivers/media/platform/rockchip/isp/isp_ispp.h b/drivers/media/platform/rockchip/isp/isp_ispp.h index c6b152a9acf0..ccbd10ebcef7 100644 --- a/drivers/media/platform/rockchip/isp/isp_ispp.h +++ b/drivers/media/platform/rockchip/isp/isp_ispp.h @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -126,4 +127,6 @@ static inline void rkisp_get_bridge_sd(struct platform_device *dev, } #endif +extern const struct vb2_mem_ops vb2_rdma_sg_memops; + #endif diff --git a/drivers/media/platform/rockchip/isp/videobuf2-rdma-sg.c b/drivers/media/platform/rockchip/isp/videobuf2-rdma-sg.c new file mode 100644 index 000000000000..3586f173df5d --- /dev/null +++ b/drivers/media/platform/rockchip/isp/videobuf2-rdma-sg.c @@ -0,0 +1,620 @@ +// SPDX-License-Identifier: GPL-2.0 +/* Copyright (C) 2020 Rockchip Electronics Co., Ltd */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +struct vb2_dma_sg_buf { + struct device *dev; + void *vaddr; + struct page **pages; + struct frame_vector *vec; + int offset; + unsigned long dma_attrs; + enum dma_data_direction dma_dir; + struct sg_table sg_table; + /* + * This will point to sg_table when used with the MMAP or USERPTR + * memory model, and to the dma_buf sglist when used with the + * DMABUF memory model. + */ + struct sg_table *dma_sgt; + size_t size; + unsigned int num_pages; + refcount_t refcount; + struct vb2_vmarea_handler handler; + + struct dma_buf_attachment *db_attach; +}; + +static void vb2_dma_sg_put(void *buf_priv); + +static int vb2_dma_sg_alloc_contiguous(struct device *dev, + struct vb2_dma_sg_buf *buf, gfp_t gfp_flags) +{ + struct page *page = NULL; + unsigned int i, align = get_order(buf->size); + + /* cma reserved area */ + if (align > CONFIG_CMA_ALIGNMENT) + align = CONFIG_CMA_ALIGNMENT; + if (dev && dev->cma_area) + page = cma_alloc(dev->cma_area, buf->num_pages, + align, gfp_flags & __GFP_NOWARN); + if (!page) + return -ENOMEM; + + for (i = 0; i < buf->num_pages; i++) + buf->pages[i] = page + i; + + return 0; +} + +static void *vb2_dma_sg_alloc(struct device *dev, unsigned long dma_attrs, + unsigned long size, enum dma_data_direction dma_dir, + gfp_t gfp_flags) +{ + struct vb2_dma_sg_buf *buf; + struct sg_table *sgt; + int ret; + int num_pages; + + if (WARN_ON(!dev)) + return ERR_PTR(-EINVAL); + + buf = kzalloc(sizeof(*buf), GFP_KERNEL); + if (!buf) + return ERR_PTR(-ENOMEM); + + buf->vaddr = NULL; + buf->dma_attrs = dma_attrs; + buf->dma_dir = dma_dir; + buf->offset = 0; + buf->size = size; + /* size is already page aligned */ + buf->num_pages = size >> PAGE_SHIFT; + buf->dma_sgt = &buf->sg_table; + + buf->pages = kvmalloc_array(buf->num_pages, sizeof(struct page *), + GFP_KERNEL | __GFP_ZERO); + if (!buf->pages) + goto fail_pages_array_alloc; + + ret = vb2_dma_sg_alloc_contiguous(dev, buf, gfp_flags); + if (ret) + goto fail_pages_alloc; + + ret = sg_alloc_table_from_pages(buf->dma_sgt, buf->pages, + buf->num_pages, 0, size, GFP_KERNEL); + if (ret) + goto fail_table_alloc; + + /* Prevent the device from being released while the buffer is used */ + buf->dev = get_device(dev); + + sgt = &buf->sg_table; + /* + * No need to sync to the device, this will happen later when the + * prepare() memop is called. + */ + sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, + buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC); + if (!sgt->nents) + goto fail_map; + + buf->handler.refcount = &buf->refcount; + buf->handler.put = vb2_dma_sg_put; + buf->handler.arg = buf; + + refcount_set(&buf->refcount, 1); + + return buf; + +fail_map: + put_device(buf->dev); + sg_free_table(buf->dma_sgt); +fail_table_alloc: + num_pages = buf->num_pages; + cma_release(dev->cma_area, buf->pages[0], num_pages); +fail_pages_alloc: + kvfree(buf->pages); +fail_pages_array_alloc: + kfree(buf); + return ERR_PTR(-ENOMEM); +} + +static void vb2_dma_sg_put(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + struct sg_table *sgt = &buf->sg_table; + int i = buf->num_pages; + + if (refcount_dec_and_test(&buf->refcount)) { + dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, + buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC); + if (buf->vaddr) + vm_unmap_ram(buf->vaddr, buf->num_pages); + sg_free_table(buf->dma_sgt); + cma_release(buf->dev->cma_area, buf->pages[0], i); + kvfree(buf->pages); + put_device(buf->dev); + kfree(buf); + } +} + +static void vb2_dma_sg_prepare(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + struct sg_table *sgt = buf->dma_sgt; + + /* DMABUF exporter will flush the cache for us */ + if (buf->db_attach) + return; + + dma_sync_sg_for_device(buf->dev, sgt->sgl, sgt->orig_nents, + buf->dma_dir); +} + +static void vb2_dma_sg_finish(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + struct sg_table *sgt = buf->dma_sgt; + + /* DMABUF exporter will flush the cache for us */ + if (buf->db_attach) + return; + + dma_sync_sg_for_cpu(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir); +} + +static void *vb2_dma_sg_get_userptr(struct device *dev, unsigned long vaddr, + unsigned long size, + enum dma_data_direction dma_dir) +{ + struct vb2_dma_sg_buf *buf; + struct sg_table *sgt; + struct frame_vector *vec; + + if (WARN_ON(!dev)) + return ERR_PTR(-EINVAL); + + buf = kzalloc(sizeof(*buf), GFP_KERNEL); + if (!buf) + return ERR_PTR(-ENOMEM); + + buf->vaddr = NULL; + buf->dev = dev; + buf->dma_dir = dma_dir; + buf->offset = vaddr & ~PAGE_MASK; + buf->size = size; + buf->dma_sgt = &buf->sg_table; + vec = vb2_create_framevec(vaddr, size, dma_dir == DMA_FROM_DEVICE || + dma_dir == DMA_BIDIRECTIONAL); + if (IS_ERR(vec)) + goto userptr_fail_pfnvec; + buf->vec = vec; + + buf->pages = frame_vector_pages(vec); + if (IS_ERR(buf->pages)) + goto userptr_fail_sgtable; + buf->num_pages = frame_vector_count(vec); + + if (sg_alloc_table_from_pages(buf->dma_sgt, buf->pages, + buf->num_pages, buf->offset, size, 0)) + goto userptr_fail_sgtable; + + sgt = &buf->sg_table; + /* + * No need to sync to the device, this will happen later when the + * prepare() memop is called. + */ + sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, + buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC); + if (!sgt->nents) + goto userptr_fail_map; + + return buf; + +userptr_fail_map: + sg_free_table(&buf->sg_table); +userptr_fail_sgtable: + vb2_destroy_framevec(vec); +userptr_fail_pfnvec: + kfree(buf); + return ERR_PTR(-ENOMEM); +} + +/* + * @put_userptr: inform the allocator that a USERPTR buffer will no longer + * be used + */ +static void vb2_dma_sg_put_userptr(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + struct sg_table *sgt = &buf->sg_table; + int i = buf->num_pages; + + dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir, + DMA_ATTR_SKIP_CPU_SYNC); + if (buf->vaddr) + vm_unmap_ram(buf->vaddr, buf->num_pages); + sg_free_table(buf->dma_sgt); + if (buf->dma_dir == DMA_FROM_DEVICE || + buf->dma_dir == DMA_BIDIRECTIONAL) + while (--i >= 0) + set_page_dirty_lock(buf->pages[i]); + vb2_destroy_framevec(buf->vec); + kfree(buf); +} + +static void *vb2_dma_sg_vaddr(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + + WARN_ON(!buf); + + if (!buf->vaddr) { + if (buf->db_attach) + buf->vaddr = dma_buf_vmap(buf->db_attach->dmabuf); + else + buf->vaddr = vm_map_ram(buf->pages, + buf->num_pages, -1, PAGE_KERNEL); + } + + /* add offset in case userptr is not page-aligned */ + return buf->vaddr ? buf->vaddr + buf->offset : NULL; +} + +static unsigned int vb2_dma_sg_num_users(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + + return refcount_read(&buf->refcount); +} + +static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + unsigned long uaddr = vma->vm_start; + unsigned long usize = vma->vm_end - vma->vm_start; + int i = 0; + + if (!buf) { + pr_err("No memory to map\n"); + return -EINVAL; + } + + do { + int ret; + + ret = vm_insert_page(vma, uaddr, buf->pages[i++]); + if (ret) { + pr_err("Remapping memory, error: %d\n", ret); + return ret; + } + + uaddr += PAGE_SIZE; + usize -= PAGE_SIZE; + } while (usize > 0); + + + /* + * Use common vm_area operations to track buffer refcount. + */ + vma->vm_private_data = &buf->handler; + vma->vm_ops = &vb2_common_vm_ops; + + vma->vm_ops->open(vma); + + return 0; +} + +/*********************************************/ +/* DMABUF ops for exporters */ +/*********************************************/ + +struct vb2_dma_sg_attachment { + struct sg_table sgt; + enum dma_data_direction dma_dir; +}; + +static int vb2_dma_sg_dmabuf_ops_attach(struct dma_buf *dbuf, + struct dma_buf_attachment *dbuf_attach) +{ + struct vb2_dma_sg_attachment *attach; + unsigned int i; + struct scatterlist *rd, *wr; + struct sg_table *sgt; + struct vb2_dma_sg_buf *buf = dbuf->priv; + int ret; + + attach = kzalloc(sizeof(*attach), GFP_KERNEL); + if (!attach) + return -ENOMEM; + + sgt = &attach->sgt; + /* Copy the buf->base_sgt scatter list to the attachment, as we can't + * map the same scatter list to multiple attachments at the same time. + */ + ret = sg_alloc_table(sgt, buf->dma_sgt->orig_nents, GFP_KERNEL); + if (ret) { + kfree(attach); + return -ENOMEM; + } + + rd = buf->dma_sgt->sgl; + wr = sgt->sgl; + for (i = 0; i < sgt->orig_nents; ++i) { + sg_set_page(wr, sg_page(rd), rd->length, rd->offset); + rd = sg_next(rd); + wr = sg_next(wr); + } + + attach->dma_dir = DMA_NONE; + dbuf_attach->priv = attach; + + return 0; +} + +static void vb2_dma_sg_dmabuf_ops_detach(struct dma_buf *dbuf, + struct dma_buf_attachment *db_attach) +{ + struct vb2_dma_sg_attachment *attach = db_attach->priv; + struct sg_table *sgt; + + if (!attach) + return; + + sgt = &attach->sgt; + + /* release the scatterlist cache */ + if (attach->dma_dir != DMA_NONE) + dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, + attach->dma_dir); + sg_free_table(sgt); + kfree(attach); + db_attach->priv = NULL; +} + +static struct sg_table *vb2_dma_sg_dmabuf_ops_map( + struct dma_buf_attachment *db_attach, enum dma_data_direction dma_dir) +{ + struct vb2_dma_sg_attachment *attach = db_attach->priv; + /* stealing dmabuf mutex to serialize map/unmap operations */ + struct mutex *lock = &db_attach->dmabuf->lock; + struct sg_table *sgt; + + mutex_lock(lock); + + sgt = &attach->sgt; + /* return previously mapped sg table */ + if (attach->dma_dir == dma_dir) { + mutex_unlock(lock); + return sgt; + } + + /* release any previous cache */ + if (attach->dma_dir != DMA_NONE) { + dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, + attach->dma_dir); + attach->dma_dir = DMA_NONE; + } + + /* mapping to the client with new direction */ + sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, + dma_dir); + if (!sgt->nents) { + pr_err("failed to map scatterlist\n"); + mutex_unlock(lock); + return ERR_PTR(-EIO); + } + + attach->dma_dir = dma_dir; + + mutex_unlock(lock); + + return sgt; +} + +static void vb2_dma_sg_dmabuf_ops_unmap(struct dma_buf_attachment *db_attach, + struct sg_table *sgt, enum dma_data_direction dma_dir) +{ + /* nothing to be done here */ +} + +static void vb2_dma_sg_dmabuf_ops_release(struct dma_buf *dbuf) +{ + /* drop reference obtained in vb2_dma_sg_get_dmabuf */ + vb2_dma_sg_put(dbuf->priv); +} + +static void *vb2_dma_sg_dmabuf_ops_kmap(struct dma_buf *dbuf, unsigned long pgnum) +{ + struct vb2_dma_sg_buf *buf = dbuf->priv; + + return buf->vaddr ? buf->vaddr + pgnum * PAGE_SIZE : NULL; +} + +static void *vb2_dma_sg_dmabuf_ops_vmap(struct dma_buf *dbuf) +{ + struct vb2_dma_sg_buf *buf = dbuf->priv; + + return vb2_dma_sg_vaddr(buf); +} + +static int vb2_dma_sg_dmabuf_ops_mmap(struct dma_buf *dbuf, + struct vm_area_struct *vma) +{ + return vb2_dma_sg_mmap(dbuf->priv, vma); +} + +static const struct dma_buf_ops vb2_dma_sg_dmabuf_ops = { + .attach = vb2_dma_sg_dmabuf_ops_attach, + .detach = vb2_dma_sg_dmabuf_ops_detach, + .map_dma_buf = vb2_dma_sg_dmabuf_ops_map, + .unmap_dma_buf = vb2_dma_sg_dmabuf_ops_unmap, + .map = vb2_dma_sg_dmabuf_ops_kmap, + .vmap = vb2_dma_sg_dmabuf_ops_vmap, + .mmap = vb2_dma_sg_dmabuf_ops_mmap, + .release = vb2_dma_sg_dmabuf_ops_release, +}; + +static struct dma_buf *vb2_dma_sg_get_dmabuf(void *buf_priv, unsigned long flags) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + struct dma_buf *dbuf; + DEFINE_DMA_BUF_EXPORT_INFO(exp_info); + + exp_info.ops = &vb2_dma_sg_dmabuf_ops; + exp_info.size = buf->size; + exp_info.flags = flags; + exp_info.priv = buf; + + if (WARN_ON(!buf->dma_sgt)) + return NULL; + + dbuf = dma_buf_export(&exp_info); + if (IS_ERR(dbuf)) + return NULL; + + /* dmabuf keeps reference to vb2 buffer */ + refcount_inc(&buf->refcount); + + return dbuf; +} + +/*********************************************/ +/* callbacks for DMABUF buffers */ +/*********************************************/ + +static int vb2_dma_sg_map_dmabuf(void *mem_priv) +{ + struct vb2_dma_sg_buf *buf = mem_priv; + struct sg_table *sgt; + + if (WARN_ON(!buf->db_attach)) { + pr_err("trying to pin a non attached buffer\n"); + return -EINVAL; + } + + if (WARN_ON(buf->dma_sgt)) { + pr_err("dmabuf buffer is already pinned\n"); + return 0; + } + + /* get the associated scatterlist for this buffer */ + sgt = dma_buf_map_attachment(buf->db_attach, buf->dma_dir); + if (IS_ERR(sgt)) { + pr_err("Error getting dmabuf scatterlist\n"); + return -EINVAL; + } + + buf->dma_sgt = sgt; + buf->vaddr = NULL; + + return 0; +} + +static void vb2_dma_sg_unmap_dmabuf(void *mem_priv) +{ + struct vb2_dma_sg_buf *buf = mem_priv; + struct sg_table *sgt = buf->dma_sgt; + + if (WARN_ON(!buf->db_attach)) { + pr_err("trying to unpin a not attached buffer\n"); + return; + } + + if (WARN_ON(!sgt)) { + pr_err("dmabuf buffer is already unpinned\n"); + return; + } + + if (buf->vaddr) { + dma_buf_vunmap(buf->db_attach->dmabuf, buf->vaddr); + buf->vaddr = NULL; + } + dma_buf_unmap_attachment(buf->db_attach, sgt, buf->dma_dir); + + buf->dma_sgt = NULL; +} + +static void vb2_dma_sg_detach_dmabuf(void *mem_priv) +{ + struct vb2_dma_sg_buf *buf = mem_priv; + + /* if vb2 works correctly you should never detach mapped buffer */ + if (WARN_ON(buf->dma_sgt)) + vb2_dma_sg_unmap_dmabuf(buf); + + /* detach this attachment */ + dma_buf_detach(buf->db_attach->dmabuf, buf->db_attach); + kfree(buf); +} + +static void *vb2_dma_sg_attach_dmabuf(struct device *dev, struct dma_buf *dbuf, + unsigned long size, enum dma_data_direction dma_dir) +{ + struct vb2_dma_sg_buf *buf; + struct dma_buf_attachment *dba; + + if (WARN_ON(!dev)) + return ERR_PTR(-EINVAL); + + if (dbuf->size < size) + return ERR_PTR(-EFAULT); + + buf = kzalloc(sizeof(*buf), GFP_KERNEL); + if (!buf) + return ERR_PTR(-ENOMEM); + + buf->dev = dev; + /* create attachment for the dmabuf with the user device */ + dba = dma_buf_attach(dbuf, buf->dev); + if (IS_ERR(dba)) { + pr_err("failed to attach dmabuf\n"); + kfree(buf); + return dba; + } + + buf->dma_dir = dma_dir; + buf->size = size; + buf->db_attach = dba; + + return buf; +} + +static void *vb2_dma_sg_cookie(void *buf_priv) +{ + struct vb2_dma_sg_buf *buf = buf_priv; + + return buf->dma_sgt; +} + +const struct vb2_mem_ops vb2_rdma_sg_memops = { + .alloc = vb2_dma_sg_alloc, + .put = vb2_dma_sg_put, + .get_userptr = vb2_dma_sg_get_userptr, + .put_userptr = vb2_dma_sg_put_userptr, + .prepare = vb2_dma_sg_prepare, + .finish = vb2_dma_sg_finish, + .vaddr = vb2_dma_sg_vaddr, + .mmap = vb2_dma_sg_mmap, + .num_users = vb2_dma_sg_num_users, + .get_dmabuf = vb2_dma_sg_get_dmabuf, + .map_dmabuf = vb2_dma_sg_map_dmabuf, + .unmap_dmabuf = vb2_dma_sg_unmap_dmabuf, + .attach_dmabuf = vb2_dma_sg_attach_dmabuf, + .detach_dmabuf = vb2_dma_sg_detach_dmabuf, + .cookie = vb2_dma_sg_cookie, +}; +EXPORT_SYMBOL_GPL(vb2_rdma_sg_memops); diff --git a/drivers/media/platform/rockchip/ispp/hw.c b/drivers/media/platform/rockchip/ispp/hw.c index 6d9842c8e130..6b68a786aa6e 100644 --- a/drivers/media/platform/rockchip/ispp/hw.c +++ b/drivers/media/platform/rockchip/ispp/hw.c @@ -293,10 +293,6 @@ static int rkispp_hw_probe(struct platform_device *pdev) hw_dev->is_fec_ext = false; hw_dev->is_dma_contig = true; hw_dev->is_mmu = is_iommu_enable(dev); - if (!hw_dev->is_mmu) - hw_dev->mem_ops = &vb2_dma_contig_memops; - else - hw_dev->mem_ops = &vb2_dma_sg_memops; ret = of_reserved_mem_device_init(dev); if (ret) { if (!hw_dev->is_mmu) @@ -304,6 +300,12 @@ static int rkispp_hw_probe(struct platform_device *pdev) else hw_dev->is_dma_contig = false; } + if (!hw_dev->is_mmu) + hw_dev->mem_ops = &vb2_dma_contig_memops; + else if (!hw_dev->is_dma_contig) + hw_dev->mem_ops = &vb2_dma_sg_memops; + else + hw_dev->mem_ops = &vb2_rdma_sg_memops; rkispp_register_fec(hw_dev); pm_runtime_enable(&pdev->dev);