static void *mfc_get_drv_data(struct platform_device *pdev);
 
+static int s5p_mfc_alloc_memdevs(struct s5p_mfc_dev *dev)
+{
+       unsigned int mem_info[2];
+
+       dev->mem_dev_l = devm_kzalloc(&dev->plat_dev->dev,
+                       sizeof(struct device), GFP_KERNEL);
+       if (!dev->mem_dev_l) {
+               mfc_err("Not enough memory\n");
+               return -ENOMEM;
+       }
+       device_initialize(dev->mem_dev_l);
+       of_property_read_u32_array(dev->plat_dev->dev.of_node,
+                       "samsung,mfc-l", mem_info, 2);
+       if (dma_declare_coherent_memory(dev->mem_dev_l, mem_info[0],
+                               mem_info[0], mem_info[1],
+                               DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
+               mfc_err("Failed to declare coherent memory for\n"
+               "MFC device\n");
+               return -ENOMEM;
+       }
+
+       dev->mem_dev_r = devm_kzalloc(&dev->plat_dev->dev,
+                       sizeof(struct device), GFP_KERNEL);
+       if (!dev->mem_dev_r) {
+               mfc_err("Not enough memory\n");
+               return -ENOMEM;
+       }
+       device_initialize(dev->mem_dev_r);
+       of_property_read_u32_array(dev->plat_dev->dev.of_node,
+                       "samsung,mfc-r", mem_info, 2);
+       if (dma_declare_coherent_memory(dev->mem_dev_r, mem_info[0],
+                               mem_info[0], mem_info[1],
+                               DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
+               pr_err("Failed to declare coherent memory for\n"
+               "MFC device\n");
+               return -ENOMEM;
+       }
+       return 0;
+}
+
 /* MFC probe function */
 static int s5p_mfc_probe(struct platform_device *pdev)
 {
        struct video_device *vfd;
        struct resource *res;
        int ret;
-       unsigned int mem_info[2];
 
        pr_debug("%s++\n", __func__);
        dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
        }
 
        if (pdev->dev.of_node) {
-               dev->mem_dev_l = kzalloc(sizeof(struct device), GFP_KERNEL);
-               if (!dev->mem_dev_l) {
-                       mfc_err("Not enough memory\n");
-                       ret = -ENOMEM;
-                       goto err_res;
-               }
-               of_property_read_u32_array(pdev->dev.of_node, "samsung,mfc-l",
-                               mem_info, 2);
-               if (dma_declare_coherent_memory(dev->mem_dev_l, mem_info[0],
-                               mem_info[0], mem_info[1],
-                               DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
-                       mfc_err("Failed to declare coherent memory for\n"
-                                       "MFC device\n");
-                       ret = -ENOMEM;
+               if (s5p_mfc_alloc_memdevs(dev) < 0)
                        goto err_res;
-               }
-
-               dev->mem_dev_r = kzalloc(sizeof(struct device), GFP_KERNEL);
-               if (!dev->mem_dev_r) {
-                       mfc_err("Not enough memory\n");
-                       ret = -ENOMEM;
-                       goto err_res;
-               }
-               of_property_read_u32_array(pdev->dev.of_node, "samsung,mfc-r",
-                               mem_info, 2);
-               if (dma_declare_coherent_memory(dev->mem_dev_r, mem_info[0],
-                               mem_info[0], mem_info[1],
-                               DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
-                       pr_err("Failed to declare coherent memory for\n"
-                                       "MFC device\n");
-                       ret = -ENOMEM;
-                       goto err_res;
-               }
        } else {
                dev->mem_dev_l = device_find_child(&dev->plat_dev->dev,
                                "s5p-mfc-l", match_child);
        s5p_mfc_release_firmware(dev);
        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
        vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
+       if (pdev->dev.of_node) {
+               put_device(dev->mem_dev_l);
+               put_device(dev->mem_dev_r);
+       }
 
        s5p_mfc_final_pm(dev);
        return 0;