optee: Fix multi page dynamic shm pool alloc

[sfrench/cifs-2.6.git] / drivers / tee / optee / shm_pool.c

diff --git a/drivers/tee/optee/shm_pool.c b/drivers/tee/optee/shm_pool.c
index de1d9b8fad9043c08c848854c93964b1478cb4d1..d767eebf30bdd5625b10350904b019edbe8166ca 100644 (file)
--- a/drivers/tee/optee/shm_pool.c
+++ b/drivers/tee/optee/shm_pool.c
@@ -17,6 +17,7 @@ static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
 {
        unsigned int order = get_order(size);
        struct page *page;
+       int rc = 0;
 
        page = alloc_pages(GFP_KERNEL | __GFP_ZERO, order);
        if (!page)
@@ -26,12 +27,34 @@ static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
        shm->paddr = page_to_phys(page);
        shm->size = PAGE_SIZE << order;
 
-       return 0;
+       if (shm->flags & TEE_SHM_DMA_BUF) {
+               unsigned int nr_pages = 1 << order, i;
+               struct page **pages;
+
+               pages = kcalloc(nr_pages, sizeof(pages), GFP_KERNEL);
+               if (!pages)
+                       return -ENOMEM;
+
+               for (i = 0; i < nr_pages; i++) {
+                       pages[i] = page;
+                       page++;
+               }
+
+               shm->flags |= TEE_SHM_REGISTER;
+               rc = optee_shm_register(shm->ctx, shm, pages, nr_pages,
+                                       (unsigned long)shm->kaddr);
+               kfree(pages);
+       }
+
+       return rc;
 }
 
 static void pool_op_free(struct tee_shm_pool_mgr *poolm,
                         struct tee_shm *shm)
 {
+       if (shm->flags & TEE_SHM_DMA_BUF)
+               optee_shm_unregister(shm->ctx, shm);
+
        free_pages((unsigned long)shm->kaddr, get_order(shm->size));
        shm->kaddr = NULL;
 }