1 files changed, 94 insertions, 0 deletions
diff --git a/libc/stdlib/malloc/memalign.c b/libc/stdlib/malloc/memalign.c
new file mode 100644
index 000000000..ce13eb99d
--- /dev/null
+++ b/libc/stdlib/malloc/memalign.c
@@ -0,0 +1,94 @@
+/*
+ * libc/stdlib/malloc/memalign.c -- memalign (`aligned malloc') function
+ *
+ *  Copyright (C) 2002  NEC Corporation
+ *  Copyright (C) 2002  Miles Bader <miles@gnu.org>
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License.  See the file COPYING.LIB in the main
+ * directory of this archive for more details.
+ *
+ * Written by Miles Bader <miles@gnu.org>
+ */
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/mman.h>
+
+#include "malloc.h"
+#include "heap.h"
+
+
+#define MAX(x,y) ((x) > (y) ? (x) : (y))
+
+/*
+      ______________________ TOTAL _________________________
+     /                                                      \
+    +---------------+-------------------------+--------------+
+    |               |                         |              |
+    +---------------+-------------------------+--------------+
+    \____ INIT ____/ \______ RETURNED _______/ \____ END ___/
+*/
+
+void *
+memalign (size_t alignment, size_t size)
+{
+  void *mem, *base;
+  unsigned long tot_addr, tot_end_addr, addr, end_addr;
+  struct heap *heap = &__malloc_heap;
+
+  /* Make SIZE something we like.  */
+  size = HEAP_ADJUST_SIZE (size);
+
+  /* Use malloc to do the initial allocation, since it deals with getting
+     system memory.  We over-allocate enough to be sure that we'll get
+     enough memory to hold a properly aligned block of size SIZE,
+     _somewhere_ in the result.  */
+  mem = malloc (size + 2 * alignment);
+  if (! mem)
+    /* Allocation failed, we can't do anything.  */
+    return 0;
+  if (alignment < MALLOC_ALIGNMENT)
+    return mem;
+
+  /* Remember the base-address, of the allocation, although we normally
+     use the user-address for calculations, since that's where the
+     alignment matters.  */
+  base = MALLOC_BASE (mem);
+
+  /* The bounds of the initial allocation.  */
+  tot_addr = (unsigned long)mem;
+  tot_end_addr = (unsigned long)base + MALLOC_SIZE (mem);
+
+  /* Find a likely place inside MEM with the right alignment.  */
+  addr = MALLOC_ROUND_UP (tot_addr, alignment);
+
+  /* Unless TOT_ADDR was already aligned correctly, we need to return the
+     initial part of MEM to the heap.  */
+  if (addr != tot_addr)
+    {
+      size_t init_size = addr - tot_addr;
+
+      /* Ensure that memory returned to the heap is large enough.  */
+      if (init_size < HEAP_MIN_SIZE)
+	{
+	  addr = MALLOC_ROUND_UP (tot_addr + HEAP_MIN_SIZE, alignment);
+	  init_size = addr - tot_addr;
+	}
+
+      __heap_free (heap, base, init_size);
+
+      /* Remember that we've freed the initial part of MEM.  */
+      base += init_size;
+    }
+
+  /* Return the end part of MEM to the heap, unless it's too small.  */
+  end_addr = addr + size;
+  if (end_addr + MALLOC_REALLOC_MIN_FREE_SIZE < tot_end_addr)
+    __heap_free (heap, (void *)end_addr, tot_end_addr - end_addr);
+  else
+    /* We didn't free the end, so include it in the size.  */
+    end_addr = tot_end_addr;
+
+  return MALLOC_SETUP (base, end_addr - (unsigned long)base);
+}