Add beginning of a simple granule allocator to support DMA IO buffer allocation

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@5129 42af7a65-404d-4744-a932-0658087f49c3

10 files changed, 796 insertions, 8 deletions

diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog
index b49394ba2..174b1cc52 100644
--- a/nuttx/ChangeLog
+++ b/nuttx/ChangeLog
@@ -3323,3 +3323,8 @@
 	  ENC28J60 GPIO interrupts.  That is because GPIO interrupts are
 	  handled in different ways by different MCUs and some do not
 	  support IRQ numbers for GPIO interrupts.
+	* mm/mm_gran* and include/nuttx/gran.h:  Add a simple granule-
+	  based allocator.  The intent of this allocator is to support
+	  simple allocation of DMA I/O buffers.  The initiali check-in
+	  is code complete but untested (not event built into the
+	  mm/Makefile yet.
diff --git a/nuttx/fs/fat/fs_fat32.c b/nuttx/fs/fat/fs_fat32.c
index 60622e2f7..0c28cea67 100644
--- a/nuttx/fs/fat/fs_fat32.c
+++ b/nuttx/fs/fat/fs_fat32.c
@@ -405,7 +405,7 @@ static int fat_close(FAR struct file *filep)
 
   if (ff->ff_buffer)
     {
-      fat_io_free(ff->ff_buffer);
+      fat_io_free(ff->ff_buffer, fs->fs_hwsectorsize);
     }
 
   /* Then free the file structure itself. */
@@ -1651,7 +1651,7 @@ static int fat_unbind(void *handle, FAR struct inode **blkdriver)
 
       if (fs->fs_buffer)
         {
-          fat_io_free(fs->fs_buffer);
+          fat_io_free(fs->fs_buffer, fs->fs_hwsectorsize);
         }
 
       kfree(fs);
diff --git a/nuttx/fs/fat/fs_fat32.h b/nuttx/fs/fat/fs_fat32.h
index c7fa5219c..71a21333b 100644
--- a/nuttx/fs/fat/fs_fat32.h
+++ b/nuttx/fs/fat/fs_fat32.h
@@ -697,11 +697,11 @@
  ****************************************************************************/
 
 #ifdef CONFIG_FAT_DMAMEMORY
-#  define fat_io_alloc(s) fat_dma_alloc(s)
-#  define fat_io_free(s)  fat_dma_free(s)
+#  define fat_io_alloc(s)  fat_dma_alloc(s)
+#  define fat_io_free(m,s) fat_dma_free(m,s)
 #else
-#  define fat_io_alloc(s) kmalloc(s)
-#  define fat_io_free(s)  kfree(s)
+#  define fat_io_alloc(s)  kmalloc(s)
+#  define fat_io_free(m,s) kfree(m)
 #endif
 
 /****************************************************************************
diff --git a/nuttx/fs/fat/fs_fat32util.c b/nuttx/fs/fat/fs_fat32util.c
index f8f5ef495..7231456d7 100644
--- a/nuttx/fs/fat/fs_fat32util.c
+++ b/nuttx/fs/fat/fs_fat32util.c
@@ -668,7 +668,7 @@ int fat_mount(struct fat_mountpt_s *fs, bool writeable)
   return OK;
 
  errout_with_buffer:
-  fat_io_free(fs->fs_buffer);
+  fat_io_free(fs->fs_buffer, fs->fs_hwsectorsize);
   fs->fs_buffer = 0;
  errout:
   fs->fs_mounted = false;
diff --git a/nuttx/include/nuttx/fs/fat.h b/nuttx/include/nuttx/fs/fat.h
index 3ae0fbee2..680aefbe9 100644
--- a/nuttx/include/nuttx/fs/fat.h
+++ b/nuttx/include/nuttx/fs/fat.h
@@ -106,7 +106,7 @@ EXTERN int fat_setattrib(const char *path, fat_attrib_t setbits, fat_attrib_t cl
  ****************************************************************************/
 
 EXTERN FAR void *fat_dma_alloc(size_t size);
-EXTERN void fat_dma_free(FAR void *memory);
+EXTERN void fat_dma_free(FAR void *memory, size_t size);
 
 #undef EXTERN
 #ifdef __cplusplus
diff --git a/nuttx/include/nuttx/gran.h b/nuttx/include/nuttx/gran.h
new file mode 100644
index 000000000..2036a29a1
--- /dev/null
+++ b/nuttx/include/nuttx/gran.h
@@ -0,0 +1,161 @@
+/****************************************************************************
+ * include/nuttx/gran.h
+ * General purpose granule memory allocator.
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_GRAN_H
+#define __INCLUDE_NUTTX_GRAN_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+/* Configuration ************************************************************/
+/* CONFIG_GRAN - Enable granual allocator support
+ * CONFIG_GRAN_SINGLE - Select if there is only one instance of the
+ *   granule allocator (i.e., gran_initialize will be called only once.
+ *   In this case, (1) there are a few optimizations that can can be done
+ *   and (2) the GRAN_HANDLE is not needed.
+ */
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+#ifndef CONFIG_GRAN_SINGLE
+typedef FAR void *GRAN_HANDLE;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/****************************************************************************
+ * Name: gran_initialize
+ *
+ * Description:
+ *   Set up one granule allocator instance.  Allocations will be aligned to
+ *   the granule size; allocations will be in units of the granule size.
+ *   Larger granules will give better performance and less overhead but more
+ *   losses of memory due to alignment and quantization waste.
+ *
+ *   NOTE: The current implementation also restricts the maximum allocation
+ *   size to 32 granaules.  That restriction could be eliminated with some
+ *   additional coding effort.
+ *
+ * Input Parameters:
+ *   heapstart - Start of the granule allocation heap
+ *   heapsize  - Size of heap in bytes
+ *   log2gran  - Log base 2 of the size of one granule.  0->1 byte,
+ *               1->2 bytes, 2->4 bytes, 3-> 8bytes, etc.
+ *
+ * Returned Value:
+ *   On success, a non-NULL handle is returned that may be used with other
+ *   granual allocator interfaces.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+EXTERN void gran_initialize(FAR void *heapstart, size_t heapsize,
+                            uint8_t log2gran);
+#else
+EXTERN GRAN_HANDLE gran_initialize(FAR void *heapstart, size_t heapsize,
+                                   uint8_t log2gran);
+#endif
+
+/****************************************************************************
+ * Name: gran_alloc
+ *
+ * Description:
+ *   Allocate memory from the granule heap.
+ *
+ *   NOTE: The current implementation also restricts the maximum allocation
+ *   size to 32 granaules.  That restriction could be eliminated with some
+ *   additional coding effort.
+ *
+ * Input Parameters:
+ *   handle - The handle previously returned by gran_initialize
+ *   size   - The size of the memory region to allocate.
+ *
+ * Returned Value:
+ *   On success, either a non-NULL pointer to the allocated memory (if
+ *   CONFIG_GRAN_SINGLE) or zero  (if !CONFIG_GRAN_SINGLE) is returned.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+EXTERN FAR void *gran_alloc(size_t size);
+#else
+EXTERN FAR void *gran_alloc(GRAN_HANDLE handle, size_t size);
+#endif
+
+/****************************************************************************
+ * Name: gran_free
+ *
+ * Description:
+ *   Return memory to the granule heap.
+ *
+ * Input Parameters:
+ *   handle - The handle previously returned by gran_initialize
+ *   memory - A pointer to memory previoiusly allocated by gran_alloc.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+EXTERN void gran_free(FAR void *memory, size_t size);
+#else
+EXTERN void gran_free(GRAN_HANDLE handle, FAR void *memory, size_t size);
+#endif
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INCLUDE_NUTTX_GRAN_H */
diff --git a/nuttx/mm/mm_gran.h b/nuttx/mm/mm_gran.h
new file mode 100644
index 000000000..6f5f890ed
--- /dev/null
+++ b/nuttx/mm/mm_gran.h
@@ -0,0 +1,84 @@
+/****************************************************************************
+ * mm/mm_gran.h
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef __MM_MM_GRAN_H
+#define __MM_MM_GRAN_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/gran.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define SIZEOF_GAT(n) \
+  ((n + 31) >> 5)
+#define SIZEOF_GRAN_S(n) \
+  (sizeof(struct gran_s) + sizeof(uint32_t) * (SIZEOF_GAT(n) - 1))
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/* This structure represents the state of on granual allocation */
+
+struct gran_s
+{
+  uint8_t    log2gran;  /* Log base 2 of the size of one granule */
+  uint16_t   ngranules; /* The total number of (aligned) granules in the heap */
+  uintptr_t  heapstart; /* The aligned start of the granule heap */
+  uint32_t   gat[i];    /* Start of the granule allocation table */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* State of the single GRAN allocator */
+
+#ifdef CONFIG_GRAN_SINGLE
+extern FAR struct gran_s *g_graninfo;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#endif /* __MM_MM_GRAN_H */
diff --git a/nuttx/mm/mm_granalloc.c b/nuttx/mm/mm_granalloc.c
new file mode 100644
index 000000000..f94f447e3
--- /dev/null
+++ b/nuttx/mm/mm_granalloc.c
@@ -0,0 +1,241 @@
+/****************************************************************************
+ * mm/mm_granalloc.c
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/gran.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_common_alloc
+ *
+ * Description:
+ *   Allocate memory from the granule heap.
+ *
+ * Input Parameters:
+ *   priv  - The granule heap state structure.
+ *   alloc - The adress of the allocation.
+ *   ngranules - The number of granules allocated
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static inline FAR void *gran_mark_allocated(FAR struct gran_s *priv, uintptr_t alloc, unsigned int ngranules)
+{
+  unsigned int granno;
+  unsigned int gatidx;
+  unsigned int gatbit;
+  unsigned int avail;
+  uint32_t mask;
+
+  /* Determine the granule number of the allocation */
+
+  granno = (alloc - priv->heapstart) >> priv->log2gran;
+
+  /* Determine the GAT table index associated with the allocation */
+
+  gatidx = granno >> 5;
+  gatbit = granno & 31;
+
+  /* Mark bits in the first GAT entry */
+
+  avail = 32 - gatbit;
+  if (ngranules > avail)
+    {
+      priv->gat[gatidx] |= (0xffffffff << gatbit);
+      ngranules -= avail;
+    }
+
+  /* Handle the cae where where all of the granules come from one entry */
+
+  else
+    {
+      mask = 0xffffffff >> (32 - ngranules);
+      priv->gat[gatidx] |= (mask << gatbit);
+      return;
+    }
+
+  /* Mark bits in the second GAT entry */
+
+  mask = 0xffffffff >> (32 - ngranules);
+  priv->gat[gatidx+1] |= (mask << gatbit);
+}
+
+/****************************************************************************
+ * Name: gran_common_alloc
+ *
+ * Description:
+ *   Allocate memory from the granule heap.
+ *
+ * Input Parameters:
+ *   priv - The granule heap state structure.
+ *   size - The size of the memory region to allocate.
+ *
+ * Returned Value:
+ *   On success, a non-NULL pointer to the allocated memory is returned.
+ *
+ ****************************************************************************/
+
+static inline FAR void *gran_common_alloc(FAR struct gran_s *priv, size_t size)
+{
+  unsigned int ngranules;
+  size_t       tmpmask;
+  uintptr_t     alloc;
+  uint32_t      curr;
+  uint32_t      next;
+  uint32_t      mask;
+  int           i;
+  int           j;
+
+  DEBUGASSERT(priv);
+  if (priv && size > 0)
+    {
+      /* How many contiguous granules we we need to find? */
+
+      tmpmask   = (1 << log2gran) - 1;
+      ngranules = (size + tmpmask) >> log2gran;
+
+      /* Then create mask for that number of granules */
+
+      DEBUGASSERT(ngranules <= 32);
+      mask = 0xffffffff >> (32 - ngranules);
+
+      /* Now search the granule allocation table for that number of contiguous */
+
+      alloc = priv->heapstart;
+
+      for (i = 0; i < priv->ngranules; i += 32)
+        {
+          /* Get the GAT index associated with the granule (i) */
+
+          j = i >> 5;
+          curr  = priv->gat[j];
+
+          /* Get the next entry from the GAT to support a 64 bit shift */
+
+          if (i < priv->ngranules)
+            {
+              next = priv->gat[j + 1];
+            }
+
+          /* Use all zeroes when are at the last entry in the GAT */
+
+          else
+            {
+              next = 0;
+            }
+
+          for (j = 0; j < 32; j++)
+            {
+              /* Check if we have the allocation at this bit position (0
+               * means unallocated).
+               */
+
+              if ((curr & mask) == 0)
+                {
+                  /* Yes.. mark these granules allocated */
+
+                  gran_mark_allocated(priv, alloc, ngranules);
+
+                  /* And return the allocation address */
+
+                  return (FAR void *)alloc;
+                }
+
+              /* Set up for the next time through the loop.  Perform a 64
+               * bit shift to move to the next gram position.
+               */
+
+              curr >>= 1;
+              if (next & 1)
+                {
+                  curr |= 0x80000000;
+                }
+              next >> 1;
+
+              /* Increment the next candidate allocation address */
+
+              alloc += (1 << priv->log2gran);
+            }
+        }
+    }
+
+  return NULL;
+}
+
+/****************************************************************************
+ * Global Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_alloc
+ *
+ * Description:
+ *   Allocate memory from the granule heap.
+ *
+ *   NOTE: The current implementation also restricts the maximum allocation
+ *   size to 32 granaules.  That restriction could be eliminated with some
+ *   additional coding effort.
+ *
+ * Input Parameters:
+ *   handle - The handle previously returned by gran_initialize
+ *   size   - The size of the memory region to allocate.
+ *
+ * Returned Value:
+ *   On success, either a non-NULL pointer to the allocated memory (if
+ *   CONFIG_GRAN_SINGLE) or zero  (if !CONFIG_GRAN_SINGLE) is returned.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+FAR void *gran_alloc(size_t size)
+{
+  return gran_common_alloc(g_graninfo, size);
+}
+#else
+FAR void *gran_alloc(GRAN_HANDLE handle, size_t size)
+{
+  return gran_common_alloc((FAR struct gran_s *)handle, size);
+}
+#endif
diff --git a/nuttx/mm/mm_granfree.c b/nuttx/mm/mm_granfree.c
new file mode 100644
index 000000000..4679ae62b
--- /dev/null
+++ b/nuttx/mm/mm_granfree.c
@@ -0,0 +1,134 @@
+/****************************************************************************
+ * mm/mm_granfree.c
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/gran.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_common_free
+ *
+ * Description:
+ *   Return memory to the granule heap.
+ *
+ * Input Parameters:
+ *   handle - The handle previously returned by gran_initialize
+ *   memory - A pointer to memory previoiusly allocated by gran_alloc.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static inline void gran_common_free(FAR struct gran_s *priv,
+                                    FAR void *memory, size_t size)
+{
+  unsigned int granno;
+  unsigned int gatidx;
+  unsigned int gatbit;
+  unsigned int avail;
+  uint32_t mask;
+
+  /* Determine the granule number of the allocation */
+
+  granno = (alloc - priv->heapstart) >> priv->log2gran;
+
+  /* Determine the GAT table index associated with the allocation */
+
+  gatidx = granno >> 5;
+  gatbit = granno & 31;
+
+  /* Clear bits in the first GAT entry */
+
+  avail = 32 - gatbit;
+  if (ngranules > avail)
+    {
+      priv->gat[gatidx] &= ~(0xffffffff << gatbit);
+      ngranules -= avail;
+    }
+
+  /* Handle the cae where where all of the granules came from one entry */
+
+  else
+    {
+      mask = 0xffffffff >> (32 - ngranules);
+      priv->gat[gatidx] &= ~(mask << gatbit);
+      return;
+    }
+
+  /* Clear bits in the second GAT entry */
+
+  mask = 0xffffffff >> (32 - ngranules);
+  priv->gat[gatidx+1] &= ~(mask << gatbit);
+}
+
+/****************************************************************************
+ * Global Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_free
+ *
+ * Description:
+ *   Return memory to the granule heap.
+ *
+ * Input Parameters:
+ *   handle - The handle previously returned by gran_initialize
+ *   memory - A pointer to memory previoiusly allocated by gran_alloc.
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+void gran_free(FAR void *memory, size_t size)
+{
+  return gran_common_free(g_graninfo, memory, size);
+}
+#else
+void gran_free(GRAN_HANDLE handle, FAR void *memory, size_t size)
+{
+  return gran_common_free((FAR struct gran_s *)handle, memory, size);
+}
+#endif
diff --git a/nuttx/mm/mm_graninit.c b/nuttx/mm/mm_graninit.c
new file mode 100644
index 000000000..99b499155
--- /dev/null
+++ b/nuttx/mm/mm_graninit.c
@@ -0,0 +1,163 @@
+/****************************************************************************
+ * mm/mm_graninit.c
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/gran.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* State of the single GRAN allocator */
+
+#ifdef CONFIG_GRAN_SINGLE
+FAR struct gran_s *g_graninfo;
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_common_initialize
+ *
+ * Description:
+ *   Perfrom common GRAN initialization.
+ *
+ * Input Parameters:
+ *   info      - Private granule data structure pointer
+ *   heapstart - Start of the granule allocation heap
+ *   heapsize  - Size of heap in bytes
+ *   log2gran  - Log base 2 of the size of one granule.  0->1 byte,
+ *               1->2 bytes, 2->4 bytes, 3-> 8bytes, etc.
+ *
+ * Returned Value:
+ *   On success, a non-NULL info structure is returned that may be used with
+ *   other granule allocator interfaces.
+ *
+ ****************************************************************************/
+
+static inline FAR struct gran_s *gran_common_initialize(FAR void *heapstart,
+                                                        size_t heapsize,
+                                                        uint8_t log2gran)
+{
+  FAR struct gran_s *priv;
+  uintptr_t          heapend;
+  uintptr_t          alignedstart;
+  unsigned int       mask;
+  unsigned int       alignedsize;
+  unsigned int       ngranules;
+
+  /* Determine the number of granules */
+
+  mask         = (1 << log2gran) - 1;
+  heapend      = (uintptr_t)heapstart + heapsize;
+  alignedstart = ((uintptr_t)heapstart + mask) & ~mask;
+  alignedsize  = (heapend - alignedstart) & ~mask;
+  ngranules    = alignedsize >> log2gran;
+
+  /* Allocate the information structure with a granule table of the
+   * correct size.
+   */
+
+  priv = ( FAR struct gran_s *)zalloc(SIZEOF_GRAN_S(ngranules));
+  if (priv)
+    {
+      /* Initialize non-zero elements of the granules heap info structure */
+
+      priv->log2gran  = log2gran;
+      priv->ngranules = ngranules;
+      priv->heapstart = alignedstart
+    }
+
+  return priv;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: gran_initialize
+ *
+ * Description:
+ *   Set up one granule allocator instance.  Allocations will be aligned to
+ *   the granule size; allocations will be in units of the granule size.
+ *   Larger granules will give better performance and less overhead but more
+ *   losses of memory due to alignment and quantization waste.
+ *
+ *   NOTE: The current implementation also restricts the maximum allocation
+ *   size to 32 granaules.  That restriction could be eliminated with some
+ *   additional coding effort.
+ *
+ * Input Parameters:
+ *   heapstart - Start of the granule allocation heap
+ *   heapsize  - Size of heap in bytes
+ *   log2gran  - Log base 2 of the size of one granule.  0->1 byte,
+ *               1->2 bytes, 2->4 bytes, 3-> 8bytes, etc.
+ *
+ * Returned Value:
+ *   On success, a non-NULL handle is returned that may be used with other
+ *   granual allocator interfaces.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_GRAN_SINGLE
+int gran_initialize(FAR void *heapstart, size_t heapsize, uint8_t log2gran)
+{
+  g_graninfo = gran_common_initialize(heapstart, heapsize, log2gran);
+  if (!g_granifo)
+    {
+      return -ENOMEM;
+    }
+
+  return OK;
+}
+#else
+GRAN_HANDLE gran_initialize(FAR void *heapstart, size_t heapsize, uint8_t log2gran)
+{
+  return (GRAN_HANDLE)gran_common_initialize(heapstart, heapsize, log2gran);
+}
+#endif
+