1 files changed, 669 insertions, 0 deletions

diff --git a/drivers/video/b2r2/b2r2_mem_alloc.c b/drivers/video/b2r2/b2r2_mem_alloc.c
new file mode 100644
index 00000000000..584b324b8fe
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_mem_alloc.c
@@ -0,0 +1,669 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal Memory allocator
+ *
+ * Author: Robert Lind <robert.lind@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_mem_alloc.h"
+
+/* Forward declarations */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+		struct b2r2_control *cont, u32 offset, u32 size, bool free);
+static void b2r2_mem_block_free(struct b2r2_mem_block *mem_block);
+static int b2r2_mem_heap_status(struct b2r2_mem_heap *mem_heap,
+		struct b2r2_mem_heap_status *mem_heap_status);
+
+/* Align value down to specified alignment */
+static inline u32 align_down(u32 align, u32 value)
+{
+	return value & ~(align - 1);
+}
+
+/* Align value up to specified alignment */
+static inline u32 align_up(u32 align, u32 value)
+{
+	return (value + align - 1) & ~(align - 1);
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/* About debugfs:
+ *  debugfs is a mountable debug file system.
+ *
+ *  Mount like this:
+ *  mkdir /debug
+ *  mount -t debugfs none /debug
+ *  ls /debug/b2r2/mem
+ *
+ *  ls -al /debug/b2r2/mem/blocks
+ *  cat /debug/b2r2/mem/stats
+ */
+
+
+/* Create string containing memory heap status */
+static char *get_b2r2_mem_stats(struct b2r2_mem_heap *mem_heap, char *buf)
+{
+	struct b2r2_mem_heap_status mem_heap_status;
+
+	if (b2r2_mem_heap_status(mem_heap, &mem_heap_status) != 0) {
+		strcpy(buf, "Error, failed to get status\n");
+		return buf;
+	}
+
+	sprintf(buf,
+			"Handle                 : 0x%lX\n"
+			"Physical start address : 0x%lX\n"
+			"Size                   : %lu\n"
+			"Align                  : %lu\n"
+			"No of blocks allocated : %lu\n"
+			"Allocated size         : %lu\n"
+			"No of free blocks      : %lu\n"
+			"Free size              : %lu\n"
+			"No of locks            : %lu\n"
+			"No of locked           : %lu\n"
+			"No of nodes            : %lu\n",
+			(unsigned long) mem_heap,
+			(unsigned long) mem_heap_status.start_phys_addr,
+			(unsigned long) mem_heap_status.size,
+			(unsigned long) mem_heap_status.align,
+			(unsigned long) mem_heap_status.num_alloc,
+			(unsigned long) mem_heap_status.allocated_size,
+			(unsigned long) mem_heap_status.num_free,
+			(unsigned long) mem_heap_status.free_size,
+			(unsigned long) mem_heap_status.num_locks,
+			(unsigned long) mem_heap_status.num_locked,
+			(unsigned long) mem_heap_status.num_nodes);
+
+	return buf;
+}
+
+/*
+ * Print memory heap status on file
+ * (Use like "cat /debug/b2r2/mem/stats")
+ */
+static int debugfs_b2r2_mem_stats_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_heap *mem_heap = filp->f_dentry->d_inode->i_private;
+	char Buf[400];
+	size_t dev_size;
+	int ret = 0;
+
+	get_b2r2_mem_stats(mem_heap, Buf);
+	dev_size = strlen(Buf);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for the "stats" file */
+static const struct file_operations debugfs_b2r2_mem_stats_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_stats_read,
+};
+
+/* read function for file in the "blocks" sub directory */
+static int debugfs_b2r2_mem_block_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_block *mem_block = filp->f_dentry->d_inode->i_private;
+	char Buf[200];
+	size_t dev_size;
+	int ret = 0;
+
+	dev_size = sprintf(Buf, "offset: %08lX %s size: %8d "
+			"lock_count: %2d\n",
+			(unsigned long) mem_block->offset,
+			mem_block->free ? "free" : "allc",
+			mem_block->size,
+			mem_block->lock_count);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for files in the "blocks" directory */
+static const struct file_operations debugfs_b2r2_mem_block_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_block_read,
+};
+
+/*
+ * Create or update the debugfs directory entry for a file in the
+ * "blocks" directory (a memory allocation)
+ */
+void debugfs_create_mem_block_entry(struct b2r2_mem_block *mem_block,
+				    struct dentry *parent)
+{
+	struct timespec tm = current_kernel_time();
+	struct timespec atime = tm;
+	struct timespec mtime = tm;
+	struct timespec ctime = tm;
+
+	if (!IS_ERR_OR_NULL(mem_block->debugfs_block)) {
+		atime = mem_block->debugfs_block->d_inode->i_atime;
+		ctime = mem_block->debugfs_block->d_inode->i_ctime;
+		debugfs_remove(mem_block->debugfs_block);
+		mem_block->debugfs_block = NULL;
+	}
+
+	/* Add the block in debugfs */
+	if (mem_block->free)
+		sprintf(mem_block->debugfs_fname, "%08lX free",
+				(unsigned long) mem_block->offset);
+	else {
+		sprintf(mem_block->debugfs_fname, "%08lX allc h:%08lX "
+				"lck:%d ",
+				(unsigned long) mem_block->offset,
+				(unsigned long) mem_block,
+				mem_block->lock_count);
+	}
+
+	mem_block->debugfs_block = debugfs_create_file(
+		mem_block->debugfs_fname,
+		0444, parent, mem_block,
+		&debugfs_b2r2_mem_block_fops);
+	if (!IS_ERR_OR_NULL(mem_block->debugfs_block)) {
+		mem_block->debugfs_block->d_inode->i_size = mem_block->size;
+		mem_block->debugfs_block->d_inode->i_atime = atime;
+		mem_block->debugfs_block->d_inode->i_mtime = mtime;
+		mem_block->debugfs_block->d_inode->i_ctime = ctime;
+	}
+}
+#endif   /* CONFIG_DEBUG_FS */
+
+/* Module initialization function */
+int b2r2_mem_init(struct b2r2_control *cont,
+		u32 heap_size, u32 align, u32 node_size)
+{
+	struct b2r2_mem_block *mem_block;
+	u32 aligned_size;
+
+	dev_info(cont->dev, "%s: Creating heap for size %d bytes\n",
+		__func__, (int) heap_size);
+
+	/* Align size */
+	aligned_size = align_down(align, heap_size);
+	if (aligned_size == 0)
+		return -EINVAL;
+
+	cont->mem_heap.start_virt_ptr = dma_alloc_coherent(cont->dev,
+		aligned_size, &(cont->mem_heap.start_phys_addr), GFP_KERNEL);
+	if (!cont->mem_heap.start_phys_addr || !cont->mem_heap.start_virt_ptr) {
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize the heap */
+	cont->mem_heap.size  = aligned_size;
+	cont->mem_heap.align = align;
+
+	INIT_LIST_HEAD(&cont->mem_heap.blocks);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debugfs */
+	if (!IS_ERR_OR_NULL(cont->mem_heap.debugfs_root_dir)) {
+		cont->mem_heap.debugfs_heap_stats = debugfs_create_file(
+			"stats", 0444, cont->mem_heap.debugfs_root_dir,
+			&cont->mem_heap, &debugfs_b2r2_mem_stats_fops);
+		cont->mem_heap.debugfs_dir_blocks = debugfs_create_dir(
+			"blocks", cont->mem_heap.debugfs_root_dir);
+	}
+#endif
+
+	/* Create the first _free_ memory block */
+	mem_block = b2r2_mem_block_alloc(cont, 0, aligned_size, true);
+	if (!mem_block) {
+		dma_free_coherent(cont->dev, aligned_size,
+			cont->mem_heap.start_virt_ptr,
+			cont->mem_heap.start_phys_addr);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Add the free block to the blocks list */
+	list_add(&mem_block->list, &cont->mem_heap.blocks);
+
+	/* Allocate separate heap for B2R2 nodes */
+	cont->mem_heap.node_size = node_size;
+	cont->mem_heap.node_heap = dma_pool_create("b2r2_node_cache",
+		cont->dev, node_size, align, 4096);
+	if (!cont->mem_heap.node_heap) {
+		b2r2_mem_block_free(mem_block);
+		dma_free_coherent(cont->dev, aligned_size,
+			cont->mem_heap.start_virt_ptr,
+			cont->mem_heap.start_phys_addr);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_init);
+
+/* Module exit function */
+void b2r2_mem_exit(struct b2r2_control *cont)
+{
+	struct list_head *ptr;
+
+	/* Free B2R2 node heap */
+	dma_pool_destroy(cont->mem_heap.node_heap);
+
+	list_for_each(ptr, &cont->mem_heap.blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		b2r2_mem_block_free(mem_block);
+	}
+
+	dma_free_coherent(cont->dev, cont->mem_heap.size,
+		cont->mem_heap.start_virt_ptr,
+		cont->mem_heap.start_phys_addr);
+}
+EXPORT_SYMBOL(b2r2_mem_exit);
+
+/* Return status of the heap */
+static int b2r2_mem_heap_status(struct b2r2_mem_heap *mheap,
+		struct b2r2_mem_heap_status *mem_heap_status)
+{
+	struct list_head *ptr;
+
+	if (!mheap || !mem_heap_status)
+		return -EINVAL;
+	memset(mem_heap_status, 0, sizeof(*mem_heap_status));
+
+	/* Lock the heap */
+	spin_lock(&mheap->heap_lock);
+
+	/* Fill in static info */
+	mem_heap_status->start_phys_addr = mheap->start_phys_addr;
+	mem_heap_status->size = mheap->size;
+	mem_heap_status->align = mheap->align;
+
+	list_for_each(ptr, &mheap->blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free) {
+			mem_heap_status->num_free++;
+			mem_heap_status->free_size += mem_block->size;
+		} else {
+			if (mem_block->lock_count) {
+				mem_heap_status->num_locked++;
+				mem_heap_status->num_locks +=
+					mem_block->lock_count;
+			}
+			mem_heap_status->num_alloc++;
+			mem_heap_status->allocated_size += mem_block->size;
+		}
+	}
+
+	spin_unlock(&mheap->heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_heap_status);
+
+/* Internal: Allocate a housekeeping structure
+ * for an allocated or free memory block
+ */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+		struct b2r2_control *cont, u32 offset, u32 size, bool free)
+{
+	struct b2r2_mem_block *mem_block = kmalloc(
+		sizeof(struct b2r2_mem_block), GFP_KERNEL);
+
+	if (mem_block) {
+		mem_block->offset     = offset;
+		mem_block->size       = size;
+		mem_block->free       = free;
+		mem_block->lock_count = 0;
+
+		INIT_LIST_HEAD(&mem_block->list);
+
+#ifdef CONFIG_DEBUG_FS
+		mem_block->debugfs_block  = NULL;
+		/* Add the block in debugfs */
+		debugfs_create_mem_block_entry(mem_block,
+			cont->mem_heap.debugfs_dir_blocks);
+#endif
+	}
+
+	return mem_block;
+}
+
+/* Internal: Release housekeeping structure */
+static void b2r2_mem_block_free(struct b2r2_mem_block *mem_block)
+{
+	if (mem_block) {
+#ifdef CONFIG_DEBUG_FS
+		debugfs_remove(mem_block->debugfs_block);
+#endif
+		kfree(mem_block);
+	}
+}
+
+/* Allocate a block from the heap */
+int b2r2_mem_alloc(struct b2r2_control *cont, u32 requested_size,
+		u32 *returned_size, u32 *mem_handle)
+{
+	int ret = 0;
+	struct list_head *ptr;
+	struct b2r2_mem_block *found_mem_block = NULL;
+	u32 aligned_size;
+
+	if (!mem_handle)
+		return -EINVAL;
+
+	printk(KERN_INFO "%s: size=%d\n", __func__, requested_size);
+
+	*mem_handle = 0;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	aligned_size = align_up(cont->mem_heap.align, requested_size);
+	/* Try to find the best matching free block of suitable size */
+	list_for_each(ptr, &cont->mem_heap.blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free && mem_block->size >= aligned_size &&
+				(!found_mem_block ||
+				mem_block->size < found_mem_block->size)) {
+			found_mem_block = mem_block;
+			if (found_mem_block->size == aligned_size)
+				break;
+		}
+	}
+
+	if (found_mem_block) {
+		struct b2r2_mem_block *new_block
+			= b2r2_mem_block_alloc(cont,
+				found_mem_block->offset,
+				requested_size, false);
+
+		if (new_block) {
+			/* Insert the new block before the found block */
+			list_add_tail(&new_block->list,
+				&found_mem_block->list);
+
+			/* Split the free block */
+			found_mem_block->offset += aligned_size;
+			found_mem_block->size   -= aligned_size;
+
+			if (found_mem_block->size == 0)
+				b2r2_mem_block_free(found_mem_block);
+			else {
+#ifdef CONFIG_DEBUG_FS
+				debugfs_create_mem_block_entry(
+					found_mem_block,
+					cont->mem_heap.debugfs_dir_blocks);
+#endif
+			}
+
+			*mem_handle = (u32) new_block;
+			*returned_size = aligned_size;
+		} else {
+			ret = -ENOMEM;
+		}
+	} else
+		ret = -ENOMEM;
+
+	if (ret != 0) {
+		*returned_size = 0;
+		*mem_handle = (u32) 0;
+	}
+
+	/* Unlock */
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_alloc);
+
+/* Free the allocated block */
+int b2r2_mem_free(struct b2r2_control *cont, u32 mem_handle)
+{
+	int ret = 0;
+	struct b2r2_mem_block *mem_block = (struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	if (!ret && mem_block->free)
+		ret = -EINVAL;
+
+	if (!ret) {
+		printk(KERN_INFO "%s: freeing block 0x%p\n", __func__, mem_block);
+		/* Release the block */
+
+		mem_block->free = true;
+		mem_block->size = align_up(cont->mem_heap.align,
+					   mem_block->size);
+
+		/* Join with previous block if possible */
+		if (mem_block->list.prev != &cont->mem_heap.blocks) {
+			struct b2r2_mem_block *prev_block =
+				list_entry(mem_block->list.prev,
+					struct b2r2_mem_block, list);
+
+			if (prev_block->free &&
+				(prev_block->offset + prev_block->size) ==
+				mem_block->offset) {
+				mem_block->offset = prev_block->offset;
+				mem_block->size   += prev_block->size;
+
+				b2r2_mem_block_free(prev_block);
+			}
+		}
+
+		/* Join with next block if possible */
+		if (mem_block->list.next != &cont->mem_heap.blocks) {
+			struct b2r2_mem_block *next_block
+				= list_entry(mem_block->list.next,
+						struct b2r2_mem_block,
+						list);
+
+			if (next_block->free &&
+				(mem_block->offset + mem_block->size) ==
+				next_block->offset) {
+				mem_block->size   += next_block->size;
+
+				b2r2_mem_block_free(next_block);
+			}
+		}
+#ifdef CONFIG_DEBUG_FS
+		debugfs_create_mem_block_entry(mem_block,
+				cont->mem_heap.debugfs_dir_blocks);
+#endif
+	}
+
+	/* Unlock */
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_free);
+
+/* Lock the allocated block in memory */
+int b2r2_mem_lock(struct b2r2_control *cont, u32 mem_handle,
+		u32 *phys_addr, void **virt_ptr, u32 *size)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	mem_block->lock_count++;
+
+	if (phys_addr)
+		*phys_addr = cont->mem_heap.start_phys_addr + mem_block->offset;
+	if (virt_ptr)
+		*virt_ptr = (char *) cont->mem_heap.start_virt_ptr +
+			mem_block->offset;
+	if (size)
+		*size = align_up(cont->mem_heap.align, mem_block->size);
+#ifdef CONFIG_DEBUG_FS
+	debugfs_create_mem_block_entry(mem_block,
+		cont->mem_heap.debugfs_dir_blocks);
+#endif
+
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_lock);
+
+/* Unlock the allocated block in memory */
+int b2r2_mem_unlock(struct b2r2_control *cont, u32 mem_handle)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	mem_block->lock_count--;
+
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	/* debugfs will be updated in release */
+	return 0;
+/*	return b2r2_mem_free(mem_handle);*/
+}
+EXPORT_SYMBOL(b2r2_mem_unlock);
+
+/* Allocate one or more b2r2 nodes from DMA pool */
+int b2r2_node_alloc(struct b2r2_control *cont, u32 num_nodes,
+		struct b2r2_node **first_node)
+{
+	int i;
+	int ret = 0;
+	u32 physical_address;
+	struct b2r2_node *first_node_ptr;
+	struct b2r2_node *node_ptr;
+
+	/* Check input parameters */
+	if ((num_nodes <= 0) || !first_node) {
+		dev_err(cont->dev,
+			"B2R2_MEM: Invalid parameter for b2r2_node_alloc, "
+			"num_nodes=%d, first_node=%ld\n",
+			(int) num_nodes, (long) first_node);
+		return -EINVAL;
+	}
+
+	/* Allocate the first node */
+	first_node_ptr = dma_pool_alloc(cont->mem_heap.node_heap,
+		GFP_DMA | GFP_KERNEL, &physical_address);
+	if (!first_node_ptr) {
+		dev_err(cont->dev,
+			"B2R2_MEM: Failed to allocate memory for node\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize first node */
+	first_node_ptr->next = NULL;
+	first_node_ptr->physical_address = physical_address +
+			offsetof(struct b2r2_node, node);
+
+	/* Allocate and initialize remaining nodes, */
+	/* and link them into a list                */
+	for (i = 1, node_ptr = first_node_ptr; i < num_nodes; i++) {
+		node_ptr->next = dma_pool_alloc(cont->mem_heap.node_heap,
+				GFP_DMA | GFP_KERNEL, &physical_address);
+		if (node_ptr->next) {
+			node_ptr = node_ptr->next;
+			node_ptr->next = NULL;
+			node_ptr->physical_address = physical_address +
+					offsetof(struct b2r2_node, node);
+		} else {
+			printk(KERN_ERR "B2R2_MEM: Failed to allocate memory for node\n");
+			ret = -ENOMEM;
+			break;
+		}
+	}
+
+	/* If all nodes were allocated successfully, */
+	/* return the first node                     */
+	if (!ret)
+		*first_node = first_node_ptr;
+	else
+		b2r2_node_free(cont, first_node_ptr);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_node_alloc);
+
+/* Free a linked list of b2r2 nodes */
+void b2r2_node_free(struct b2r2_control *cont, struct b2r2_node *first_node)
+{
+	struct b2r2_node *current_node = first_node;
+	struct b2r2_node *next_node = NULL;
+
+	/* Traverse the linked list and free the nodes */
+	while (current_node != NULL) {
+		next_node = current_node->next;
+		dma_pool_free(cont->mem_heap.node_heap, current_node,
+				current_node->physical_address -
+				offsetof(struct b2r2_node, node));
+		current_node = next_node;
+	}
+}
+EXPORT_SYMBOL(b2r2_node_free);
+
+MODULE_AUTHOR("Robert Lind <robert.lind@ericsson.com");
+MODULE_DESCRIPTION("Ericsson AB B2R2 physical memory driver");
+MODULE_LICENSE("GPL");