#define DEFAULT_ALIGNMENT   16
//#define DEBUG_ALLOC

/*
  __mingw_aligned_malloc and friends, implemented using Microsoft's public
  interfaces and with the help of the algorithm description provided
  by Wu Yongwei: http://sourceforge.net/mailarchive/message.php?msg_id=3847075

  I hereby place this implementation in the public domain.
               -- Steven G. Johnson (stevenj@alum.mit.edu)
*/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stddef.h>             /* ptrdiff_t */
#include <string.h>             /* memmove */

#include <windows.h>

#ifdef DEBUG_ALLOC
#undef fprintf
extern int fprintf (FILE *__stream, const char *__format, ...);
#define TRACE(...) fprintf(stderr, __VA_ARGS__)
#else
#define TRACE(...)
#endif

#define WILL_WRAP(size, alignment) (size > (0xffffffff - (alignment + sizeof (void *))))

#ifdef HAVE_STDINT_H
#  include <stdint.h>           /* uintptr_t */
#else
#  define uintptr_t size_t
#endif

#define NOT_POWER_OF_TWO(n) (((n) & ((n) - 1)))
#define UI(p) ((uintptr_t) (p))
#define CP(p) ((char *) p)

#define PTR_ALIGN(p0, alignment, offset)                                \
            ((void *) (((UI(p0) + (alignment + sizeof(void*)) + offset) \
                        & (~UI(alignment - 1)))                         \
                       - offset))

/* Pointer must sometimes be aligned; assume sizeof(void*) is a power of two. */
#define ORIG_PTR(p) (*(((void **) (UI(p) & (~UI(sizeof(void*) - 1)))) - 1))

static void *
__mingw_aligned_offset_malloc (size_t size, size_t alignment, size_t offset)
{
  void *p0, *p;

  if (NOT_POWER_OF_TWO (alignment))
    {
      errno = EINVAL;
      return ((void *) 0);
    }
  if ((size == 0) || WILL_WRAP(size, alignment))
    return ((void *) 0);
  if (alignment < sizeof (void *))
    alignment = sizeof (void *);

  /* Including the extra sizeof(void*) is overkill on a 32-bit
     machine, since malloc is already 8-byte aligned, as long
     as we enforce alignment >= 8 ...but oh well.  */

  p0 = HeapAlloc (GetProcessHeap(), 0, size + (alignment + sizeof (void *)));
  if (!p0)
    return ((void *) 0);
  p = PTR_ALIGN (p0, alignment, offset);
  ORIG_PTR (p) = p0;
  return p;
}

static void *
__mingw_aligned_malloc (size_t size, size_t alignment)
{
  return __mingw_aligned_offset_malloc (size, alignment, 0);
}

static void
__mingw_aligned_free (void *memblock)
{
  if (memblock)
    HeapFree(GetProcessHeap(), 0, ORIG_PTR (memblock));
}

static void *
__mingw_aligned_offset_realloc (void *memblock, size_t size,
                                size_t alignment, size_t offset)
{
  void *p0, *p;
  ptrdiff_t shift;

  if (!memblock)
    return __mingw_aligned_offset_malloc (size, alignment, offset);
  if (NOT_POWER_OF_TWO (alignment))
    goto bad;
  if ((size == 0) || WILL_WRAP(size, alignment))
    {
      __mingw_aligned_free (memblock);
      return ((void *) 0);
    }
  if (alignment < sizeof (void *))
    alignment = sizeof (void *);

  p0 = ORIG_PTR (memblock);
  /* It is an error for the alignment to change. */
  if (memblock != PTR_ALIGN (p0, alignment, offset))
    goto bad;
  shift = CP (memblock) - CP (p0);

  p0 = HeapReAlloc(GetProcessHeap(), 0, p0, size + (alignment + sizeof (void *)));
  if (!p0)
    return ((void *) 0);
  p = PTR_ALIGN (p0, alignment, offset);

  /* Relative shift of actual data may be different from before, ugh.  */
  if (shift != CP (p) - CP (p0))
    /* ugh, moves more than necessary if size is increased.  */
    memmove (CP (p), CP (p0) + shift, size);

  ORIG_PTR (p) = p0;
  return p;

bad:
  errno = EINVAL;
  return ((void *) 0);
}

static void *
__mingw_aligned_realloc (void *memblock, size_t size, size_t alignment)
{
  return __mingw_aligned_offset_realloc (memblock, size, alignment, 0);
}

/* WRAPPERS */

void *malloc(size_t size)
{
    void *p = __mingw_aligned_malloc(size, DEFAULT_ALIGNMENT);
    TRACE("MALLOC(%d) = %p\n", size, p);
    return p;
}

void *memalign(size_t boundary, size_t size)
{
    void *p = __mingw_aligned_malloc(size, boundary);
    TRACE("MEMALIGN(%d, %d) = %p\n", boundary, size, p);
    return p;
}

void *calloc(size_t nmemb, size_t size)
{
    size_t sz = nmemb * size;
    char *d = __mingw_aligned_malloc(sz, DEFAULT_ALIGNMENT);
    memset(d, 0, sz);
    TRACE("CALLOC(%d, %d) = %p\n", nmemb, size, d);
    return d;
}

void *realloc(void *ptr, size_t size)
{
    void *p = __mingw_aligned_realloc(ptr, size, DEFAULT_ALIGNMENT);
    TRACE("REALLOC(%p, %d) = %p\n", ptr, size, p);
    return p;
}

void free(void *ptr)
{
    TRACE("FREE(%p)\n", ptr);
    __mingw_aligned_free(ptr);
}

char *strdup(const char *s)
{
    char *d;
    size_t size = strlen(s) + 1;
    d = __mingw_aligned_malloc(size, DEFAULT_ALIGNMENT);
    memcpy(d, s, size - 1);
    d[size - 1] = 0;
    TRACE("STRDUP(%p) = %p\n", s, d);
    return d;
}