source: branches/lib/GDE/RAxML/ll_alloc.c

/*
 *   Copyright (C) 2009, 2010, 2011 Lockless Inc., Steven Von Fuerst.
 *
 * This library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * Implement a lockfree allocator based upon lockless queues
 * that communicate between processors, and btrees to hold the
 * unallocated memory.
 */
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif

#include "compiler.h"
#include "ll_asm.h"
#include "ll_list.h"
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdarg.h>

#ifndef WINDOWS
#include <sys/mman.h>
#include <unistd.h>
#include <sys/syscall.h>
#include <linux/futex.h>
#endif /* !WINDOWS */

#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <assert.h>
// #include <malloc.h>
#define USE_PREFIX 1

#ifdef USE_DLL
#define PREFIX(X)   llalloc##X
#else
#ifdef USE_PREFIX
#define PREFIX(X)   llalloc##X
#else
#define PREFIX(X)   X
#endif
#endif


void *PREFIX(memalign)(size_t align, size_t size);
void *PREFIX(malloc)(size_t size);
void *PREFIX(realloc)(void *p, size_t size);
int PREFIX(posix_memalign)(void **p, size_t align, size_t size);
void *PREFIX(calloc)(size_t n, size_t size);
void PREFIX(free)(void *p);

static size_t malloc_usable_size(void *p);

/* Debugging */
//#define DEBUG_ALLOC

/* Extra checking */
//#define DEBUG_ALLOC_SLOW

/* Memory leak debugging */
//#define DEBUG_LEAK
//#define DEBUG_LEAK_DISP               0

/* For windows and valgrind */
//#define EMU_SBRK
//#define EMU_SBRK_VG

/* Turn off slab usage - useful for debugging btree and small alloc code */
//#define DEBUG_NO_SLAB

/* Turn on home-made profiler */
//#define DEBUG_PROFILE

#ifdef DEBUG_PROFILE
#include "prof.h"
#else
#define DECL_PROF_FUNC int __attribute((unused)) profile_dummy
#endif


#ifndef WINDOWS
#define PAGESIZE                4096UL
#else
#define PAGESIZE                ((size_t) 65536)
#endif

/* Seperator between allocations */
#define SEPSIZE                 16
#define PTRSIZE                 8

#define HEADERSIZE              32

#define ADDR_SIZE               27

#define SLABSIZE                ((uintptr_t) (1 << 17))
//#define SLABBMAX              ((SLABSIZE / 8) - 2)
#define SLABBMAX                64      /* About 4M per thread */

/* Slab sizes 0 to 512 bytes in steps of 16 */
#define NUM_SB                  33
#define SB_MAX                  ((NUM_SB - 1) * 16)

/* Maximum size of medium allocations */
#define BTMALLOC                ((1L << ADDR_SIZE) - HEADERSIZE)

#define TOP_SIZE                (-(PAGESIZE * 2))

/* Minimum size to allocate at a time */
#define MINALLOC                (1L << 21)

/* 64 queues */
#define NUM_QS                  64
#define QS_MAX                  (NUM_QS * 16 - SEPSIZE)

/* Only check four fast bins */
#define FAST_MASK               0x0fULL

/* Clear the fast lists at least this often on free() */
#define FREE_FAST               ((1 << 16) - 1)

/* The biggest size that can reasonably be stored in the fast lists */
#ifdef __x86_64__
#define FAST_64_BIN             67108863
#else
#define FAST_64_BIN             3669975
#endif


#ifdef __x86_64__
#define MYSIZE_TO_PTR(T, N) ((dlist *) (((char *) T) + offsetof(atls, qs) + N - SEPSIZE))
#else
#define MYSIZE_TO_PTR(T, N) ((dlist *) (((char *) T) + offsetof(atls, qs) + N/2 - PTRSIZE))
#endif

/* Fast-lists */
#define NUM_FL                  64

/* btree size */
#define BT_MAX                  16

/* 64bit mask type */
typedef unsigned long long u64b;

/* Pre declare */
typedef struct btree btree;

typedef struct sep sep;
struct sep
{
        btree *left;

#ifndef __x86_64__
        int pad;
#endif

        __extension__ union
        {
                __extension__ struct
                {
                        unsigned bs_offset;
                        unsigned size;
                };
                uintptr_t prev;
        };
};

struct btree
{
        /* Seperator */
        sep s;

        __extension__ union
        {
                slist list;
                dlist list2;
                void *data;

                __extension__ struct
                {
                        btree *parent;
                        unsigned bsize[BT_MAX + 1];
                        char prev[BT_MAX + 1];
                        btree *ptr[BT_MAX];
                };
        };
#ifndef __x86_64__
        unsigned pad;
#endif
};

#ifdef WINDOWS
/* For documentation purposes only */
struct mallinfo
{
        /* Total space allocated with sbrk in all threads */
        int arena;

        /* Number of ordinary (non-slab and non-mmap) allocations */
        int ordblks;

        /* Number of blocks in this threads slab */
        int smblks;

        /* Number of mmaped chunks in our thread */
        int hblks;

        /* Number of btree nodes for our thread */
        int hblkhd;

        /* Total (possibly partially) used slab blocks */
        int usmblks;

        /* Total number of free slab blocks */
        int fsmblks;

        /* Total allocated space for this thread including overhead */
        int uordblks;

        /* Total free space for this thread in ordinary mmap region */
        int fordblks;

        /* zero */
        int keepcost;
};
#endif


/* Seperator bitflags */
#define FLG_UNUSED      0x01
#define FLG_LUNUSED     0x02
#define FLG_LSIZE8      0x04
#define FLG_SIZE8       0x08

static int b_leaf(btree *b);

#define SEP_INDEX(b, loc, v) (((unsigned char *) &((b)->bsize[loc]))[v])

/* Index into the zeroth int in bsize[] */
#define b_start(b) SEP_INDEX(b, 0, 0)
#define b_pindex(b) SEP_INDEX(b, 0, 1)
#define b_mask(b) (*(unsigned short*) &SEP_INDEX(b, 0, 2))

#define b_next(b, loc) SEP_INDEX(b, loc, 0)
#define b_prev(b, loc) (b->prev[loc])
#define b_last(b) b_prev(b, 0)
#define b_ptr(b, loc) (b->ptr[(loc) - 1])

typedef union mealloc mealloc;
union mealloc
{
        __extension__ struct
        {
                slist **tail;
                dlist m_list;
        };

        __extension__ struct
        {
                char pad[16];
                btree b;
        };

        /* Prevent compiler warning "no named members" */
        void *dummy;
};

typedef struct sbheader sbheader;
struct sbheader
{
        __extension__ union
        {
                __extension__ struct
                {
                        slist **tail;

                        dlist list;

                        uintptr_t max;

                        unsigned size;

                };

                /* First cache line is mostly read-only */
                char pad[64];
        };

        /* Second cache line is read-write */
        uintptr_t first;

        unsigned used;

#ifndef __x86_64__
        u64b dummy;     /* padding to get right alignment */
#endif

        /* This needs to be 16 byte aligned */
        void *data;
};

/* 64k block of pointers to free blocks */
typedef struct freesb freesb;
struct freesb
{
        freesb *next;
        unsigned count;

        sbheader *blocks[SLABBMAX];
};

typedef struct atls atls;
struct atls
{
        slist fl[NUM_FL];
        u64b f_mask;

#ifndef __x86_64__
        unsigned dummy; /* padding to get right q8 alignment */
#endif

        /* Note that qs[0] is a miss-aligned btree pointer! */
        dlist qs[NUM_QS];

        __extension__ union
        {
                __extension__ struct
                {
                        /* Overlap with the seperator in bheap */
                        slist btree_freenode;
                        unsigned b_hgt;
                        unsigned b_cnt;
                };

                btree bheap;
        };

        u64b q_mask;

        /* Partially full slabs */
        dlist slab[NUM_SB];

        dlist slab_full;

        freesb *slab_chunk;

        size_t percpu_hash;

        size_t a_alloced;
        size_t s_wanted;

        slist *head;

        dlist bl;

#ifdef DEBUG_LEAK
        int leak_fd;
#endif

        /* Hazard list */
        dlist h_list;
        void *hazard;

        /* Deleted list */
        atls *d_list;

        int fcount;

        char callocable;

        char dummy3[59];

        /* Off by itself to reduce false sharing */
        slist *tail;
};

#ifdef USE_DLL
#define PREFIX(X)       llalloc##X
#else
#ifdef USE_PREFIX
#define PREFIX(X)       llalloc##X
#else
#define PREFIX(X)       X
#endif
#endif

#ifndef DEBUG_ALLOC
#define always_inline inline __attribute__((always_inline))
#else
#define always_inline
#endif

#ifdef WINDOWS

void llmutex_lock(void *l);
void llmutex_unlock(void *l);
int llmutex_trylock(void *l);

typedef void * mutex_t;
#define mutex_lock llmutex_lock
#define mutex_unlock llmutex_unlock
#define mutex_trylock llmutex_trylock
#define MUTEX_INITIALIZER {0}

#ifndef EMU_SBRK
#define EMU_SBRK
#endif

#define set_enomem() _set_errno(ENOMEM)

/* Other functions that need prototypes */
//#if defined( USE_DLL ) || defined( USE_PREFIX )
// #ifdef USE_PREFIX
// void PREFIX(free)(void *p);
// void *PREFIX(malloc)(size_t size);
// void *PREFIX(calloc)(size_t size, size_t n);
// void *PREFIX(realloc)(void *p, size_t size);
// size_t PREFIX(_msize)(void *p);
// void *PREFIX(_expand)(void *p, size_t size);
// 
// /* Hack - indirect calls to crt functions */
// int (* __callnewh)(size_t);
// int (* __newmode)(void);
// #endif /* USE_DLL */


void PREFIX(free)(void *p);
void *PREFIX(malloc)(size_t size);
void *PREFIX(calloc)(size_t size, size_t n);
void *PREFIX(realloc)(void *p, size_t size);
size_t PREFIX(_msize)(void *p);
void *PREFIX(_expand)(void *p, size_t size);



void cfree(void *p);
void *memalign(size_t align, size_t size);
int posix_memalign(void **p, size_t align, size_t size);
void *valloc(size_t size);
void *pvalloc(size_t size);
struct mallinfo mallinfo(void);
int malloc_trim(size_t pad);
int mallopt(int param, int val);
void *PREFIX(_calloc_impl)(size_t n, size_t size, int *errno_tmp);
void PREFIX(_free_nolock)(void *p);
void *PREFIX(_realloc_nolock)(void *p, size_t size);
void *PREFIX(_calloc_nolock)(size_t n, size_t size);
size_t PREFIX(_msize_nolock)(void *p);
static size_t malloc_usable_size(void *p);

void __tlregdtor(void (*)(void *));

#else /* WINDOWS */

static int sys_futex(void *addr1, int op, int val1, struct timespec *timeout, void *addr2, int val3)
{
        return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
}

#define cmpxchg(P, O, N) __sync_val_compare_and_swap((P), (O), (N))

typedef union mutex_t mutex_t;

union mutex_t
{
        unsigned u;
        struct
        {
                unsigned char locked;
                unsigned char contended;
        } b;
};

static void mutex_init(mutex_t *m)
{
        m->u = 0;
}

static void mutex_lock(mutex_t *m)
{
        int i;

        /* Try to grab lock */
        for (i = 0; i < 100; i++)
        {
                if (!xchg_8(&m->b.locked, 1)) return;

                cpu_relax();
        }

        /* Have to sleep */
        while (xchg_32(&m->u, 257) & 1)
        {
                sys_futex(m, FUTEX_WAIT_PRIVATE, 257, NULL, NULL, 0);
        }
}

static void mutex_unlock(mutex_t *m)
{
        DECL_PROF_FUNC;

        int i;

        /* Locked and not contended */
        if ((m->u == 1) && (cmpxchg(&m->u, 1, 0) == 1)) return;

        /* Unlock */
        m->b.locked = 0;

        barrier();

        /* Spin and hope someone takes the lock */
        for (i = 0; i < 200; i++)
        {
                if (m->b.locked) return;

                cpu_relax();
        }

        /* We need to wake someone up */
        m->b.contended = 0;

        sys_futex(m, FUTEX_WAKE_PRIVATE, 1, NULL, NULL, 0);
}

static int mutex_trylock(mutex_t *m)
{
        unsigned c;

        if (m->b.locked) return EBUSY;
        c = xchg_8(&m->b.locked, 1);
        if (!c) return 0;
        return EBUSY;
}

#define MUTEX_INITIALIZER {0}


// static void malloc_stats_aux(int show_nodes);

static gcc_used char dummy1[64];
static pthread_once_t init_once = PTHREAD_ONCE_INIT;
static pthread_key_t death_key;

/*
 * If pthread isn't linked in,
 * have weak replacements for the single-threaded case
 */
#pragma weak pthread_atfork
#pragma weak pthread_key_create
#pragma weak pthread_setspecific
#pragma weak pthread_once

#define set_enomem() (errno = ENOMEM)

#endif /* WINDOWS */

typedef union percpu_list percpu_list;
union percpu_list
{
        __extension__ struct
        {
                mutex_t m;
                freesb *list;
        };
        char pad[64];
};

/* Global thread hazard list */
static cache_align mutex_t h_lock = MUTEX_INITIALIZER;
static dlist h_list = DLIST_INIT(h_list);

static cache_align mutex_t d_lock = MUTEX_INITIALIZER;
static atls *d_list = NULL;

/* List of freed slab blocks */
static cache_align percpu_list *pc_slab;
static size_t cpu_total;

/* sbrk information */
static cache_align mutex_t sb_lock = MUTEX_INITIALIZER;
static cache_align uintptr_t sbrk_start = 0;
static uintptr_t sbrk_size = 0;
static int sbrk_oom = 0;
static unsigned sltotal[NUM_SB];

#ifdef DEBUG_LEAK
#define LEAK_MAX                1024
typedef struct bigallocs bigallocs;
struct bigallocs
{
        void *p;
        size_t size;
};

static bigallocs big_leak[LEAK_MAX] = {{NULL, 0}};
static mutex_t l_lock = MUTEX_INITIALIZER;

#endif

/* Hack */
#define BUILD_ASSERT(C) do {switch (0){case 0:; case (C):;}} while (0)

/* Pre-declares */
static always_inline void *split_node(atls *tl, btree *b, size_t t_size, size_t size);
static void merge_node(atls *tl, void *p);
static int init_sldata(void);
static void slab_free(atls *tl, void *p);
static void local_free(atls *tl, void *p);
static void *local_alloc(atls *tl, size_t size);
static void *slab_alloc_safe(atls *tl, size_t size);
static always_inline void *fast_alloc(atls *tl, size_t size);
static void *slow_alloc(atls *tl, size_t size);
static void atls_merge(atls *tl1, atls *tl2);
static void test_all(atls *tl);
static void *zalloc(atls *tl, size_t size);
void **independent_calloc(size_t n, size_t size, void **chunks);
void **independent_comalloc(size_t n, size_t *sizes, void **chunks);

static inline btree *small_next(btree *b)
{
        return b->data;
}

static inline void set_small_next(btree *b, btree *next)
{
        b->data = next;
}

#ifdef __x86_64__
static inline btree *small_prev(btree *b)
{
        return (btree *) (b->s.prev & ~15);
}

static inline void set_small_prev(btree *b, btree *prev)
{
        uintptr_t p = b->s.prev & 15;
        b->s.prev = p + (uintptr_t) prev;
}
#else
static inline btree *small_prev(btree *b)
{
        return (btree *) b->s.size;
}

static inline void set_small_prev(btree *b, btree *prev)
{
        b->s.size = (unsigned) prev;
}

#endif

static inline void *shift(void *p, size_t s)
{
        return &(((char *)p)[s]);
}

/* Unfortunately, TLS support with mingw is totally broken... so we need to emulate it */
#ifdef WINDOWS
static DWORD tls_index = TLS_OUT_OF_INDEXES;
static atls *get_tls(void)
{
        if (tls_index == TLS_OUT_OF_INDEXES) return NULL;

        return TlsGetValue(tls_index);
}

static void set_tls(atls *tls)
{
        TlsSetValue(tls_index, tls);
}
#else
static __thread__ atls *tls = NULL;
#define get_tls() tls
#define set_tls(T) (tls = (T))
#endif

static size_t cpu_num(void)
{
#ifdef WINDOWS                  
        SYSTEM_INFO info;

        GetSystemInfo(&info);
        return info.dwNumberOfProcessors;
#else
#ifdef SYS_MACOSX
        int num;
        size_t len = sizeof(num);
        if (sysctlbyname("hw.ncpu", &num, &len, NULL, 0)) num = 1;
        return num;
#else
        return sysconf(_SC_NPROCESSORS_ONLN);
#endif  /* SYS_MACOSX */
#endif  /* WINDOWS */
}

/*
 * Emulate sbrk()
 * Assumes we are called under a lock */
#ifdef EMU_SBRK

#ifdef EMU_SBRK_VG
#define SBRK_SIZE (1ULL << 30)
#else /* EMU_SBRK_VG */

#ifdef __x86_64__

/* Default to 32GiB of sbrk space */
#define SBRK_SIZE (1ULL << 37)
#else /* __x86_64__ */

/* Default to 1GiB of sbrk space */
#define SBRK_SIZE (1ULL << 30)
#endif /* __x86_64__ */
#endif /* EMU_SBRK_VG */


static void *sbrk_mmap_base = NULL;
static void *sbrk_mmap_end = 0;
static void init_sbrk(void)
{
        DECL_PROF_FUNC;

        size_t size = SBRK_SIZE;

        while (1)
        {
#ifndef WINDOWS
                sbrk_mmap_base = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS|MAP_NORESERVE, -1, 0);
#else
                /* Allocate address space - but no memory */
                sbrk_mmap_base = VirtualAlloc(NULL, size, MEM_RESERVE, PAGE_READWRITE);
#endif
                if (sbrk_mmap_base == MAP_FAILED)
                {
                        sbrk_mmap_base = NULL;
                        size = size / 2;
                        if (size < 65536) return;
                }
                else
                {
                        sbrk_mmap_end = shift(sbrk_mmap_base, size);

                        return;
                }
        }
}

static void *emu_sbrk(size_t size)
{
        DECL_PROF_FUNC;

        void *out;

        /* Hack - initialize if required */
        if (!size) init_sbrk();
        if (!sbrk_mmap_base) return MAP_FAILED;

        out = sbrk_mmap_base;
        sbrk_mmap_base = shift(sbrk_mmap_base, size);
        if (sbrk_mmap_base >= sbrk_mmap_end)
        {
                sbrk_mmap_base = out;

                return MAP_FAILED;
        }

#ifdef WINDOWS
        /* Enable memory */
        VirtualAlloc(out, size, MEM_COMMIT, PAGE_READWRITE);
#endif

        return out;
}

#define sbrk(S) emu_sbrk(S)
#endif

static inline int is_slab(void *p)
{
        return ((uintptr_t) p - sbrk_start < sbrk_size);
}

static inline void *page_start(void *p)
{
        return (void *) (-PAGESIZE & (uintptr_t) p);
}

static inline size_t page_align(size_t s)
{
        return -PAGESIZE & (s + PAGESIZE - 1);
}

static inline size_t sep_align(size_t s)
{
        /*
         * We want to align on 16byte boundaries
         *
         * 16 -> 16
         * 24 -> 16
         * 25 -> 32
         * 32 -> 32
         */
        /*
         * Then we want to include the extra 8 bytes of last ptr that are free.
         * Finally, we want to include the following sep data.
         */
        /*
         *      0 -> 16
         *  8 -> 16
         *  9 -> 32
         * 16 -> 32
         * 24 -> 32
         * 25 -> 48
         * 32 -> 48
         * 40 -> 48
         */

        return (s + 7 + 16) & ~15;
}

static inline int un_used(btree *b)
{
        return (b->s.bs_offset & FLG_UNUSED);
}

static inline int left_unused(btree *b)
{
        return b->s.bs_offset & FLG_LUNUSED;
}

static inline void set_unused(btree *b, btree *br)
{
        br->s.bs_offset |= FLG_LUNUSED;
        br->s.left = b;

        b->s.bs_offset |= FLG_UNUSED;
}

static inline void set_used(btree *b, size_t size)
{
        btree *br = shift(b, size);
        br->s.bs_offset &= ~FLG_LUNUSED;

#ifdef DEBUG_ALLOC_SLOW
        if (size != b->s.size) errx(1, "size missmatch\n");
#endif

        b->s.bs_offset &= ~FLG_UNUSED;
}

static inline void set_size8(btree *b)
{
        btree *br = shift(b, 16);
        br->s.bs_offset |= FLG_LSIZE8;
        b->s.bs_offset |= FLG_SIZE8;
}

static inline void unset_size8(btree *b)
{
        btree *br = shift(b, 16);
        br->s.bs_offset &= ~FLG_LSIZE8;
        b->s.bs_offset &= ~FLG_SIZE8;
        b->s.size = 16;
        b->s.bs_offset &= 15;
        b->s.bs_offset += (br->s.bs_offset & ~15) - 16;
}

#ifdef __x86_64__
static inline btree *get_q8(atls *tl)
{
        /* Mega hack - align so that we return a btree object pointer to the correct memory locations */
        return shift(&tl->qs[0], -(uintptr_t)8);
}
#else
static inline btree *get_q8(atls *tl)
{
        /* Mega hack - align so that we return a btree object pointer to the correct memory locations */
        return shift(&tl->qs[0], -(uintptr_t)12);
}
#endif

static inline btree *read_left(btree *b)
{
        if (!left_unused(b)) return NULL;
        if (b->s.bs_offset & FLG_LSIZE8) return shift(b, -(uintptr_t)16);
        return b->s.left;
}

static inline mealloc *read_bs(btree *b)
{
        uintptr_t s = b->s.bs_offset & ~15;

#ifdef DEBUG_ALLOC_SLOW
        void *v = shift(b, -s);
        if ((PAGESIZE - 1) & (uintptr_t) v) errx(1, "mealloc misaligned\n");
#endif

        return (mealloc *) shift(b, -s);
}

static void btree_init(btree *b)
{
        /* Init header */
        //b_start(b) = 0;
        b_mask(b) = -1;
        //b_pindex(b) = 0;
        //b_last(b) = 0;
}

static inline void set_sep(btree *b, int size, btree *bo)
{
        unsigned offset = bo->s.bs_offset & ~15;

        /* Store split block offset + size + used indicators */
        b->s.bs_offset = offset + ((uintptr_t) b - (uintptr_t) bo);
        b->s.size = size;
}

#ifdef DEBUG_ALLOC
static void check_sep(btree *b)
{
        btree *br = shift(b, b->s.size);

        if (((uintptr_t) b) & 15) errx(1, "btree misaligned\n");

        if (is_slab(&b->data)) errx(1, "btree slab overlap\n");

        /* Test unused bit */
        if (un_used(b))
        {
                if (b->s.bs_offset & FLG_SIZE8)
                {
                        br = shift(b, 16);

                        if (!(br->s.bs_offset & FLG_LSIZE8)) errx(1, "size8 bit missmatch\n");
                }
                else
                {
                        if (b->s.size & 15) errx(1, "mysize misaligned\n");

                        if ((b->s.size == 16) || (br->s.bs_offset & FLG_LSIZE8)) errx(1, "size8 bit missmatch\n");
                        if (read_left(br) != b) errx(1, "left pointer wrong\n");
                }

                if (!left_unused(br)) errx(1, "Unused flag conflict\n");
        }
        else
        {
                if (b->s.size & 15) errx(1, "mysize misaligned\n");
                if (left_unused(br)) errx(1, "Unused flag conflict\n");
        }
}
#else
#define check_sep(B) ((void) sizeof(B))
#endif

#ifndef WINDOWS
static __attribute__((format (gnu_printf, 2, 3))) void leak_print(atls *tl, const char *format, ...)
{
        char buf[1024];

        va_list ap;

        va_start(ap, format);
        vsnprintf(buf, 1024, format, ap);
        va_end(ap);

#ifdef DEBUG_LEAK
        /* Need tls and leak_fd initialized */
        if (tl)
        {
                if (tl->leak_fd == -1)
                {
                        char buf[1024];
                        int pid = getpid();
                        int tid = syscall(SYS_gettid);

                        snprintf(buf, 1024, "/tmp/leak-%d:%d.txt", pid, tid);

                        tl->leak_fd = open(buf, O_WRONLY | O_CREAT | O_APPEND,  S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
                }

                if (tl->leak_fd != -1)
                {
                        int len = strlen(buf);
                        char *c = buf;

                        while (len)
                        {
                                int out = write(tl->leak_fd, c, len);

                                /* Interrupted - try again */
                                if (out == -1) continue;

                                /* Device is full - stop writing */
                                if (!out) return;

                                len -= out;
                                c += out;
                        }
                }
        }
#else
        /* Shut up compiler warning */
        (void) tl;

        /* Otherwise output to stderr */
        fprintf(stderr, "%s", buf);

#endif
}
#else
#define leak_print(...)
#define malloc_stats_aux(...)
#endif

#ifdef DEBUG_LEAK
static void big_alloced(void *p, size_t size)
{
        int i;

        mutex_lock(&l_lock);
        for (i = 0; i < LEAK_MAX; i++)
        {
                if (big_leak[i].p) continue;

                big_leak[i].p = p;
                big_leak[i].size = size;
                mutex_unlock(&l_lock);
                leak_print(get_tls(), "Big alloc %p %llu\n", p, (unsigned long long) size);
                return;
        }
        mutex_unlock(&l_lock);

        errx(1, "debug leak oom, increase LEAK_MAX\n");
}

static void big_freed(void *p, size_t size)
{
        int i;

        mutex_lock(&l_lock);
        for (i = 0; i < LEAK_MAX; i++)
        {
                if (big_leak[i].p != p) continue;

                if (big_leak[i].size != size) errx(1, "big alloc size missmatch\n");
                big_leak[i].p = NULL;
                mutex_unlock(&l_lock);
                leak_print(get_tls(), "Big free %p %llu\n", p, (unsigned long long) size);
                return;
        }
        mutex_unlock(&l_lock);

        errx(1, "freeing unknown large block %p\n", p);
}

static size_t big_block_size(void *p)
{
        int i;

        mutex_lock(&l_lock);
        for (i = 0; i < LEAK_MAX; i++)
        {
                if (big_leak[i].p != p) continue;
                mutex_unlock(&l_lock);
                return big_leak[i].size;
        }

        mutex_unlock(&l_lock);

        errx(1, "freeing unknown large block %p\n", p);
}

static void test_leak_aux(void)
{
        atls *tl = get_tls();
        if (!tl) return;
        malloc_stats_aux(3);
        leak_print(tl, "Done\n");
        close(tl->leak_fd);
}

static void test_leak(void)
{
        static int count = 0;

        /* Display turned off? */
        if (!DEBUG_LEAK_DISP) return;

        /* Don't bother to be thread safe - it doesn't matter much */
        if (count++ == DEBUG_LEAK_DISP)
        {
                count = 0;
                malloc_stats_aux(3);
        }
}


#else
#define big_alloced(p, size) ((void) (sizeof(p) + sizeof(size)))
#define big_freed(p, size) ((void)(sizeof(p) + sizeof(size)))
#define test_leak_aux()
#define test_leak()
#define big_block_size(p) (sizeof(p) * 0)
#endif


/* Big allocations */
static void *big_alloc_aux(size_t size)
{
        DECL_PROF_FUNC;

        /* Get memory */
#ifndef WINDOWS
        void *p = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);
#else
        void *p = VirtualAlloc(NULL, size, MEM_COMMIT | MEM_RESERVE, PAGE_READWRITE);
#endif

        /* Out of memory */
        if (p == MAP_FAILED) return NULL;

        big_alloced(p, size);

        /* Done */
        return p;
}

#ifdef WINDOWS
int handle_oom(int aize);
#else
#define handle_oom(S)   ((errno = ENOMEM), 0)
#endif


static noinline void *big_alloc(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        size_t psize;

        size_t *p;

        /* This arguement prevents register problems in the fast path */
        (void) tl;

        if (size > TOP_SIZE) goto nomem;

        /* Get real size to allocate */
        psize = page_align(size + SEPSIZE);

        p = big_alloc_aux(psize);

        if (p)
        {
                *p = psize;
                return shift(p, SEPSIZE);
        }

nomem:

        if (handle_oom(size)) return big_alloc(tl, size);

        return NULL;
}

#ifdef WINDOWS
static noinline void big_free_aux(size_t *p)
{
        DECL_PROF_FUNC;

        big_freed(p, *p);

        VirtualFree(p, 0, MEM_RELEASE);
}
#else
static inline void big_free_aux(size_t *p)
{
        big_freed(p, *p);

        munmap(p, *p);
}
#endif


#ifdef DEBUG_ALLOC_SLOW
static void test_queue(atls *tl)
{
        slist *q;

        btree *b;

        /* Scan incoming queue, looking for corruption */
        for (q = tl->head; q; q = q->next)
        {
                /* Ignore slab nodes */
                if (is_slab(q)) continue;

                if (((uintptr_t) q) & 15) errx(1, "incoming queue corrupted\n");

                b = CONTAINER(btree, data, q);

                if (un_used(b)) errx(1, "queue element marked as unused\n");
        }
}
#else
#define test_queue(T) ((void) sizeof(T))
#endif

#ifdef __x86_64__
/* Magic code that converts size to entry in fast-list array */
static always_inline size_t size2fl(ssize_t size)
{
        size_t n = (size / 32);

        /* Make sure we don't overflow */
        if (size == 16) return 0;
        if (size > FAST_64_BIN) return NUM_FL - 1;

        n = n * n * n;

        return flsq(n);
}
#else
/* Magic code that converts size to entry in fast-list array */
static inline size_t size2fl(ssize_t size)
{
        size_t n;

        /* 32 bit version uses old floating point instructions */
        union di
        {
                double d;
                unsigned u2[2];
        } di;

        di.d = size + 40;

        n = (di.u2[1] >> 18) - 4115;

        /* Make sure we don't overflow */
        if (n >= NUM_FL) n = NUM_FL - 1;

        return n;
}
#endif


/* Add to previous list - but don't set flag */
static always_inline void fast_add(atls *tl, btree *b, size_t n)
{
        slist_add(&tl->fl[n], &b->list);
        tl->f_mask |= 1ULL << n;
}

/* Add to free lists */
static always_inline void fast_free(atls *tl, btree *b, size_t ms)
{
        size_t n = size2fl(ms);

#ifdef DEBUG_ALLOC_SLOW
        if (un_used(b)) errx(1, "fast_free() needs used node\n");
        if (b->s.size != ms) errx(1, "fast_free size wrong\n");
#endif

        fast_add(tl, b, n);
}

static int scan_queue(atls *tl, slist **qh, size_t wanted)
{
        DECL_PROF_FUNC;

        slist *q, *qn, *qp = NULL;

        btree *b;

        size_t msize;
        int flag = 0;

        /* Size wanted */
        tl->s_wanted = wanted;

        /* Scan incoming queue, freeing as we go */
        for (q = *qh; q; q = qn)
        {
#ifdef DEBUG_ALLOC_SLOW
                if (!is_slab(q))
                {
                        if (((uintptr_t) q) & 15) errx(1, "incoming queue corrupted\n");
                }
#endif

                qn = q->next;
                qp = q;
                if (qn)
                {
                        if (is_slab(q))
                        {
                                slab_free(tl, q);
                        }
                        else
                        {
                                merge_node(tl, q);
                        }

                        flag = 1;
                }
        }

        *qh = qp;

        /* Reset size wanted */
        tl->s_wanted = 0;

        /*
         * Make sure the last node isn't taking up too much room.
         * Not that a slab node could only take up a max of SB_MAX bytes.
         * (They aren't splittable anyway)
         */
        if (is_slab(qp)) return flag;

        b = CONTAINER(btree, data, qp);

        msize = b->s.size;

        /* Don't split if too small */
        if (msize <= (1 << 16)) return flag;

        /* Make the head node take up less room.  Also, size 32 is faster than 16. */
        split_node(tl, b, msize, 32);

        return 1;
}


#ifdef DEBUG_ALLOC_SLOW

static void test_node(atls *tl, btree *b)
{
        mealloc *m = read_bs(b);

        if (tl != get_tls()) errx(1, "tls incorrect\n");
        if (m->tail != &tl->tail) errx(1, "node owner wrong\n");
}

/* Test fast list constraints */
static void test_fast_lists(atls *tl)
{
        int i, j;

        //if (tl->fl[63].next) errx(1, "fast list overflow\n");

        for (i = 0; i < NUM_FL; i++)
        {
                slist *p = &tl->fl[i];
                slist *f;

                scan_slist(p, f)
                {
                        btree *b = CONTAINER(btree, list, f);

                        /* Are we a slab node? */
                        if (is_slab(&b->data))
                        {
                                errx(1, "Slab node on fast list\n");
                        }

                        test_node(tl, b);

                        if (un_used(b)) errx(1, "Unused element in fast list\n");
                        check_sep(b);

                        j = size2fl(b->s.size);
                        if ((i != j) && (i != j - 1)) errx(1, "Fast element in wrong bin\n");


                        if (!(((uintptr_t) b ^ (uintptr_t) tl) & ~(PAGESIZE - 1))) errx(1, "tls on fast list!\n");
                        //if (f == tl->head) errx(1, "queue head in fast list\n");

                        if (f->next == f) errx(1, "fast list loop\n");
                }
        }
}
#else
#define test_fast_lists(T) ((void) sizeof(T))
#endif

/* Clear fast-lists */
static void clear_fast(atls *tl)
{
        DECL_PROF_FUNC;

        u64b mask = tl->f_mask;

        /* Anything to do? */
        while (mask)
        {
                size_t n = ffsq(mask);

                slist *p = &tl->fl[n];

                /* Get mask bit */
                mask &= -mask;

                /* Convert to a mask */
                mask = ~mask;

                /* Properly free everything in the list */
                while (p->next)
                {
                        merge_node(tl, slist_rem(p));
                }

                /* Clear bottom bit */
                tl->f_mask &= mask;
                mask = tl->f_mask;
        }
}

/* Hack - same as clear_fast() but free nodes from tl2 into tl1 */
static void fast_merge(atls *tl1, atls *tl2)
{
        size_t n;
        slist *p = tl2->fl;

        /* Anything to do? */
        while (tl2->f_mask)
        {
                n = ffsq(tl2->f_mask);
                p = &tl2->fl[n];

                /* Turn off bit in f_mask, as nothing will be left there */
                tl2->f_mask &= tl2->f_mask - 1;

                /* Properly free everything in the list */
                while (p->next)
                {
                        void *l = slist_rem(p);

                        merge_node(tl1, l);
                }
        }
}

static noinline int reap_dead(atls *tl)
{
        DECL_PROF_FUNC;

        dlist *d;

        atls *tl2, *tl3;

        /* Check without taking mutex */
        if (!d_list) return 0;

        /* Try to get dead thread */
        if (mutex_trylock(&d_lock)) return 0;

        if (!d_list)
        {
                /* Nothing there */
                mutex_unlock(&d_lock);
                return 0;
        }

        /* Grab dead thread */
        tl2 = d_list;
        d_list = tl2->d_list;
        mutex_unlock(&d_lock);

        mutex_lock(&h_lock);

        /* Remove from hazard list */
        dlist_del(&tl2->h_list);

        /* Set flag so that memless free works */
        tl2->h_list.next = NULL;

        /* Merge data + update tail pointers */
        atls_merge(tl, tl2);

        /* Wait for all threads to not point to dead thread */
        scan_list(&h_list, d)
        {
                tl3 = list_entry(atls, h_list, d);

                while (tl3->hazard == &tl2->tail) cpu_relax();
        }

        mutex_unlock(&h_lock);

        /* Scan all final pending */
        scan_queue(tl, &tl2->head, 0);

        /* Free head */
        local_free(tl, tl2->head);

        /* Finally free tls data for dead thread */
#ifdef WINDOWS          
        VirtualFree(page_start(tl2), 0, MEM_RELEASE);
#else
        munmap(page_start(tl2), PAGESIZE);
#endif

        test_all(tl);

        /* Try to free up memory */
        return 1;
}

static void prepend_queue(slist *p, atls *tl, slist ***bs)
{
        DECL_PROF_FUNC;

        slist *tail;

        slist **btail = *bs;
        slist **btold;

        do
        {
                btold = btail;

                /* Make sure we write to the hazard pointer */
                xchg_ptr(&tl->hazard, btail);

                /* Has it changed while we were writing to our hazard pointer? */
                btail = *bs;
        }
        while (btold != btail);

        p->next = NULL;
        tail = xchg_ptr(btail, p);
        tail->next = p;

        barrier();

        tl->hazard = NULL;
}

static void destroy_tls(void *dummy)
{
        DECL_PROF_FUNC;

        atls *tl = get_tls();

        (void) dummy;

        test_all(tl);

        test_leak_aux();

        /*
         * Make sure that any recursion via signals or other
         * pthread_key destructors will reset this handler.
         */
#ifdef WINDOWS
        set_tls((atls *) 1);
#else
        set_tls(NULL);
#endif

        /* The above line isn't allowed to be moved inside the lock due to possible signals */
        barrier();

        /* Add to dead list */
        mutex_lock(&d_lock);
        tl->d_list = d_list;
        d_list = tl;
        mutex_unlock(&d_lock);
}


/* Convert a pointer into a 32bit random number */
static unsigned rnd_ptr(void *p)
{
        u64b rnd_seed = (uintptr_t) p;
        rnd_seed *= 7319936632422683443ULL;
        rnd_seed ^= rnd_seed >> 32;
        rnd_seed *= 7319936632422683443ULL;
        rnd_seed ^= rnd_seed >> 32;

        /* Truncate to 32 bits */
        return rnd_seed;
}

/*
 * Pick a random offset from p into a region of size total
 * to fit an object of size size.
 *
 * Return a pointer to the object
 */
static void *rnd_offset(void *p, size_t total, size_t size)
{
        u64b slack_space = total - size;

        unsigned rng = rnd_ptr(p);

        unsigned offset = (slack_space * rng) >> 32;

        /* Keep 16-byte alignment */
        offset &= ~15;

        return shift(p, offset);
}

static atls *init_atls(atls *tl)
{
        int i;

        mealloc *m;
        btree *b, *br;

        btree *q8;

        /* Init lists */
        dlist_init(&tl->bl);

        /* queue 0 is taken by size 8 small-list */
        for (i = 1; i < NUM_QS; i++)
        {
                dlist_init(&tl->qs[i]);
        }

        /* Init small list */
        q8 = get_q8(tl);

#ifdef DEBUG_ALLOC_SLOW
        /* Btree needs to be correctly aligned */
        if (((uintptr_t) q8) & 15) errx(1, "q8 misaligned\n");
#endif

        set_small_next(q8, q8);
        set_small_prev(q8, q8);

        /* Init slabs */
        for (i = 0; i < NUM_SB; i++)
        {
                dlist_init(&tl->slab[i]);
        }
        dlist_init(&tl->slab_full);

        tl->percpu_hash = rnd_ptr(tl);

        /* Init btree */
        btree_init(&tl->bheap);

        /* Need a maximum of 2 nodes at this point */
        tl->b_hgt = 2;

#ifdef DEBUG_LEAK
        tl->leak_fd = -1;
#endif

        /* Grab initial allocation */
        m = big_alloc_aux(PAGESIZE);
        if (!m)
        {
                set_tls(NULL);
#ifdef WINDOWS
                VirtualFree(page_start(tl), 0, MEM_RELEASE);
#else
                munmap(page_start(tl), PAGESIZE);
#endif
                return NULL;
        }

        /* Keep track of total allocations */
        tl->a_alloced = PAGESIZE;

        /* Fill in header */
        dlist_add(&tl->bl, &m->m_list);

        m->tail = &tl->tail;

        b = &m->b;

        /* Create left seperator */
        b->s.size = PAGESIZE - HEADERSIZE;
        b->s.bs_offset = 16;

        /* Position of right seperator */
        br = shift(b, b->s.size);

        /* Create right seperator */
        br->s.bs_offset = PAGESIZE - SEPSIZE;
        split_node(tl, b, b->s.size, SEPSIZE);

        /* Make queue */
        tl->head = (void *) &b->data;
        tl->tail = tl->head;
        tl->head->next = NULL;

        /* Add to hazard list */
        mutex_lock(&h_lock);
        dlist_add(&h_list, &tl->h_list);
        mutex_unlock(&h_lock);

        return tl;
}

#ifndef WINDOWS

static void prepare_fork(void)
{
        size_t i;

        /* Stablize slab */
        for (i = 0; i < cpu_total; i++)
        {
                mutex_lock(&pc_slab[i].m);
        }

        /* Stablize hazard list */
        mutex_lock(&h_lock);

        /* Stabilize dead list */
        mutex_lock(&d_lock);

        /* Stablize sbrk */
        mutex_lock(&sb_lock);
}

static void parent_fork(void)
{
        size_t i;

        /* Done with sbrk */
        mutex_unlock(&sb_lock);

        /* Done with dead list */
        mutex_unlock(&d_lock);

        /* Done with hazard list */
        mutex_unlock(&h_lock);

        /* Done with slab */
        for (i = 0; i < cpu_total; i++)
        {
                mutex_unlock(&pc_slab[i].m);
        }
}

static void child_fork(void)
{
        size_t i;

        /* Clean up sb_lock in child */
        mutex_init(&sb_lock);

        /* Clean up d_lock in child */
        mutex_init(&d_lock);

        /* Clean up h_lock in child */
        mutex_init(&h_lock);

        /* Clean up slab locks in child */
        for (i = 0; i < cpu_total; i++)
        {
                mutex_unlock(&pc_slab[i].m);
        }

        /*
         * Wipe hazard list as the other threads no longer exist
         * This leaks memory, but we can't help it,
         * as the other threads may be concurrently modifying internal
         * data structures now.
         */
        dlist_init(&h_list);

        /* We are the only member */
        dlist_add(&h_list, &tls->h_list);
}

/*
 * Initialize things.
 * Unfortunately, we don't have a failure return value so we must die instead.
 */
static void init_handler(void)
{
        int res;

        void *v;

        /* Init sbrk information */
        v = sbrk(0);
        sbrk_start = (uintptr_t) v;

        /* Add a fork handler */
        if (pthread_atfork)
        {
                res = pthread_atfork(prepare_fork, parent_fork, child_fork);
                if (res) errx(1, "pthread_atfork failed\n");
        }

        /* Create thread death key */
        if (pthread_key_create)
        {
                res = pthread_key_create(&death_key, destroy_tls);
                if (res) errx(1, "pthread_key_create() failed\n");
        }

        if (init_sldata())
        {
                errx(1, "Failed to allocate enough memory to initialize slab\n");
        }

#ifdef DEBUG_LEAK
        atexit(test_leak_aux);
#endif
}

static atls *init_tls(void)
{
        DECL_PROF_FUNC;

        atls *tl;

        /* Can we use a dead thread's tls data? */
        if (d_list)
        {
                mutex_lock(&d_lock);

                if (d_list)
                {
                        /* Grab from death list */
                        tl = d_list;
                        d_list = tl->d_list;
                        mutex_unlock(&d_lock);

                        set_tls(tl);

                        /* Init my thread destructor */
                        if (pthread_setspecific) pthread_setspecific(death_key, tl);

                        test_all(tl);

                        /* Done */
                        return tl;
                }
                mutex_unlock(&d_lock);
        }

        /* Hack - use a full page for it */
        tl = mmap(NULL, PAGESIZE, PROT_READ | PROT_WRITE, MAP_PRIVATE|MAP_ANONYMOUS, -1, 0);

        /* Out of memory */
        if (tl == MAP_FAILED) goto nomem;

        /* Randomly cache colour the tls data */
        tl = rnd_offset(tl, PAGESIZE, sizeof(atls));

        /* Save pointer for later memory calls */
        set_tls(tl);

        /* Make sure that we can always allocate two btree nodes from within itself */
        BUILD_ASSERT(NUM_QS * 8 + 16 >= sizeof(btree) * 2);

        /* Make sure atls isn't too big */
        BUILD_ASSERT(sizeof(atls) <= PAGESIZE);

        /* Make sure btree nodes fit in the slab */
        BUILD_ASSERT(sizeof(btree) <= SB_MAX);

        /* Hack - we should use the rest of the space to init the heap... */
        tl = init_atls(tl);
        if (!tl) goto nomem;

        /*
         * Init handler.
         * Note that this can allocate memory, so needs to be done last
         */
        if (pthread_once)
        {
                pthread_once(&init_once, init_handler);
        }
        else
        {
                /* Since there are no threads... */
                if (!sbrk_start) init_handler();
        }

        /* Init my thread destructor */
        if (pthread_setspecific) pthread_setspecific(death_key, tl);

        test_all(tl);

        return tl;

nomem:
        set_enomem();
        return NULL;
}
#else /* WINDOWS */


#ifdef USE_DLL

typedef struct patch patch;
struct patch
{
        const char *name;
        void *func;
};


#define PATCH_FUNC(X)\
        {#X, (void *) ((uintptr_t) llalloc##X)}

static patch patch_list[] =
{
        PATCH_FUNC(free),
        PATCH_FUNC(malloc),
        PATCH_FUNC(calloc),
        PATCH_FUNC(realloc),
        PATCH_FUNC(_msize),
        PATCH_FUNC(_expand),
        PATCH_FUNC(_free_nolock),
        PATCH_FUNC(_realloc_nolock),
        PATCH_FUNC(_calloc_nolock),
        PATCH_FUNC(_msize_nolock),
        {NULL, NULL}
};

#define JMP_OP  0xE9
#define CALL_OP 0xE8
#define JMP_OFFSET(P1, P2)      (((uintptr_t)(P1)) - ((uintptr_t)(P2)) - 5)

/* Follow a call to its target */
static void *follow_call(void *p)
{
        int target;

        /* Are we pointing to a jump? */
        if ((*(unsigned char *)p) != CALL_OP) return NULL;

        target = *(int *) shift(p, 1);

        return (void *) shift(p, (uintptr_t) target + 5);
}

/* Find a jump in a dumb manner */
static void *find_call(void *p)
{
        while ((*(unsigned char *) p) != CALL_OP)
        {
                p = shift(p, 1);
        }

        return p;
}

static void patch_function(void *func, void *my_func)
{
        MEMORY_BASIC_INFORMATION mbi;

        /* Make code read/write */
        VirtualQuery(func, &mbi, sizeof(mbi));
        VirtualProtect(mbi.BaseAddress, mbi.RegionSize,
                                                PAGE_EXECUTE_READWRITE, &mbi.Protect);

        /* Patch in a jmp to our routine */
        *(unsigned char *) func = JMP_OP;
        *(unsigned *) shift(func, 1) = JMP_OFFSET(my_func, func);

        /* Reset code permissions */
        VirtualProtect(mbi.BaseAddress, mbi.RegionSize, mbi.Protect, &mbi.Protect);
}

static void *init_crt_funcs(void)
{
        FARPROC func_f;
        patch *p;
        void *f;

        HMODULE library = GetModuleHandle("MSVCR90.DLL");
        if (!library) return NULL;

        func_f = GetProcAddress(library, "_callnewh");
        if (!func_f) return NULL;
        __callnewh = (typeof(__callnewh)) func_f;

        func_f = GetProcAddress(library, "?_query_new_mode@@YAHXZ");
        if (!func_f) return NULL;
        __newmode = (typeof(__newmode)) func_f;

        for (p = patch_list; p->name; p++)
        {
                func_f = GetProcAddress(library, p->name);
                if (!func_f) continue;

                patch_function((void *) (uintptr_t) func_f, p->func);
        }

        func_f = GetProcAddress(library, "calloc");
        f = (void *) (uintptr_t) func_f;

        /* Not here... don't crash */
        if (!f) goto out;

        /* Get pointer to _calloc_impl() */
        f = find_call(f);
        f = follow_call(f);

        /* Finally patch _calloc_impl */
        patch_function(f, (void *) (uintptr_t) llalloc_calloc_impl);

out:

        /* Success */
        return (void*) 1;
}


#endif

void lldebug_hook(void);
static atls *init_tls(void)
{
        DECL_PROF_FUNC;

        atls *tl;

        static void *init = (void *) 1;
        void *first = xchg_ptr(&init, NULL);

        if (!first)
        {
                /* We've already died - use free_nomem() */
                if (get_tls() == (atls *) 1) return NULL;

                /* Can we use a dead thread's tls data? */
                if (d_list)
                {
                        mutex_lock(&d_lock);

                        if (d_list)
                        {
                                /* Grab from death list */
                                tl = d_list;
                                d_list = tl->d_list;
                                mutex_unlock(&d_lock);

                                set_tls(tl);

                                test_all(tl);

                                /* Undocumented crt function */
                                __tlregdtor(destroy_tls);

                                /* Done */
                                return tl;
                        }
                        mutex_unlock(&d_lock);
                }
        }
        else
        {
                void *v = sbrk(0);

                /* Init slab */
                if (init_sldata())
                {
                        init = (void *) 1;
                        return NULL;
                }

                tls_index = TlsAlloc();
                if (tls_index == TLS_OUT_OF_INDEXES)
                {
                        /* We can't handle this */
                        init = (void *) 1;
                        return NULL;
                }

#ifdef USE_DLL
                /* Initialize function pointers */
                if (!init_crt_funcs())
                {
                        /* Doesn't work... fail and bail out of dll init */
                        init = (void *) 1;
                        return NULL;
                }
#endif
                /* Init sbrk information */
                sbrk_start = (uintptr_t) v;
        }

        /* Hack - use a full page for it */
        tl = VirtualAlloc(NULL, PAGESIZE, MEM_COMMIT | MEM_RESERVE, PAGE_READWRITE);

        /* Out of memory */
        if (!tl) goto nomem;

        /* Randomly cache colour the tls data */
        tl = rnd_offset(tl, PAGESIZE, sizeof(atls));

        /* Save pointer for later memory calls */
        set_tls(tl);

        /* Make sure that we can always allocate two btree nodes from within itself */
        //BUILD_BUG(NUM_QS * 8 + 16 >= sizeof(btree) * 2);

        /* Hack - we should use the rest of the space to init the heap... */
        tl = init_atls(tl);
        if (!tl) goto nomem;

        /* Undocumented crt function */
        __tlregdtor(destroy_tls);

        test_all(tl);

        return tl;

nomem:
        /* Try again if possible */
        if (handle_oom(PAGESIZE * 2)) return init_tls();

        return NULL;
}

#ifdef USE_DLL
BOOL DllMain(HINSTANCE h, DWORD reason, LPVOID reserved);
BOOL DllMain(HINSTANCE h, DWORD reason, LPVOID reserved)
{
        /* Silence compiler warnings */
        (void) h;
        (void) reserved;

        /* Init the memory allocator */
        if ((reason == DLL_PROCESS_ATTACH) || (reason == DLL_THREAD_ATTACH))
        {
                if (!init_tls()) return 0;
        }
#ifdef DEBUG_PROFILE
        else if(reason == DLL_PROCESS_DETACH)
        {
                ll_print_prof();
        }
#endif

        return 1;
}
#endif /* USE_DLL */

#endif /* WINDOWS */

#ifdef DEBUG_ALLOC_SLOW

/* Get node previous to loc in b */
static void test_btree_linked(btree *b, int loc)
{
        int i, j = 0;

        if (b_leaf(b)) errx(1, "No previous!\n");

        for (i = b_start(b); i != loc; i = b_next(b, i))
        {
                j++;

                if (j > BT_MAX) errx(1, "Btree node loop!\n");
        }
}

static void test_in_btree(atls *tl, btree *b)
{
        btree *bp;

        if (!b_leaf(b)) errx(1, "Unused btree object that is not a leaf\n");

        while (b->parent)
        {
                bp = b->parent;

                if (b_ptr(bp, b_pindex(b)) != b) errx(1, "Parent doesn't own %p\n", (void *) b);
                if (!bp->bsize[b_pindex(b)]) errx(1, "Parent link broken\n");

                test_btree_linked(bp, b_pindex(b));

                b = bp;
        }

        if (&tl->bheap != b) errx(1, "Heap doesn't own %p\n", (void *) b);
}

#ifdef UNUSED_FUNC
static int is_fast_node(atls *tl, btree *b)
{
        size_t bin = size2fl(b->s.size);
        slist *f;

        scan_slist(&tl->fl[bin], f)
        {
                /* Found it? */
                if (f == &b->list) return 1;
        }

        /* Didn't find it */
        return 0;
}
#endif /* UNUSED_FUNC */

static void test_blocks(atls *tl)
{
        mealloc *m;
        dlist *d;

        btree *b;

        size_t size;

        if (tl->bl.next->prev != &tl->bl) errx(1, "Block list corrupt\n");

        /* Scan blocks */
        scan_list(&tl->bl, d)
        {
                m = list_entry(mealloc, m_list, d);

                if (d->next->prev != d) errx(1, "Block list corrupt\n");

                /* Scan seps for this block */
                for (b = &m->b;; b = shift(b, size))
                {
                        if (b < &m->b) errx(1, "Node before block start!\n");

                        if (b->s.bs_offset & FLG_SIZE8)
                        {
                                size = 16;
                        }
                        else
                        {
                                size = b->s.size;

                                if (!size) break;

                                if (shift(b, -(uintptr_t)(b->s.bs_offset & ~15)) != m) errx(1, "Block back link broken\n");

                                if (read_bs(b) != m) errx(1, "Block start corrupted\n");
                        }

                        check_sep(b);

                        if ((size > QS_MAX) && un_used(b)) test_in_btree(tl, b);
                }
        }
}
#else
#define test_blocks(T) ((void) sizeof(T))
#endif

/* Medium allocations */

#ifdef DEBUG_ALLOC_SLOW

static unsigned test_btree_aux(atls *tl, btree *b, unsigned lsize)
{
        btree *bn;
        int n = b_start(b);
        int i = 0;
        unsigned ssize = lsize;

        unsigned short msk = -1;

        /* Size of node can be incorrect if splitting not possible */
        if (n && b->parent && !is_slab(b) && (b->s.size < sizeof(btree)))
        {
                errx(1, "Btree nodesize wrong\n");
        }

        while (n)
        {
                bn = b_ptr(b, n);

                i++;

                if (bn->parent != b) errx(1, "Btree parent incorrect\n");
                if (b_pindex(bn) != n) errx(1, "Btree p_index incorrect\n");

                if (b_mask(b) & (1 << (n - 1))) errx(1, "Used btree node marked free\n");
                msk -= (1 << (n - 1));

                /* Scan lower */
                ssize = test_btree_aux(tl, bn, ssize);

                if (b->bsize[n] < ssize) errx(1, "Btree size misordered\n");
                ssize = b->bsize[n] & ~0xff;

                if (b_leaf(bn))
                {
                        if (bn->s.size*16 != ssize) errx(1, "Btree leaf size wrong\n");

                        if (!un_used(bn)) errx(1, "Btree leaf marked used!\n");
                }
                else if (!is_slab(bn) && un_used(bn)) errx(1, "Btree node marked unused!\n");

                if (b_prev(b, b_next(b, n)) != n) errx(1, "prev link broken\n");

                n = b_next(b, n);

                if (i > BT_MAX) errx(1, "Btree node loop!\n");
        }

        /* Leaf node? */
        if (!i) return ssize;

        if (msk != b_mask(b)) errx(1, "Btree free mask missmatch\n");

        if (b->parent && (i <= 3)) errx(1, "Btree has too few children\n");

        return ssize & ~0xff;
}

static void test_btree(atls *tl)
{
        if ((tl->b_hgt > 100) || (tl->b_cnt > 100)) errx(1, "btree height corrupt\n");

        test_btree_aux(tl, &tl->bheap, 0);
}
#else
#define test_btree(T) ((void) sizeof(T))
#endif

static char btree_count(btree *b)
{
        int x = b_mask(b);

        /* See Wikipedia for this algorithm for popcount */
        int m1 = 0x5555;
        int m2 = 0x3333;
        int m4 = 0x0f0f;

        /* Put counts into pairs of bits */
        x -= (x >> 1) & m1;

        /* 4 bit counts */
        x = (x & m2) + ((x >> 2) & m2);

        /* Make 8bit counts */
        x = (x + (x >> 4)) & m4;

        return 16 - (x + (x >> 8));
}

static inline int btree_alloc(btree *b)
{
        int loc = ffsu(b_mask(b));

        b_mask(b) &= ~(1 << loc);

        return loc + 1;
}

static inline void btree_free(btree *b, int loc)
{
        b_mask(b) |= 1 << (loc - 1);
}

static int b_leaf(btree *b)
{
        /* Check to see if there are no children */
        return !b_start(b);
}

static inline unsigned btree_ssize(btree *b, int loc)
{
        return b->bsize[loc] & ~0xff;
}

static inline void btree_update_daughter(btree *bp, btree *b, int loc)
{
        b_ptr(bp, loc) = b;
        b->parent = bp;
        b_pindex(b) = loc;
}


/* Update my parent with my new size */
static void btree_update_psize(btree *b, unsigned ssize)
{
        int bpi = b_pindex(b);
        btree *bp;

        /* Update parent size value */
        for (bp = b->parent; bp; bp = bp->parent)
        {
                bp->bsize[bpi] &= 0xff;
                bp->bsize[bpi] += ssize;

                /* Are we done with the chain of updates? */
                if (b_next(bp, bpi)) break;

                bpi = b_pindex(bp);
        }
}

/* Forward declare */
static void btree_node_del(atls *tl, btree *b, int loc);

static void btree_merge_aux(atls *tl, btree *bl, btree *br)
{
        int i, j, k;

        int ip, pi;

        int next;

        unsigned ssize;

        btree *bp;

        int bcl, bcr;

#ifdef DEBUG_ALLOC_SLOW 
        if (!bl->parent || !br->parent) errx(1, "Trying to merge heap top\n");
        if (bl->parent != br->parent) errx(1, "Trying to merge two nodes with different parents\n");
#endif

        bcl = btree_count(bl);
        bcr = btree_count(br);

        /* Move some from neighbour to me */
        if (bcr + bcl > BT_MAX)
        {
                if (bcr > bcl)
                {
                        /* Silence compiler warning */
                        next = 0;

                        /* Move some nodes from br to bl */
                        ip = b_last(bl);

                        for (j = bcr / 3, k = b_start(br); j; k = next, j--)
                        {
                                next = b_next(br, k);
                                i = btree_alloc(bl);

                                /* Add in new node */
                                bl->bsize[i] = br->bsize[k];
                                b_next(bl, ip) = i;
                                b_prev(bl, i) = ip;
                                btree_update_daughter(bl, b_ptr(br, k), i);
                                ip = i;

                                /* Remove old node */
                                btree_free(br, k);
                        }
                        b_next(bl, ip) = 0;
                        ssize = bl->bsize[ip];
                        b_last(bl) = ip;

                        b_start(br) = next;
                        b_prev(br, next) = 0;

                        /* Notify parent of my new size */
                        btree_update_psize(bl, ssize);

                        return;
                }

                /* Scan 2/3rds of the way through bl */
                for (j = bcl / 3, ip = b_last(bl); j; ip = b_prev(bl, ip), j--);

                k = b_start(br);
                ssize = btree_ssize(bl, ip);
                j = b_next(bl, ip);
                b_next(bl, ip) = 0;
                b_last(bl) = ip;

                /* Copy remainder to br, deleting as we go */
                for (ip = 0; j; j = next)
                {
                        next = b_next(bl, j);
                        i = btree_alloc(br);

                        /* Add in new node */
                        br->bsize[i] = bl->bsize[j];
                        b_next(br, ip) = i;
                        b_prev(br, i) = ip;
                        ip = i;
                        btree_update_daughter(br, b_ptr(bl, j), i);

                        /* Remove old node */
                        btree_free(bl, j);
                }

                /* link to remainder of nodes in br */
                b_next(br, ip) = k;
                b_prev(br, k) = ip;

                /* Notify parent of my new size */
                btree_update_psize(bl, ssize);

                return;
        }

        /* merge bl into br and delete bl */
        ip = 0;
        k = b_start(br);
        for (j = b_start(bl); j; j = b_next(bl, j))
        {
                i = btree_alloc(br);

                /* Add in new node */
                br->bsize[i] = bl->bsize[j];
                b_next(br, ip) = i;
                b_prev(br, i) = ip;
                ip = i;

                btree_update_daughter(br, b_ptr(bl, j), i);
        }

#ifdef DEBUG_ALLOC_SLOW
        if (!ip) errx(1, "Empty left node?\n");
#endif

        b_next(br, ip) = k;
        b_prev(br, k) = ip;

        /* Save these so we can delete */
        bp = bl->parent;
        pi = b_pindex(bl);

        /* Delete this node when done */
        local_free(tl, &bl->data);

        /* Delete bl */
        btree_node_del(tl, bp, pi);

        /* Tail recursion */
}

static noinline void btree_node_del_aux(atls *tl, btree *b, btree *bp)
{
        size_t prev, next;

        int pi = b_pindex(b);

        int i;

#ifdef DEBUG_ALLOC_SLOW
        if (!pi) errx(1, "Corrupted leaf\n");
#endif

        /* Rebalance if possible */
        next = b_next(bp, pi);
        if (next)
        {
                /* Merge with next node */
                btree_merge_aux(tl, b, b_ptr(bp, next));

                return;
        }

        prev = b_prev(bp, pi);
        if (prev)
        {
                /* Merge with previous node */
                btree_merge_aux(tl, b_ptr(bp, prev), b);

                return;
        }

        /* Just me here? */
#ifdef DEBUG_ALLOC_SLOW
        if (bp != &tl->bheap) errx(1, "Invalid node count\n");
#endif

        /* Move my data to the top of the btree */
        b_start(bp) = b_start(b);
        b_last(bp) = b_last(b);
        b_mask(bp) = b_mask(b);

        /* Init alloced list */
        for (i = b_start(b); i; i = b_next(b, i))
        {
                bp->bsize[i] = b->bsize[i];
                bp->prev[i] = b->prev[i];
                btree_update_daughter(bp, b_ptr(b, i), i);
        }

        /* Btree is shorter */
        tl->b_hgt--;

        /* Delete this node when done */
        local_free(tl, &b->data);

        /* Prevent having too many spare nodes which can cause fragmentation */
        if (tl->b_hgt < tl->b_cnt)
        {
                /* Pop off the extra node */
                void *st = slist_rem(&tl->btree_freenode);
                b = list_entry(btree, data, st);

                /* Delete it */
                local_free(tl, &b->data);
                tl->b_cnt--;
        }
}

/* Delete node at location loc */
static void btree_node_del(atls *tl, btree *b, int loc)
{
        size_t prev = b_prev(b, loc);
        size_t next = b_next(b, loc);

        btree *bp = b->parent;

        b_next(b, prev) = next;
        b_prev(b, next) = prev;

        /* Add to free list */
        btree_free(b, loc);

        /* If top - am done */
        if (!bp) return;

        /* Was last? */
        if (!next)
        {
                /* Update parent size (we know there must be at least one other node) */
                btree_update_psize(b, btree_ssize(b, prev));
        }

        /* Still not empty enough btree_count(b) > 3) (faster than popcount) */
        if (b_next(b, b_next(b, b_next(b, b_start(b))))) return;

        btree_node_del_aux(tl, b, bp);
}

static inline btree *btree_search(atls *tl, unsigned ssize)
{
        btree *b = &tl->bheap;
        size_t i = b_start(b);

        while (i)
        {
                /* Scan level below? */
                if (b->bsize[i] < ssize)
                {
                        i = b_next(b, i);
                }
                else
                {
                        b = b_ptr(b, i);
                        i = b_start(b);
                }
        }

        return b;
}

/* Return node of size ssize if possible */
static btree *btree_remove(atls *tl, unsigned ssize)
{
        btree *b = btree_search(tl, ssize);

        /* Nothing? */
        if (b == &tl->bheap) return NULL;

        /* Disconnect it */
        btree_node_del(tl, b->parent, b_pindex(b));

        return b;
}

/* Find space for node of size ssize */
static btree *btree_find(atls *tl, unsigned ssize, int *ipv)
{
        btree *b = btree_search(tl, ssize);
        btree *bp = &tl->bheap;

        if (b != bp)
        {
                bp = b->parent;
                *ipv = b_prev(bp, b_pindex(b));
                return bp;
        }

        /* Nothing in btree? */
        if (b_leaf(b)) return bp;

        /* We are larger than anything */
        do
        {
                /* Scan level below */
                b = b_ptr(b, (int) b_last(b));
        }
        while (!b_leaf(b));

        *ipv = b_pindex(b);

        return b->parent;
}

/* Cleanup - make sure we have enough temp nodes */
static noinline void btree_cleanup(atls *tl)
{
        /* First try to use slab allocations to prevent fragmentation */
        while (tl->b_hgt > tl->b_cnt)
        {
                slist *s = slab_alloc_safe(tl, sizeof(btree) - SEPSIZE);

                /* Fall back to in-btree allocations */
                if (!s) goto use_btree;

                slist_add(&tl->btree_freenode, s);
                tl->b_cnt++;
        }

        return;

use_btree:

        /* In-btree allocation by manual memory manipulation */
        while (tl->b_hgt > tl->b_cnt)
        {
                size_t num, msize;

                unsigned i;

                btree *br;

                unsigned offset;

                /* Get smallest allocation in btree */
                btree *b = btree_remove(tl, 0);

                msize = b->s.size;

                /* How many nodes can fit? */
                num = msize / sizeof(btree);

                /* As many as required */
                if (num > tl->b_hgt - tl->b_cnt) num = tl->b_hgt - tl->b_cnt;

                /* Prevent recursion by always adding at least one node */
                if (num < 1) num = 1;

                /* We are using this */
                set_used(b, msize);
                offset = b->s.bs_offset & ~15;

                for (i = 0; i < num; i++)
                {
                        br = shift(b, sizeof(btree));
                        b->s.size = sizeof(btree);
                        offset += sizeof(btree);
                        if (i != num - 1) br->s.bs_offset = offset;
                        msize -= sizeof(btree);

                        slist_add(&tl->btree_freenode, &b->list);
                        b = br;
                }

                tl->b_cnt += num;

                /* Any room left? */
                if (!msize) continue;

                /* Free remaining fragment */
                b->s.size = msize;
                b->s.bs_offset = offset;
                fast_free(tl, b, msize);
        }
}

static void btree_node_insert(atls *tl, btree *b, int loc, unsigned ssize, btree *node);

/* Split myself */
static noinline void btree_node_insert_aux(atls *tl, btree *b, int loc, unsigned ssize, btree *node)
{
        btree *tmp, *tmp2, *bp;

        unsigned bsize = 0, tsize;

        int i, j, bn;

        void *st;

        size_t new;
        int inserted = 0;
        size_t next;

        /* New node */
        st = slist_rem(&tl->btree_freenode);
        tmp = list_entry(btree, data, st);
        tl->b_cnt--;

        /* Clear it */
        memset(&tmp->data, 0, offsetof(btree, prev) - SEPSIZE);

#if 0
        /* Hack - get daughter testing to work */
        tmp->parent = (btree *) 1;
#endif

        /* Special case - insert at start? */
        if (!loc)
        {
                /* Insert at the beginning */
                tmp->bsize[1] = ssize + 2;
                tmp->prev[1] = 0;
                btree_update_daughter(tmp, node, 1);
                inserted = 1;

                /* Copy things below median here */
                for (i = 2, j = b_start(b); i <= BT_MAX/2; i++, j = bn)
                {
                        bn = b_next(b, j);

                        tmp->bsize[i] = btree_ssize(b, j) + i + 1;
                        tmp->prev[i] = i - 1;
                        btree_update_daughter(tmp, b_ptr(b, j), i);

                        btree_free(b, j);
                }
        }
        else
        {
                /* Copy things below median here */
                for (i = 1, j = b_start(b); i <= BT_MAX/2; i++, j = bn)
                {
                        bn = b_next(b, j);

                        tmp->bsize[i] = btree_ssize(b, j) + i + 1;
                        tmp->prev[i] = i - 1;
                        btree_update_daughter(tmp, b_ptr(b, j), i);

                        btree_free(b, j);

                        /* Need to insert new node? */
                        if (j == loc)
                        {
                                i++;
                                tmp->bsize[i] = ssize + i + 1;
                                tmp->prev[i] = i - 1;
                                btree_update_daughter(tmp, node, i);
                                inserted = 1;
                        }
                }
        }
        b_start(b) = j;
        b_prev(b, j) = 0;

        /* Finish initialization of new node */
        b_start(tmp) = 1;
        b_last(tmp) = i - 1;
        b_next(tmp, i - 1) = 0;
        tsize = tmp->bsize[i - 1];
        b_mask(tmp) = -(1 << (i - 1));

        /* Need to insert in remainder? */
        if (!inserted)
        {
                next = b_next(b, loc);

                /* We have space - add it */
                new = btree_alloc(b);

                b->bsize[new] = ssize + next;
                b_prev(b, next) = new;
                b_next(b, loc) = new;
                b_prev(b, new) = loc;

                /* Am I last?  Need to update parents */
                if (!next) btree_update_psize(b, ssize);

                btree_update_daughter(b, node, new);
        }

        bp = b->parent;
        if (bp)
        {
                /* Get node previous to myself above */
                size_t ip = b_prev(bp, b_pindex(b));

                /* Easy - just insert into the parent, tail recurse */
                btree_node_insert(tl, bp, ip, tsize, tmp);

                return;
        }

        /* I'm the top node */

        /* New node */
        st = slist_rem(&tl->btree_freenode);
        tmp2 = list_entry(btree, data, st);
        tl->b_cnt--;

        /* btree is taller */
        tl->b_hgt++;

        /* Copy b into this -shouldn't need this, use allocated root instead */
        memcpy(&tmp2->data, &b->data, sizeof(btree) - SEPSIZE);

        for (i = b_start(b); i; i = b_next(b, i))
        {
                b_ptr(b, i)->parent = tmp2;
                bsize = b->bsize[i];
        }

        /* Init b */
        b->bsize[1] = tsize + 2;
        b->bsize[2] = bsize & ~0xff;
        b_ptr(b, 1) = tmp;
        b_ptr(b, 2) = tmp2;
        b_prev(b, 0) = 2;
        b_prev(b, 1) = 0;
        b_prev(b, 2) = 1;
        b_start(b) = 1;
        b_mask(b) = -4;
        b->parent = NULL;

        /* Make links */
        tmp->parent = b;
        tmp2->parent = b;
        b_pindex(tmp) = 1;
        b_pindex(tmp2) = 2;
}

static void btree_node_insert(atls *tl, btree *b, int loc, unsigned ssize, btree *node)
{
        size_t new;
        size_t next;

#ifdef DEBUG_ALLOC_SLOW
        if (ssize & 0xff) errx(1, "ssize not clean\n");
#endif

        if (!b_mask(b))
        {
                btree_node_insert_aux(tl, b, loc, ssize, node);

                return;
        }

        /* We have space - add it */
        new = btree_alloc(b);

        next = b_next(b, loc);
        b->bsize[new] = ssize + next;
        b_prev(b, next) = new;
        b_next(b, loc) = new;
        b_prev(b, new) = loc;

        /* Am I last?  Need to update parents */
        if (!next) btree_update_psize(b, ssize);

        btree_update_daughter(b, node, new);
}

static void btree_insert(atls *tl, btree *n, size_t size)
{
        int ip = 0;

        /* Convert to internal size (upper 24bits of 32bit bsize) */
        unsigned ssize = size * 16;

        /* First find where to put it, splitting to make room */
        btree *b = btree_find(tl, ssize, &ip);

#ifdef DEBUG_ALLOC_SLOW
        if (!un_used(n)) errx(1, "inserting a used node\n");
        if (size != n->s.size) errx(1, "size missmatch\n");
#endif

        /* Make a leaf node */
        //b_start(n) = 0;

        /* Hack - do the above more efficiently */
        n->bsize[0] = 0;

        /* Insert it */
        btree_node_insert(tl, b, ip, ssize, n);

        btree_cleanup(tl);
}

static noinline btree *btree_get(atls *tl, unsigned size)
{
        DECL_PROF_FUNC;

        unsigned ssize = size * 16;
        btree *b;

        b = btree_remove(tl, ssize);

        if (b)
        {
                /* Do not try to merge with me - I'm already taken! */
                set_used(b, b->s.size);
        }

        return b;
}

/* Dumb nlogn merge.  Avoids recursion though */
static void btree_merge(atls *tl1, atls *tl2)
{
        btree *b;

        slist *s, *sn;

        while ((b = btree_remove(tl2, 0)))
        {
                btree_insert(tl1, b, b->s.size);
        }

        /* Update allocated size */
        tl1->a_alloced += tl2->a_alloced;

        /* Free the old extra btree nodes */
        scan_slist_safe(&tl2->btree_freenode, s, sn)
        {
                b = list_entry(btree, data, s);

                /* Delete it */
                local_free(tl1, &b->data);
        }

        tl2->b_cnt = 0;
}

/* Count number of nodes + leaves in the btree recursively */
static int count_btree(btree *b)
{
        int i, count = 1;

        for (i = b_start(b); i; i = b_next(b, i))
        {
                count += count_btree(b_ptr(b, i));
        }

        return count;
}

static int count_btree_space(btree *b)
{
        int i, count = 0;

        if (b_leaf(b)) return b->s.size - PTRSIZE;

        for (i = b_start(b); i; i = b_next(b, i))
        {
                count += count_btree_space(b_ptr(b, i));
        }

        return count;
}


#ifdef DEBUG_ALLOC_SLOW

/* Check double list constraints */
static void test_double_lists(atls *tl)
{
        btree *b;
        dlist *d, *dn;

        unsigned i;

        for (i = 1; i < NUM_QS; i++)
        {
                if (tl->qs[i].next->prev != &tl->qs[i]) errx(1, "First double link broken\n");

                scan_list_safe(&tl->qs[i], d, dn)
                {
                        b = list_entry(btree, list, d);
                        check_sep(b);

                        if (!un_used(b)) errx(1, "False used\n");
                        if (b->s.size != (i+1)*16) errx(1, "Wrong sized double link\n");
                        if (b->s.bs_offset & FLG_SIZE8) errx(1, "flag size wrong\n");

                        if (dn->prev != d) errx(1, "Back-link broken\n");
                }
        }

        if (tl->q_mask & (1ULL << 63)) errx(1, "double list last bit set\n");
}

#else
#define test_double_lists(T) ((void) sizeof(T))
#endif


#ifdef DEBUG_ALLOC_SLOW
/* Test small list constraints */
static void test_small_list(atls *tl)
{
        btree *b, *bn;

        btree *q8 = get_q8(tl);

        if (small_prev(small_next(q8)) != q8) errx(1, "First link broken\n");

        for (b = small_next(q8); b != q8; b = bn)
        {
                check_sep(b);
                bn = small_next(b);

                if (!(b->s.bs_offset & FLG_SIZE8)) errx(1, "Wrong sized small link\n");
                if (!un_used(b)) errx(1, "False used\n");
                if (small_prev(bn) != b) errx(1, "Back-link broken\n");
        }
}
#else
#define test_small_list(T) ((void) sizeof(T))
#endif

/* Add to end of small list */
static void small_insert(atls *tl, btree *b)
{
        btree *q8 = get_q8(tl);
        btree *qp;

        /* Set flag */
        set_size8(b);

        qp = small_prev(q8);

        set_small_prev(b, qp);
        set_small_next(qp, b);

        set_small_next(b, q8);
        set_small_prev(q8, b);
}

static void small_remove(btree *b)
{
        btree *qn = small_next(b);
        btree *qp = small_prev(b);

        set_small_next(qp, qn);
        set_small_prev(qn, qp);

        /* Clear flag */
        unset_size8(b);
}

static btree *small_del_first(atls *tl)
{
        btree *q8 = get_q8(tl);
        btree *b = small_next(q8);
        btree *qn = small_next(b);

        /* List is empty */
        if (b == q8) return NULL;

        /* Dequeue b */
        set_small_next(q8, qn);
        set_small_prev(qn, q8);

        /* Clear flag */
        unset_size8(b);

        return b;
}

static void small_merge(atls *tl1, atls *tl2)
{
        btree *q81 = get_q8(tl1);
        btree *q82 = get_q8(tl2);

        btree *q1p = small_prev(q81);
        btree *q2n = small_next(q82);
        btree *q2p = small_prev(q82);

        /* Don't need to do anything if adding an empty list */
        if (q2n == q82) return;

        set_small_next(q1p, q2n);
        set_small_prev(q2n, q1p);

        set_small_prev(q81, q2p);
        set_small_next(q2p, q81);
}

/* Slab implementation */

static sbheader *slab_start(void *p)
{
        return (sbheader *) (-SLABSIZE & (uintptr_t) p);
}

#ifdef DEBUG_ALLOC_SLOW

static void test_slab(atls *tl)
{
        int i;

        dlist *d, *dn;

        for (i = 0; i < NUM_SB; i++)
        {
                scan_list_safe(&tl->slab[i], d, dn)
                {
                        if (dn->prev != d) errx(1, "Back-link broken\n");
                }
        }

        scan_list_safe(&tl->slab_full, d, dn)
        {
                if (dn->prev != d) errx(1, "Back-link broken\n");
        }
}
#else
#define test_slab(T) ((void) sizeof(T))
#endif


static freesb *slab_alloc_chunk(atls *tl)
{
        DECL_PROF_FUNC;

        freesb *fsb;

        size_t alloc_amount = SLABSIZE * (SLABBMAX + 1);
        size_t sbrk_end;

        unsigned i;
        unsigned alloced = SLABBMAX;

        /* Handle oom more efficiently */
        if (sbrk_oom) return NULL;

        /* Make sure percpu value isn't too big */
        if (tl->percpu_hash > cpu_total)
        {
                tl->percpu_hash %= cpu_total;
        }

        /* Find an unlocked list with something in it */
        for (i = tl->percpu_hash; i < cpu_total; i++)
        {
                if (pc_slab[i].list && !mutex_trylock(&pc_slab[i].m))
                {
                        if (pc_slab[i].list)
                        {
                                fsb = pc_slab[i].list;
                                pc_slab[i].list = fsb->next;

                                mutex_unlock(&pc_slab[i].m);
#ifdef WINDOWS
                                /* Reallow use of pages */
                                for (i = 0; i < fsb->count; i++)
                                {
                                        VirtualAlloc(fsb->blocks[i], SLABSIZE, MEM_COMMIT, PAGE_READWRITE);
                                }
#endif

                                return fsb;
                        }

                        mutex_unlock(&pc_slab[i].m);
                }
        }

        for (i = 0; i < tl->percpu_hash; i++)
        {
                if (pc_slab[i].list && !mutex_trylock(&pc_slab[i].m))
                {
                        if (pc_slab[i].list)
                        {
                                fsb = pc_slab[i].list;
                                pc_slab[i].list = fsb->next;

                                mutex_unlock(&pc_slab[i].m);
#ifdef WINDOWS
                                /* Reallow use of pages */
                                for (i = 0; i < fsb->count; i++)
                                {
                                        VirtualAlloc(fsb->blocks[i], SLABSIZE, MEM_COMMIT, PAGE_READWRITE);
                                }
#endif

                                return fsb;
                        }

                        mutex_unlock(&pc_slab[i].m);
                }
        }

        mutex_lock(&sb_lock);

        sbrk_end = sbrk_start + sbrk_size;

        /* Try to realign with SLABSIZE boundaries */
        if (sbrk_end & (SLABSIZE - 1))
        {
                alloc_amount += SLABSIZE - (sbrk_end & (SLABSIZE - 1));
        }

        fsb = sbrk(alloc_amount);

        if (fsb == MAP_FAILED)
        {
                /* Too much to allocate - fall back on mmap */
                sbrk_oom = 1;
                mutex_unlock(&sb_lock);

                return NULL;
        }

        /* Update sbrk information */
        sbrk_size = alloc_amount + (uintptr_t) fsb - sbrk_start;

        mutex_unlock(&sb_lock);

        /* Are we improperly aligned? */
        if ((SLABSIZE - 1) & (uintptr_t) fsb)
        {
                /* Realign myself (wastes memory) */
                freesb *fsb_new = (freesb *) slab_start(shift(fsb, SLABSIZE - 1));

                /* Did we shift too much? */
                if ((uintptr_t) fsb_new - (uintptr_t) fsb > alloc_amount - SLABSIZE * (SLABBMAX + 1))
                {
                        alloced--;
                }

                fsb = fsb_new;
        }

        /* Fill in details */
        for (i = 0; i < alloced; i++)
        {
                fsb->blocks[i] = shift(fsb, SLABSIZE * (i + 1));
        }
        fsb->count = alloced;

        return fsb;
}

static noinline void slab_free_chunk(atls *tl, freesb *fsb)
{
        DECL_PROF_FUNC;

        unsigned i;

        /* Mark memory as unused */
        for (i = 0; i < fsb->count; i++)
        {
#ifdef WINDOWS
                VirtualFree(fsb->blocks[i], SLABSIZE, MEM_DECOMMIT);
#else
                madvise(fsb->blocks[i], SLABSIZE, MADV_DONTNEED);
#endif
        }

        /* Make sure percpu value isn't too big */
        if (tl->percpu_hash > cpu_total)
        {
                tl->percpu_hash %= cpu_total;
        }

        /* First trylock everything, to find something that works */
        for (i = tl->percpu_hash; i < cpu_total; i++)
        {
                if (!mutex_trylock(&pc_slab[i].m))
                {
                        tl->percpu_hash = i;
                        goto success;
                }
        }

        for (i = 0; i < tl->percpu_hash; i++)
        {
                if (!mutex_trylock(&pc_slab[i].m))
                {
                        tl->percpu_hash = i;
                        goto success;
                }
        }

        /* Failure - too much contention, just use our current value */
        i = tl->percpu_hash;
        mutex_lock(&pc_slab[i].m);

success:
        tl->percpu_hash = i;
        fsb->next = pc_slab[i].list;
        pc_slab[i].list = fsb;
        mutex_unlock(&pc_slab[i].m);
}


static sbheader *slab_alloc_block(atls *tl)
{
        freesb *fsb;
        sbheader *sb;

        /* Make sure we have empty blocks */
        if (!tl->slab_chunk)
        {
                tl->slab_chunk = slab_alloc_chunk(tl);
                if (!tl->slab_chunk) return NULL;
        }

        fsb = tl->slab_chunk;

        if (!fsb->count)
        {
                sb = (sbheader *) fsb;

                tl->slab_chunk = NULL;

                /* Prevent reuse of this overwritten block */
                sb->size = 0;

                return sb;
        }

        fsb->count--;
        sb = fsb->blocks[fsb->count];

        return sb;
}

static void slab_free_block(atls *tl, sbheader *sb)
{
        freesb *ofsb = tl->slab_chunk;
        freesb *fsb = (freesb *) sb;

        if (ofsb)
        {
                if (ofsb->count < SLABBMAX - 1)
                {
                        ofsb->blocks[ofsb->count] = sb;
                        ofsb->count++;
                        return;
                }

                /* Simplest case - no chunk yet */
                fsb->count = 0;
                tl->slab_chunk = fsb;

                slab_free_chunk(tl, ofsb);

                return;
        }

        /* Simplest case - no chunk yet */
        fsb->count = 0;
        tl->slab_chunk = fsb;
}

static int init_sldata(void)
{
        unsigned i;

        /* Init total number of cpus */
        cpu_total = cpu_num();

        /* Init per-cpu free slab lists */
        pc_slab = big_alloc_aux(page_align(cpu_total * 64));
        if (!pc_slab) return 1;

        /*
         * Initialized mutexes have state zero, and initialized lists are NULL
         * so we don't have to do anything to pc_slab to finish initializing it.
         */

        /* Calculate slab total sizes so we avoid a division later */
        for (i = 1; i < NUM_SB; i++)
        {
                unsigned size = i * 16;

                /* total size taken by all blocks */
                sltotal[i] = ((SLABSIZE - offsetof(sbheader, data))/size) * size;
        }

        return 0;
}

static sbheader *slab_create(atls *tl, dlist *slab, unsigned size)
{
        DECL_PROF_FUNC;

        unsigned index = size/16;
        unsigned total = sltotal[index];

        uintptr_t offset;

        sbheader *sb = slab_alloc_block(tl);
        if (!sb) return NULL;

        /* Normalize size */
        size = index * 16;

        /* Fill in details */
        sb->tail = &tl->tail;

        dlist_add(slab, &sb->list);

        /* Already initialized? */
        if (sb->size == size) return sb;

        sb->used = 0;
        sb->size = size;

        /* Calculate offset */
        if ((size == 64) || (size == 256))
        {
                /* Make cacheline-aligned */
                offset = (uintptr_t) sb + 128 + 1;
        }
        else
        {
                void *tmp;

                /* Start of region */
                offset = (-16 & (uintptr_t) &sb->data);

                /* Randomize offset */
                tmp = rnd_offset((void *) offset, (uintptr_t) sb + SLABSIZE - (uintptr_t) offset, total);

                offset = (uintptr_t) tmp + 1;
        }

#ifdef DEBUG_ALLOC
        if (offset - 1 + total - (uintptr_t) sb > SLABSIZE) errx(1, "slab overflow\n");
#endif

        sb->first = offset;
        sb->max = (uintptr_t) sb + SLABSIZE - sb->size;

        return sb;
}

static void slab_free(atls *tl, void *p)
{
        DECL_PROF_FUNC;

        sbheader *sb = slab_start(p);

        /* Do I own this? */
        if (unlikely(sb->tail != &tl->tail))
        {
                /* Hack wrt mealloc */
                prepend_queue(p, tl, &sb->tail);

                return;
        }

        /* Add to this slab's free list */
        *(uintptr_t *)p = sb->first;
        sb->first = (uintptr_t) p;

        sb->used--;
        if (!sb->used)
        {
                /* If I am the only one in the partial list, don't bother to delete */
                if (sb->list.next == sb->list.prev) return;

                dlist_del(&sb->list);

                /* Free it */
                slab_free_block(tl, sb);
        }
}

static void *slab_alloc(atls *tl, size_t size);
static noinline void *slab_alloc_nolist(size_t size, dlist *slab, atls *tl)
{
        DECL_PROF_FUNC;

        void *res;

        /* Out of line zero-check */
        if (!size)
        {
                size++;
        }
        else
        {
                /* Scan queue if we have nothing left */
                if (!tl->slab_chunk)
                {
                        scan_queue(tl, &tl->head, 0);
                }

                /* Still nothing? */
                if (dlist_empty(slab))
                {
                        if (!slab_create(tl, slab, size + 15))
                        {
                                goto again;
                        }
                }
        }

        /* We have something to use */
        res = slab_alloc(tl, size);
        if (res) return res;

again:

        size = sep_align(size);
        return local_alloc(tl, size);
}

static void *slab_alloc_aux(atls *tl, dlist *slab)
{
        DECL_PROF_FUNC;

        /* Get sbheader */
        sbheader *sb = list_entry(sbheader, list, slab->next);

        uintptr_t p = sb->first;

        sb->used++;

        if (!(p & 1))
        {
                sb->first = *(uintptr_t *) (void *) p;

                if (!sb->first) goto done;
        }
        else
        {
                p--;
                sb->first += sb->size;
                if (sb->first > sb->max)
                {
                        sb->first = 0;

                        goto done;
                }
        }

        return (void *) p;

done:
        /* Move to full list */
        dlist_del(&sb->list);
        dlist_add(&tl->slab_full, &sb->list);

        return (void *) p;
}

static void *slab_alloc(atls *tl, size_t size)
{
        dlist *slab;

        size_t nsize = size + 15;

#ifdef DEBUG_NO_SLAB
        size = sep_align(size);
        return local_alloc(tl, size);
#endif

        /* Get slab */
#ifdef __x86_64__
        slab = shift(&tl->slab[0], nsize & ~15);
#else
        slab = shift(&tl->slab[0], (nsize & ~15) / 2);
#endif

        if (dlist_empty(slab)) return slab_alloc_nolist(size, slab, tl);

        return slab_alloc_aux(tl, slab);
}

/* Same as above, but fail if we can't quickly allocate */
static void *slab_alloc_safe(atls *tl, size_t size)
{
        dlist *slab;

#ifdef DEBUG_NO_SLAB
        return NULL;
#endif

        size += 15;

        /* Get slab */
#ifdef __x86_64__
        slab = shift(&tl->slab[0], size & ~15);
#else
        slab = shift(&tl->slab[0], (size & ~15) / 2);
#endif

        /* Fail if we can't quickly allocate (don't call scan_queue) */
        if (dlist_empty(slab) && !slab_create(tl, slab, size)) return NULL;

        return slab_alloc_aux(tl, slab);
}

static noinline void *slab_zalloc(atls *tl, size_t size)
{
        void *p = slab_alloc(tl, size);
        if (p) return memset(p, 0, size);

        size = sep_align(size);

        p = fast_alloc(tl, size);
        if (!p)
        {
                tl->callocable = 0;
                p = slow_alloc(tl, size);

                /* No need to memset? */
                if (!p || tl->callocable)
                {
                        return p;
                }
        }

        /* Success */
        return memset(p, 0, size - 8);
}

static void slab_merge(atls *tl1, atls *tl2)
{
        int i;
        dlist *d, *dn;

        /* Merge partial slabs */
        for (i = 0; i < NUM_SB; i++)
        {
                /* Update all the tail pointers */
                scan_list_safe(&tl2->slab[i], d, dn)
                {
                        sbheader *sb = list_entry(sbheader, list, d);
                        sb->tail = &tl1->tail;

                        /* There may be one empty slab in this slot */
                        if (!sb->used)
                        {
                                /* Move to full list */
                                dlist_del(&sb->list);
                                dlist_add(&tl1->slab_full, &sb->list);
                        }
                }

                dlist_merge(&tl1->slab[i], &tl2->slab[i]);
        }

        /* Merge full and empty slabs */

        /* Update all the tail pointers */
        scan_list(&tl2->slab_full, d)
        {
                sbheader *sb = list_entry(sbheader, list, d);
                sb->tail = &tl1->tail;
        }

        dlist_merge(&tl1->slab_full, &tl2->slab_full);

        /* Get rid of empty pages */
        if (tl2->slab_chunk) slab_free_chunk(tl1, tl2->slab_chunk);
}

static void local_free(atls *tl, void *p)
{
        if (is_slab(p))
        {
                slab_free(tl, p);
        }
        else
        {
                btree *b = CONTAINER(btree, data, p);
                fast_free(tl, b, b->s.size);
        }
}

static void *local_alloc(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        void *p = fast_alloc(tl, size);
        if (p) return p;

        return slow_alloc(tl, size);
}

static void test_all(atls *tl)
{
        test_fast_lists(tl);
        test_double_lists(tl);
        test_small_list(tl);
        test_btree(tl);
        test_queue(tl);
        test_slab(tl);
        test_blocks(tl);
}

static void block_list_merge(atls *tl1, atls *tl2)
{
        mealloc *m;
        dlist *d;

        /* Scan block list, and update all tail pointers */
        scan_list(&tl2->bl, d)
        {
                m = list_entry(mealloc, m_list, d);
                m->tail = &tl1->tail;
        }

        dlist_merge(&tl1->bl, &tl2->bl);
}

static void atls_merge(atls *tl1, atls *tl2)
{
        int i;

        /* Merge block lists so others know about us */
        block_list_merge(tl1, tl2);

        /* Then merge the btrees */
        btree_merge(tl1, tl2);

        /* Merge the fast lists */
        fast_merge(tl1, tl2);

        /* Merge the slabs */
        slab_merge(tl1, tl2);

        /* Then the double links */
        for (i = 1; i < NUM_QS; i++)
        {
                dlist_merge(&tl1->qs[i], &tl2->qs[i]);
        }

        /* Finally the small list */
        small_merge(tl1, tl2);

        test_all(tl1);
}

static btree *split_node_rhs(atls *tl, btree *b, size_t t_size, size_t msize)
{
        size_t r_size = t_size - msize;

        btree *bm = shift(b, msize);

#ifdef DEBUG_ALLOC_SLOW 
        if (t_size != b->s.size) errx(1, "size missmatch\n");
        if (msize > t_size) errx(1, "too big to fit in split\n");
        check_sep(b);
#endif

        /* Too large to split profitably? */
        if (!r_size) return b;

        /* Update local size */
        b->s.size = msize;

        /* Create middle seperator */
        set_sep(bm, r_size, b);

        check_sep(bm);

        /* Make sure to try to use remainder */
        fast_free(tl, b, msize);

        /* Paranoia */
        check_sep(b);
        check_sep(bm);

        /* Used for when right node is used */
        return bm;
}

static always_inline void *split_node(atls *tl, btree *b, size_t t_size, size_t msize)
{
        size_t r_size = t_size - msize;

        btree *bm = shift(b, msize);

#ifdef DEBUG_ALLOC_SLOW 
        if (t_size != b->s.size) errx(1, "size missmatch\n");
        if (msize > t_size) errx(1, "too big to fit in split\n");
        check_sep(b);
#endif

        /* Too large to split profitably? */
        if (r_size * 4 < msize)
        {
                /* Used this whole */
                return &b->data;
        }

        /* Update local size */
        b->s.size = msize;

        /* Create middle seperator */
        set_sep(bm, r_size, b);

        check_sep(bm);

        /* Make sure to try to use remainder */
        fast_free(tl, bm, r_size);

        /* Paranoia */
        check_sep(b);
        check_sep(bm);

        return &b->data;
}

static void node_insert(atls *tl, btree *b)
{
        size_t size = b->s.size;

        if (size > QS_MAX)
        {
                /* Insert new segment into btree */
                btree_insert(tl, b, size);

                return;
        }

        if (size == 16)
        {
                small_insert(tl, b);

                return;
        }

        dlist_add(MYSIZE_TO_PTR(tl, size), &b->list2);
        tl->q_mask |= 1ULL << ((size - 8) / 16);
}

/* Complete merge */
static void merge_node_aux(atls *tl, btree *bl)
{
        DECL_PROF_FUNC;

        size_t msize = bl->s.size;

        /* Are we the only element in the allocation? */
        btree *br = shift(bl, msize);

        mealloc *m;

        /* Is block empty? */
        if ((bl->s.bs_offset >= HEADERSIZE) || br->s.size || (msize * 4 > tl->a_alloced)) goto save;

        /* Save a block, if it is big enough to use for a pending allocation */
        if (tl->s_wanted && (tl->s_wanted <= bl->s.size))
        {
                /* No longer wanted */
                tl->s_wanted = 0;

                goto save;
        }

        /* Get header */
        m = page_start(bl);

        /* Remove from the list */
        dlist_del(&m->m_list);

        /* Size of block */
        msize = m->b.s.size + HEADERSIZE;

#ifdef DEBUG_ALLOC_SLOW
        if (msize & (PAGESIZE - 1)) errx(1, "big block size incorrect\n");
#endif

        tl->a_alloced -= msize;

        big_freed(m, msize);

#ifndef WINDOWS
        munmap(m, msize);
#else
        VirtualFree(m, 0, MEM_RELEASE);
#endif
        return;

save:
        /* element is unallocated */
        set_unused(bl, br);

        /* Insert into correct data structure */
        node_insert(tl, bl);
}

/* Merge a node with unallocated neighbours + insert into free list */
static void merge_node(atls *tl, void *p)
{
        DECL_PROF_FUNC;

        btree *b = CONTAINER(btree, data, p);
        btree *bl = b, *br = shift(b, b->s.size);

        size_t tsize;

#ifdef DEBUG_ALLOC_SLOW
        if (un_used(b)) errx(1, "merging unused node");
#endif

        /* Test right */
        if (un_used(br))
        {
                if (br->s.bs_offset & FLG_SIZE8)
                {
                        small_remove(br);
                        tsize = 16;
                }
                else
                {
                        tsize = br->s.size;

                        if (tsize > QS_MAX)
                        {
                                btree_node_del(tl, br->parent, b_pindex(br));
                        }
                        else
                        {
                                dlist_del(&br->list2);
                        }
                }

                /* Fixup sizes */
                b->s.size += tsize;
        }

        /* Test left */
        if (left_unused(b))
        {
                if (b->s.bs_offset & FLG_LSIZE8)
                {
                        bl = shift(b, -(uintptr_t)16);

                        small_remove(bl);
                }
                else
                {
                        bl = b->s.left;

                        if (bl->s.size > QS_MAX)
                        {
                                btree_node_del(tl, bl->parent, b_pindex(bl));
                        }
                        else
                        {
                                dlist_del(&bl->list2);
                        }
                }

                /* Fixup sizes */
                bl->s.size += b->s.size;
        }

        merge_node_aux(tl, bl);
}

#ifdef __x86_64__

#define SAVE_REG(N, V)\
        asm volatile ("movq %0, %%xmm" #N "\n\t" :: "mr" (V))

#define RESTORE_REG(N, V)\
        asm volatile ("movq %%xmm" #N ", %0\n\t" : "=r" (V))

static always_inline void *fast_alloc(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        size_t n;
        u64b mask, tmask;
        slist *p;

        btree *b;
        size_t rsize;

        if (unlikely(size > FAST_64_BIN)) return NULL;

        n = size2fl(size);
        mask = FAST_MASK << n;
        tmask = tl->f_mask & mask;

        /* Anything to do? */
        while (tmask)
        {
                n = ffsq(tmask);
                p = &tl->fl[n];

                SAVE_REG(0, tmask);

                while (p->next)
                {
                        slist *s = slist_rem(p);
                        b = CONTAINER(btree, list, s);

                        rsize = b->s.size;

                        check_sep(b);

                        /* Found a match? */
                        if (likely(rsize >= size)) return &b->data;

                        RESTORE_REG(0, tmask);

                        /* Move to lower bin */
                        //fast_add(tl, b, n - 1);

                        /* Inlined version of the above so %rbx isn't used */
                        slist_add(&p[-1], &b->list);
                        tmask = tmask / 2;
                        tl->f_mask |= tmask & (-tmask);
                }

                RESTORE_REG(0, tmask);

                /*
                 * Turn off least significant bit in tmask, as nothing is left there
                 */
                mask = (tmask - 1) | ~tmask;
                tmask &= mask;
                tl->f_mask &= mask;
        }

        return NULL;
}

static void *slow_alloc_aux(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        size_t n;
        u64b mask, tmask;

        btree *b;
        dlist *d;
        size_t rsize;

        /* Special case empty allocations */
        if (size == 16)
        {
                b = small_del_first(tl);
                if (b)
                {
                        set_used(b, 16);

                        return &b->data;
                }

                n = 1;
        }
        else
        {
                n = (size / 16) - 1;
        }

        mask = (~0ULL) << n;
        tmask = tl->q_mask & mask;

        /* Are there nodes big enough in the queues? */
        while (tmask)
        {
                /* Ignore if bit unset */
                n = ffsq(tmask);
                d = &tl->qs[n];

                /* Found something? */
                if (d->next != d)
                {
                        b = list_entry(btree, list2, d->next);

                        /* Paranoia */
                        check_sep(b);

                        dlist_del(&b->list2);

                        rsize = (n + 1) * 16;
                        set_used(b, rsize);
                        return split_node(tl, b, rsize, size);
                }

                /*
                 * Turn off least significant bit in tmask, as nothing is left there
                 */
                mask = (tmask - 1) | ~tmask;
                tmask &= mask;
                tl->q_mask &= mask;
        }

        return NULL;
}

#else

/* Versions optimized for 32bit */
static always_inline void *fast_alloc(atls *tl, size_t size)
{
        size_t n = size2fl(size);

        unsigned tmask;

        slist *p;

        btree *b;
        size_t rsize;

        if (n < 32)
        {
                tmask = tl->f_mask & (FAST_MASK << n);

                /* Anything to do? */
                while (tmask)
                {
                        n = ffsu(tmask);
                        p = &tl->fl[n];

                        while (p->next)
                        {
                                slist *s = slist_rem(p);
                                b = CONTAINER(btree, list, s);

                                rsize = b->s.size;

                                check_sep(b);

                                /* Found a match? */
                                if (likely(rsize >= size)) return &b->data;

                                /* Move to lower bin */
                                fast_add(tl, b, n - 1);
                        }

                        /*
                         * Turn off least significant bit in tmask, as nothing is left there
                         */
                        tmask &= tmask - 1;
                        tl->f_mask &= ~(1ULL << n);
                }

                tmask = (tl->f_mask >> 32) & (FAST_MASK >> (32 - n));
        }
        else
        {
                tmask = (tl->f_mask >> 32) & (FAST_MASK << (n - 32));
        }

        if (unlikely(size >= FAST_64_BIN)) return NULL;

        /* Anything to do? */
        while (tmask)
        {
                n = ffsu(tmask) + 32;
                p = &tl->fl[n];

                while (p->next)
                {
                        slist *s = slist_rem(p);
                        b = CONTAINER(btree, list, s);

                        rsize = b->s.size;

                        check_sep(b);

                        /* Found a match */
                        if (likely(rsize >= size)) return &b->data;

                        /* Move to lower bin */
                        fast_add(tl, b, n - 1);
                }

                /*
                 * Turn off least significant bit in tmask, as nothing is left there
                 */
                tmask &= tmask - 1;
                tl->f_mask &= ~(1ULL << n);
        }

        return NULL;
}

static void *slow_alloc_aux(atls *tl, size_t size)
{
        size_t n;
        unsigned tmask;

        btree *b;
        dlist *d;
        size_t rsize;

        /* Special case empty allocations */
        if (size == 16)
        {
                b = small_del_first(tl);
                if (b)
                {
                        set_used(b, 16);
                        return &b->data;
                }

                n = 1;
        }
        else
        {
                n = (size / 16) - 1;
        }

        if (n < 32)
        {
                tmask = tl->q_mask & (~0 << n);

                /* Are there nodes big enough in the queues? */
                while (tmask)
                {
                        /* Ignore if bit unset */
                        n = ffsu(tmask);
                        d = &tl->qs[n];

                        /* Found something? */
                        if (d->next != d)
                        {
                                b = list_entry(btree, list, d->next);

                                /* Paranoia */
                                check_sep(b);

                                dlist_del(&b->list2);

                                rsize = (n + 1) * 16;
                                set_used(b, rsize);

                                return split_node(tl, b, rsize, size);
                        }

                        /*
                         * Turn off least significant bit in tmask, as nothing is left there
                         */
                        tmask &= tmask - 1;
                        tl->q_mask &= ~(1ULL << n);
                }

                tmask = tl->q_mask >> 32;
        }
        else
        {
                tmask = (tl->q_mask >> 32) & (~0 << (n - 32));
        }

        /* Are there nodes big enough in the queues? */
        while (tmask)
        {
                /* Ignore if bit unset */
                n = ffsu(tmask) + 32;
                d = &tl->qs[n];

                /* Found something? */
                if (d->next != d)
                {
                        b = list_entry(btree, list, d->next);

                        /* Paranoia */
                        check_sep(b);

                        dlist_del(&b->list2);

                        rsize = (n + 1) * 16;
                        set_used(b, rsize);

                        return split_node(tl, b, rsize, size);
                }

                /*
                 * Turn off least significant bit in tmask, as nothing is left there
                 */
                tmask &= tmask - 1;
                tl->q_mask &= ~(1ULL << n);
        }

        /* Failed */
        return NULL;
}

#endif

static void *block_alloc_aux(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        btree *b, *br;
        mealloc *ma;
        size_t rsize, tasize;

        /* Make overhead 1/4th of total allocated after this allocation */
        tasize = size + (size + tl->a_alloced) / 3;
        tasize = page_align(tasize);

        /* Clip to BTMALLOC */
        if (tasize > BTMALLOC) tasize = BTMALLOC;

        /* Must be more than MINALLOC */
        if (tasize < MINALLOC) tasize = MINALLOC;

        ma = big_alloc_aux(tasize);

        if (!ma)
        {
                /* Try with smaller alloc */
                tasize = page_align(size + HEADERSIZE);

                ma = big_alloc_aux(tasize);

                if (!ma)
                {
                        /* Try again if new handler works */
                        if (handle_oom(size)) return slow_alloc(tl, size);

                        return NULL;
                }
        }

        rsize = tasize - HEADERSIZE;

        /* Keep track of total allocations */
        tl->a_alloced += tasize;

        /* Fill in header */
        dlist_add(&tl->bl, &ma->m_list);

        ma->tail = &tl->tail;

        b = &ma->b;

        /* Create left seperator */
        b->s.size = rsize;
        b->s.bs_offset = 16;

        /* Position of right seperator */
        br = shift(b, rsize);

        /* Create right seperator */
        br->s.bs_offset = tasize - SEPSIZE;

        tl->callocable = 1;

        return split_node(tl, b, rsize, size);
}

static void *block_alloc(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        btree *b;
        void *p;

        if (size >= BTMALLOC)
        {
                tl->callocable = 1;
                return big_alloc(tl, size);
        }

        if (size <= QS_MAX)
        {
                p = slow_alloc_aux(tl, size);
                if (p) return p;

                /* Clear fast lists */
                clear_fast(tl);

                p = slow_alloc_aux(tl, size);
                if (p) return p;
        }
        else
        {
                /* Clear fast lists */
                clear_fast(tl);
        }

        /* Try to alloc on the btree */
        b = btree_get(tl, size);
        if (b) return split_node(tl, b, b->s.size, size);

        /* Try to grab space from a dead thread */
        if (reap_dead(tl)) return local_alloc(tl, size);

        /* We need more space, so try to free memory. */
        if (scan_queue(tl, &tl->head, size)) return local_alloc(tl, size);

#ifdef DEBUG_LEAK
        leak_print(tl, "Trying to allocate %llu (%p) but cannot\n", (unsigned long long) size, (void *) size);
        malloc_stats_aux(3);
#endif

        /* Failure - make a big alloc, and add to the btree */
        return block_alloc_aux(tl, size);
}


static void *slow_alloc(atls *tl, size_t size)
{
        DECL_PROF_FUNC;

        /* Fast allocation failed - try normal data structures */
        if (size <= QS_MAX)
        {
                void *res = slow_alloc_aux(tl, size);
                if (res) return res;
        }

        return block_alloc(tl, size);
}

/* Free with no memory usage */
static void free_nomem(void *p)
{
        DECL_PROF_FUNC;

        btree *b = CONTAINER(btree, data, p);

        mealloc *m;

        slist *v, *tail;

#ifdef DEBUG_ALLOC      
        /* Check for double-free errors */
        if (un_used(b)) errx(1, "Double free with %p\n", p);
#endif

        /* Get block start */
        m = read_bs(b);

        /* Treat node as a list now */
        v = &b->list;

        v->next = NULL;

        /*
         * Prevent other threads from dying because we have no hazard pointer
         * This protects the dereference of m->tail
         */
        mutex_lock(&h_lock);

        /* Prepend the data */
        tail = xchg_ptr(m->tail, v);

        /* Done */
        mutex_unlock(&h_lock);

        tail->next = v;
}

static noinline void free_aux(void *p)
{
        DECL_PROF_FUNC;

        atls *tl = init_tls();
        if (!tl)
        {
                free_nomem(p);
                return;
        }

        PREFIX(free)(p);
}

static noinline void free_clear(atls *tl)
{
        clear_fast(tl);
}

void PREFIX(free)(void *p)
{
        DECL_PROF_FUNC;

        btree *b;
        size_t size;

        atls *tl;

        if (!p) return;

        tl = get_tls();

        if (!tl)
        {
                free_aux(p);
                return;
        }

        test_all(tl);

        if (likely(is_slab(p)))
        {
                slab_free(tl, p);

                test_all(tl);

                return;
        }

        b = CONTAINER(btree, data, p);
        size = b->s.size;

#ifdef DEBUG_ALLOC
        /* Check for double-free errors */
        if (un_used(b)) errx(1, "Double free with %p\n", p);
#endif

        if (size)
        {
                /* Get block start */
                mealloc *m = read_bs(b);

                /* My tail = a local node */
                if (unlikely(m->tail != &tl->tail))
                {

                        /* Add to their queue, and let them deal with it */
                        prepend_queue(p, tl, &m->tail);

                        return;
                }

                /* Inlined version of fast_free() */
                size = size2fl(size);
                tl->f_mask |= 1ULL << size;
                slist_add(&tl->fl[size], p);

                tl->fcount++;
                if (!(tl->fcount & FREE_FAST)) free_clear(tl);

                test_all(tl);
        }
        else
        {
                big_free_aux(page_start(b));
        }
}
#if 0
void cfree(void *p)
{
        PREFIX(free)(p);
}
#endif
static noinline void *malloc_aux(size_t size)
{
        DECL_PROF_FUNC;

        atls *tl = init_tls();
        if (!tl) return NULL;
        return PREFIX(malloc)(size);
}

void *PREFIX(malloc)(size_t size)
{
        DECL_PROF_FUNC;

        void *res;
        atls *tl;

        test_leak();

        /* Init local data if required */
        tl = get_tls();

        if (!tl) return malloc_aux(size);

        test_all(tl);

        if (likely(size <= SB_MAX)) return slab_alloc(tl, size);

        /* Prevent overflow bug in sep_align() below */
        if (unlikely(size > BTMALLOC)) return big_alloc(tl, size);

        size = sep_align(size);
        res = fast_alloc(tl, size);
        if (res) return res;

        return slow_alloc(tl, size);
}

#ifdef DEBUG_ALLOC_SLOW
static void test_wiped(void *p, size_t len)
{
        char *endy = &(((char *)p)[len - 8]);
        char *y;

        if (!len) return;

        for (y = p; y < endy; y++)
        {
                if (*y) errx(1, "found non-zero\n");
        }
}
#else
#define test_wiped(P, L) ((void) (sizeof(P) + sizeof(L)))
#endif

static noinline void *zalloc_aux(size_t size)
{
        atls *tl = init_tls();
        if (!tl) return NULL;
        return zalloc(tl, size);
}

static void *zalloc(atls *tl, size_t size)
{
        void *p;

        test_leak();
        test_all(tl);

        if (likely(size <= SB_MAX)) return slab_zalloc(tl, size);

        /* Prevent overflow bug in sep_align() below */
        if (unlikely(size > BTMALLOC)) return big_alloc(tl, size);

        size = sep_align(size);

        p = fast_alloc(tl, size);

        if (!p)
        {
                tl->callocable = 0;
                p = slow_alloc(tl, size);

                /* No need to memset? */
                if (!p || tl->callocable)
                {
                        test_wiped(p, size);

                        return p;
                }
        }

        test_all(tl);

        return memset(p, 0, size - 8);
}

static size_t safemul(size_t n, size_t size)
{
#ifdef __x86_64__
        /* 64 bit */
        __uint128_t dn = n;
        __uint128_t dsize = size;
        __uint128_t drsize = dn*dsize;
        size_t rsize = drsize;
        if (drsize >> 64)
        {
                /* Overflow */
                return TOP_SIZE + 1;
        }

#else

        /* 32 bit */
        u64b dn = n;
        u64b dsize = size;
        u64b drsize = dn*dsize;
        size_t rsize = drsize;

        if (drsize >> 32)
        {
                /* Overflow */
                return TOP_SIZE + 1;
        }
#endif

        return rsize;
}

void *PREFIX(calloc)(size_t n, size_t size)
{
        DECL_PROF_FUNC;

        /* Init local data if required */
        atls *tl = get_tls();

        size = safemul(n, size);

        test_leak();

        if (!tl) return zalloc_aux(size);

        return zalloc(tl, size);
}

#ifdef WINDOWS
void *PREFIX(_calloc_impl)(size_t n, size_t size, int *errno_tmp)
{
        DECL_PROF_FUNC;

        void *ret;
        atls *tl;

        int errno_orig;
        if (!errno_tmp) return PREFIX(calloc)(n, size);

        /* Init local data if required */
        tl = get_tls();

        size = safemul(n, size);

        test_leak();

        if (!tl) return zalloc_aux(size);

        _get_errno(&errno_orig);

        ret = zalloc(tl, safemul(n, size));
        _get_errno(errno_tmp);
        _set_errno(errno_orig);

        return ret;
}
#endif

static noinline void *realloc_aux(void *p, size_t size)
{
        atls *tl = init_tls();

        /* Cannot allocate anything */
        if (!tl) return NULL;

        return PREFIX(realloc)(p, size);
}

static noinline void *realloc_aux2(void *p, size_t size, atls *tl)
{
        btree *b = CONTAINER(btree, data, p);
        size_t msize = b->s.size;

        size_t old_size;

#ifdef DEBUG_ALLOC
        if (un_used(b)) errx(1, "Realloc of unmalloced pointer %p\n", p);
#endif

        /* Was a big block? */
        if (!msize)
        {
#ifndef WINDOWS
                size_t *np;
#endif
                size_t *ps = page_start(b);

                size_t offset = (char *) b - (char *) ps;

                /* Get old size */
                old_size = *ps;

                /* Don't bother resizing shrinks that are more than half the allocated size */
                if ((old_size - offset <= size * 2) && (old_size - offset >= size)) return p;

                /* Resize to new big block if possible */
                if (size >= BTMALLOC)
                {
                        /* Align */
                        size = page_align(size + offset + offsetof(btree, data));

#ifndef WINDOWS
                        /* Use (nonportable) mremap */
                        np = mremap(ps, old_size, size, MREMAP_MAYMOVE);

                        /* Success? */
                        if (np != MAP_FAILED)
                        {
                                /* Save new size */
                                *np = size;

                                /* Return new pointer */
                                return shift(np, offset + offsetof(btree, data));
                        }
#endif

                        if (size < old_size)
                        {
#ifndef WINDOWS
                                if (!munmap(shift(ps, size), old_size - size))
                                {
                                        /* Update size */
                                        *ps = size;
                                }
#else
                                /*
                                 * Say we no longer want the memory....
                                 * But it is still mapped into our address space taking up room!
                                 */
                                if (VirtualAlloc(shift(ps, size), old_size - size, MEM_RESET,
                                PAGE_NOACCESS))
                                {
                                        /* Update size */
                                        *ps = size;
                                }
#endif

                                return p;
                        }
                }
        }
        else
        {
                mealloc *m;

                /* Get old size */
                old_size = msize;

                size = sep_align(size);

                /* Don't bother resizing shrinks that are more than half the allocated size */
                if ((old_size <= size * 2) && (old_size >= size)) return p;

                m = read_bs(b);

                /* Local node? */
                if (m->tail == &tl->tail)
                {
                        btree *br;

                        /* Easy case */
                        if (size <= msize) return split_node(tl, b, msize, size);

                        /* Make sure adjacent nodes are in the btree */
                        clear_fast(tl);

                        /* Medium or small size - try to merge */
                        br = shift(b, msize);
                        if (un_used(br))
                        {
                                if (br->s.bs_offset & FLG_SIZE8)
                                {
                                        small_remove(br);

                                        /* Fixup sizes */
                                        b->s.size += 16;
                                        msize += 16;

                                        br = shift(br, 16);

                                        /* Set it as used */
                                        br->s.bs_offset &= ~FLG_LUNUSED;
                                }
                                else
                                {
                                        size_t rsize = br->s.size;
                                        if (rsize)
                                        {
                                                if (rsize > QS_MAX)
                                                {
                                                        btree_node_del(tl, br->parent, b_pindex(br));
                                                }
                                                else
                                                {
                                                        dlist_del(&br->list2);
                                                }

                                                /* Fixup sizes */
                                                b->s.size += rsize;
                                                msize += rsize;

                                                br = shift(br, rsize);

                                                /* Set it as used */
                                                br->s.bs_offset &= ~FLG_LUNUSED;
                                        }
                                }
                        }

                        /* Region fits? */
                        if (size <= msize) return split_node(tl, b, msize, size);
                }
                else
                {
                        /* We can only shrink a foreign node */
                        if (size <= msize)
                        {
                                /* Open coded split node */
                                btree *bm = shift(b, size);

                                /* Update my size */
                                b->s.size = size;

                                /* Create middle seperator */
                                set_sep(bm, old_size - size, b);

                                /* Free the foreign excess */
                                prepend_queue((void *) &bm->data, tl, &m->tail);

                                return p;
                        }
                }
        }

        /* Failure */
        return NULL;
}

void *PREFIX(realloc)(void *p, size_t size)
{
        DECL_PROF_FUNC;

        void *np;

        size_t old_size;

        /* Init local data if required */
        atls *tl = get_tls();

        int old_errno;

        test_leak();

        if (!tl) return realloc_aux(p, size);

        test_all(tl);

        /* realloc(p, 0) is the same as free(p) */
        if (!size)
        {
                PREFIX(free)(p);

                return NULL;
        }

        /* Relloc NULL is the same as malloc */
        if (!p) return PREFIX(malloc)(size);

        /* Too large to allocate */
        if (size > TOP_SIZE) goto nomem;

        if (!is_slab(p))
        {
                /* See if merging will work */
                np = realloc_aux2(p, size, tl);

                if (np) return np;
        }

        old_size = malloc_usable_size(p);

#ifdef WINDOWS
        _get_errno(&old_errno);
#else
        /* Failure - have to do it manually */
        old_errno = errno;
#endif

        np = PREFIX(malloc)(size);
        if (!np)
        {
                /* Is original allocation still okay? */
                if (size <= old_size)
                {
                        /* Don't set errno to be ENOMEM */
#ifdef WINDOWS
                        _set_errno(old_errno);
#else
                        errno = old_errno;
#endif

                        /* Return old allocation */
                        return p;
                }
                goto nomem;
        }

        /* Copy data */
        if (size > old_size) size = old_size;
        memcpy(np, p, size);

        PREFIX(free)(p);

        /* Done */
        return np;

nomem:
        set_enomem();
        return NULL;
}

#ifndef WINDOWS
static void unmap_range(void *x1, void *x2)
{
        if (x1 != x2)
        {
                munmap(x1, (char *) x2 - (char *) x1);
        }
}
#endif

#ifdef DEBUG_ALLOC_SLOW
static void test_align(size_t align, void *p)
{
        uintptr_t x = (uintptr_t) p;
        if (align && (x & (align - 1))) errx(1, "Incorrect alignment of pointer\n");
}
#else
#define test_align(a, p) ((void) (sizeof(a) + sizeof(p)))
#endif

static noinline void *memalign_aux(size_t align, size_t size)
{
        atls *tl = init_tls();

        /* Cannot allocate anything! */
        if (!tl) return NULL;

        return PREFIX(memalign)(align, size);
}

#ifdef WINDOWS
static void *memalign_aux2(size_t align, size_t size, size_t rsize)
{
        size_t psize = page_align(rsize + PAGESIZE);
        size_t *ps;

        (void) size;

        if (align > PAGESIZE) goto nomem;
        if (rsize > TOP_SIZE) goto nomem;

        ps = VirtualAlloc(NULL, rsize, MEM_COMMIT | MEM_RESERVE, PAGE_READWRITE);
        if (ps == MAP_FAILED) goto nomem;

        /* Small alignment */
        *ps = psize;
        ps = shift(ps, PAGESIZE);

        test_align(align, ps);

        return ps;

nomem:
        set_enomem();
        return NULL;
}
#else
static void *memalign_aux2(size_t align, size_t size, size_t rsize)
{
        size_t pssize = page_align(size + PAGESIZE);
        size_t psize = page_align(rsize + PAGESIZE);
        size_t lsize;
        int flags = MAP_PRIVATE | MAP_ANONYMOUS;

        size_t *lstart, *lend;

        size_t *ps;
        void *p;

        if (rsize > TOP_SIZE) goto nomem;

        /*
         * Hack - large alignments require no reservation,
         * otherwise we run out of memory
         */
        if (align > size) flags |= MAP_NORESERVE;

        ps = mmap(NULL, psize, PROT_READ | PROT_WRITE, flags, -1, 0);

        /* Out of memory */
        if (ps == MAP_FAILED) goto nomem;

        /* Small alignment */
        if (align <= PAGESIZE)
        {
                *ps = psize;
                p = shift(ps, PAGESIZE);

                test_align(align, p);

                return p;
        }

        /* Large alignment */
        lstart = ps;
        lsize = (-(uintptr_t) ps) & (align - 1);

        /* Already aligned - need to shift to get sep+size at beginning */
        if (!lsize)
        {
                ps = shift(ps, align - PAGESIZE);

                /* Fragment at beginning to unmap */
                unmap_range(lstart, ps);

                *ps = pssize;
                p = shift(ps, PAGESIZE);

                test_align(align, p);
                return p;
        }

        lend = shift(ps, rsize);
        ps = shift(ps, lsize - PAGESIZE);

        /* Fragment at beginning to unmap */
        unmap_range(lstart, ps);
        *ps = pssize;
        p = shift(ps, PAGESIZE);

        lstart = shift(p, pssize);

        /* Fragment at end to unmap */
        unmap_range(lstart, lend);

        test_align(align, p);
        return p;

nomem:
        set_enomem();
        return NULL;
}
#endif

void *PREFIX(memalign)(size_t align, size_t size)
{
        DECL_PROF_FUNC;

        size_t rsize, lsize;

        void *p;

        btree *b;

        atls *tl = get_tls();

        test_leak();

        /* Too large to allocate */
        if (size > TOP_SIZE) goto nomem;

        if (align <= SEPSIZE) return PREFIX(malloc)(size);

        if (!tl) return memalign_aux(align, size);

        /* Try to cache-line align via slab special case */
        if ((size <= 64) && (align <= 64))
        {
                p = slab_alloc(tl, 64);
                if (p)
                {
                        /* Double-check alignment as slab_alloc may fall-back in low mem */
                        if (!(63 & (uintptr_t) p)) return p;
                        local_free(tl, p);
                }
        }

        size = sep_align(size);
        rsize = sep_align(size + align);

        /* Check for overflow */
        if ((rsize <= size) || (rsize <= align)) goto nomem;

#ifdef WINDOWS
        /* Large allocations are special */
        if (rsize >= BTMALLOC)
        {
                return memalign_aux2(align, size, rsize);
        }
#else
        /* Large allocations are special */
        if (rsize >= BTMALLOC)
        {
                return memalign_aux2(align, size, rsize);
        }
#endif

        while (1)
        {
                p = fast_alloc(tl, rsize);
                if (p) break;

                if (rsize < QS_MAX)
                {
                        p = slow_alloc_aux(tl, rsize);
                        if (p) break;

                        /* Clear fast lists */
                        clear_fast(tl);

                        p = slow_alloc_aux(tl, rsize);
                        if (p) break;
                }
                else
                {
                        /* Clear fast lists */
                        clear_fast(tl);
                }

                /* Try to alloc on the btree */
                b = btree_get(tl, rsize);
                if (b)
                {
                        p = split_node(tl, b, b->s.size, rsize);
                        break;
                }

                /* Try to grab space from a dead thread */
                if (reap_dead(tl)) continue;

                /* We need more space, so try to free memory. */
                if (scan_queue(tl, &tl->head, rsize)) continue;

                /* Everything failed - fall back to large allocation */
                return memalign_aux2(align, size, rsize);
        }

        lsize = (-(uintptr_t) p) & (align - 1);

        b = CONTAINER(btree, data, p);

#ifdef DEBUG_ALLOC_SLOW
        if (rsize > b->s.size) errx(1, "node received is too small\n");
#endif

        /* get real size allocated */
        rsize = b->s.size;

        /* Already aligned? */
        if (!lsize)
        {
                test_align(align, &b->data);

                /* Split off part we need */
                return split_node(tl, b, rsize, size);
        }

        b = split_node_rhs(tl, b, rsize, lsize);
        test_align(align, &b->data);

        return split_node(tl, b, rsize - lsize, size);

nomem:
        set_enomem();
        return NULL;
}

int PREFIX(posix_memalign)(void **p, size_t align, size_t size)
{
        /* Make sure power of two and greater than sizeof(void *) */
#ifdef __x86_64__       
        if (align & ((align - 1) | 7))
        {
                *p = NULL;
                return EINVAL;
        }
#else
        if (align & ((align - 1) | 3))
        {
                *p = NULL;
                return EINVAL;
        }
#endif

        *p = PREFIX(memalign)(align, size);

        if (!*p) return ENOMEM;

        return 0;
}
#if 0
void *valloc(size_t size)
{
        return PREFIX(memalign)(PAGESIZE, size);
}

void *pvalloc(size_t size)
{
        return PREFIX(memalign)(PAGESIZE, page_align(size));
}
#endif
//#ifdef WINDOWS
#if 1
static
#endif
size_t malloc_usable_size(void *p)
{
        size_t offset;
        size_t *ps;
        size_t size;

        DECL_PROF_FUNC;

        btree *b = CONTAINER(btree, data, p);

        /* Don't crash on a NULL pointer */
        if (!p) return 0;

        /* Handle slab allocations */
        if (is_slab(p))
        {
                sbheader *sb = slab_start(p);
                return sb->size;
        }

        size = b->s.size;

        /* Small allocation */
        if (size) return size - PTRSIZE;

        /* Large allocation */
        ps = page_start(b);
        offset = (uintptr_t) &b->data - (uintptr_t) ps;

        return *ps - offset;
}

#ifdef WINDOWS
#ifdef PREFIX
size_t PREFIX(_msize)(void *p)
#else /* !PREFIX */
__attribute__((dllimport)) size_t _msize(void *p)
#endif /* PREFIX */
{
        return malloc_usable_size(p);
}
#endif


#if 0
struct mallinfo mallinfo(void)
{
        atls *tl = get_tls();
        struct mallinfo mi = {0,0,0,0,0,0,0,0,0,0};

        dlist *d;
        slist *s;

        int i;

        btree *b;

        size_t size;

        mi.arena = sbrk_size;

        if (!tl)
        {
                tl = init_tls();

                /* Cannot allocate anything, just return arena count */
                if (!tl) return mi;
        }

        /* Scan slab */
        for (i = 0; i < NUM_SB; i++)
        {
                scan_list(&tl->slab[i], d)
                {
                        mi.smblks++;
                        mi.usmblks++;
                }
        }

        scan_list(&tl->slab_full, d)
        {
                mi.smblks++;
                mi.usmblks++;
        }

        if (tl->slab_chunk)
        {
                mi.fsmblks = 1 + tl->slab_chunk->count;
                mi.smblks += mi.fsmblks;
        }

        /* Scan dlists */
        for (i = 1; i < NUM_QS; i++)
        {
                scan_list(&tl->qs[i], d)
                {
                        mi.ordblks++;

                        b = CONTAINER(btree, list, d);
                        size = b->s.size - PTRSIZE;
                        mi.fordblks += size;
                }
        }

        /* Add in results from small list */
        for (b = small_next((btree *) &tl->qs[0]); b != (btree *) &tl->qs[0]; b = small_next(b))
        {
                mi.ordblks++;
                mi.fordblks += 8;
        }

        /* Scan fastlists */
        for (i = 0; i < NUM_FL; i++)
        {
                scan_slist(&tl->fl[i], s)
                {
                        mi.ordblks++;

                        b = CONTAINER(btree, list, s);
                        size = b->s.size - PTRSIZE;
                        mi.fordblks += size;
                }
        }

        /* Count memory blocks */
        scan_list(&tl->bl, d)
        {
                mi.hblks++;
        }

        /* Count btree nodes */
        mi.hblkhd = count_btree(&tl->bheap);

        /* Count btree space */
        mi.fordblks += count_btree_space(&tl->bheap);

        /* Total allocated space (including overhead of seperators and atls) */
        mi.uordblks = tl->a_alloced - mi.fordblks + PAGESIZE;

        /* Total easily callocable region */
        mi.keepcost = 0;

        /* Done */
        return mi;
}

int malloc_trim(size_t pad)
{
        atls *tl = get_tls();

        /* Nothing allocated - do nothing */
        if (!tl) return 1;

        /* Clear incoming frees */
        scan_queue(tl, &tl->head, 0);

        /* Hack - ignore pad - and just free as much as possible */
        clear_fast(tl);

        (void) pad;

        /* Always return success */
        return 1;
}



int mallopt(int param, int val)
{
        /* Ignore parameters */
        (void) param;
        (void) val;

        /* Just return success - we don't have any parameters to modify */
        return 1;
}

#endif

#ifdef DEBUG_LEAK
static int btree_print(atls *tl, btree *b)
{
        int i;
        btree *bc;

        int count = 1;

        if (b_leaf(b))
        {
                leak_print(tl, "%u\n", b->s.size);
                return 0;
        }

        leak_print(tl, "Btree: %p\n", (void *) b);

        for (i = b_start(b); i; i = b_next(b, i))
        {
                bc = b_ptr(b, i);
                leak_print(tl, "link %p\n", (void *) bc);
                count += btree_print(tl, bc);
        }

        return count;
}
#endif

#if 0
// #ifndef WINDOWS

static void mem_slab(void)
{
        int i;
        int count;
        dlist *d;

        atls *tl = get_tls();
        if (!tl) return;

        leak_print(tl, "Total Slab Virtual: %llu\n", (unsigned long long) sbrk_size);

        for (i = 0; i < NUM_SB; i++)
        {
                if (dlist_empty(&tl->slab[i])) continue;

                count = 0;
                scan_list(&tl->slab[i], d)
                {
                        count++;
                }
                leak_print(tl, "Partial slab %d used: %lld\n", i * 16, count * 65536ULL);
        }

        if (!dlist_empty(&tl->slab_full))
        {
                count = 0;
                scan_list(&tl->slab_full, d)
                {
                        count++;
                }
                leak_print(tl, "Full slab used: %lld\n", count * 65536ULL);
        }

        if (tl->slab_chunk)
        {
                leak_print(tl, "Local free slabs: %lld\n", (tl->slab_chunk->count + 1) * 65536LL);
        }
        else
        {
                leak_print(tl, "Local free slabs: 0\n");
        }
}

#ifdef DEBUG_LEAK
static void mem_big(void)
{
        int i;

        /* If vsnprintf allocates, we may have a problem... */
        mutex_lock(&l_lock);

        for (i = 0; i < LEAK_MAX; i++)
        {
                if (big_leak[i].p)
                {
                        leak_print(get_tls(), "big block %p: %llu\n", big_leak[i].p, (unsigned long long) big_leak[i].size);
                }
        }

        mutex_unlock(&l_lock);
}
#endif

static void malloc_stats_aux(int show_nodes)
{
        atls *tl = get_tls();

        dlist *d;
        btree *b;

        size_t size;
        size_t tsize = 0;
        size_t asize = 0;

        /* Nothing allocated - print nothing */
        if (!tl) return;

        clear_fast(tl);

        scan_list(&tl->bl, d)
        {
                mealloc *m = list_entry(mealloc, m_list, d);

                size = big_block_size(m);

                if (size)
                {
                        leak_print(tl, "Block: %p %llu\n", (void *) m, (unsigned long long) size);
                }

                /* Scan seps for this block */
                for (b = &m->b;; b = shift(b, size))
                {
                        if (b->s.bs_offset & FLG_SIZE8)
                        {
                                size = 16;
                        }
                        else
                        {
                                size = b->s.size;
                        }

                        if (!size) break;

                        tsize += size;

                        if (un_used(b))
                        {
                                if (show_nodes) leak_print(tl, "  %p\n", (void *) size);
                        }
                        else
                        {
                                if (show_nodes) leak_print(tl, "* %p\n", (void *) size);
                                asize += size;
                        }
                }
        }

        leak_print(tl, "Total in btree %llu, total alloced %llu\n", (unsigned long long) tsize, (unsigned long long) asize);

#ifdef DEBUG_LEAK
        if (show_nodes & 2)
        {
                int count = btree_print(tl, &tl->bheap);

                leak_print(tl, "b_cnt = %d, b_hgt = %d, total = %d\n", tl->b_cnt, tl->b_hgt, count);
        }

        mutex_lock(&h_lock);
        size = 0;
        scan_list(&h_list, d)
        {
                size++;
        }
        mutex_unlock(&h_lock);
        leak_print(tl, "%d threads\n", (int) size);

        mem_big();
#endif
        mem_slab();
}

void malloc_stats(void)
{
        malloc_stats_aux(0);
}
#endif

static void **ialloc_fallback(atls *tl, size_t n, size_t *sizes, void **chunks, int clear)
{
        size_t i;
        void **out;

        /* Get storage for pointers */
        if (!chunks)
        {
                out = local_alloc(tl, sep_align(sizeof(void *) * n));
                if (!out) return NULL;
        }
        else
        {
                out = chunks;
        }

        /* Do it manually */
        if (clear)
        {
                for (i = 0; i < n; i++)
                {
                        out[i] = zalloc(tl, sizes[0]);
                        if (!out[i]) goto fail;
                }
        }
        else
        {
                for (i = 0; i < n; i++)
                {
                        out[i] = local_alloc(tl, sizes[i]);
                        if (!out[i]) goto fail;
                }
        }

        return out;

fail:
        for (n = 0; n < i; n++)
        {
                PREFIX(free)(out[i]);
        }

        if (!chunks) PREFIX(free)(out);

        return NULL;
}

static void **ialloc(atls *tl, size_t n, size_t *sizes, void **chunks, int clear)
{
        size_t i;

        size_t nsize;
        size_t total_size = 0;

        void *p;
        btree *b, *br;
        unsigned offset;

        void **out;

        test_all(tl);

        test_leak();

        /* Zero sized array? */
        if (!n)
        {
                if (chunks) return chunks;

                return PREFIX(malloc)(0);
        }

        /* Get total size to allocate */
        if (clear)
        {
                total_size = safemul(sep_align(sizes[0]), n);

                /* Overflow */
                if (total_size >= TOP_SIZE)
                {
                        set_enomem();
                        return NULL;
                }
        }
        else
        {
                for (i = 0; i < n; i++)
                {
                        nsize = sep_align(sizes[i]);
                        total_size += nsize;

                        /* Overflow */
                        if (total_size < nsize)
                        {
                                set_enomem();
                                return NULL;
                        }
                }
        }

        if (clear) tl->callocable = 0;

        while (1)
        {
                p = fast_alloc(tl, total_size);
                if (p) break;

                if (total_size < QS_MAX) p = slow_alloc(tl, total_size);
                if (p) break;

                /* Too large to allocate normally */
                if (total_size >= BTMALLOC) return ialloc_fallback(tl, n, sizes, chunks, clear);

                /* Clear fast lists */
                clear_fast(tl);

                /* Try to alloc on the btree */
                b = btree_get(tl, total_size);
                if (b)
                {
                        p = split_node(tl, b, b->s.size, total_size);
                        break;
                }

                /* Try to grab space from a dead thread */
                if (reap_dead(tl)) continue;

                /* We need more space, so try to free memory. */
                if (scan_queue(tl, &tl->head, total_size)) continue;

                /* Try to allocate a new block */
                p = block_alloc_aux(tl, total_size);
                if (p) break;

                /* Everything failed - fall back to individual allocations */
                return ialloc_fallback(tl, n, sizes, chunks, clear);
        }

        b = CONTAINER(btree, data, p);

        /* Get real total size */
        total_size = b->s.size;
        offset = b->s.bs_offset & ~15;

        /* Do we need to clear it? */
        if (clear && !tl->callocable) memset(p, 0, total_size - 8);

        /* Get storage for pointers */
        if (!chunks)
        {
                out = local_alloc(tl, sep_align(sizeof(void *) * n));

                if (!out)
                {
                        PREFIX(free)(p);
                        return NULL;
                }
        }
        else
        {
                out = chunks;
        }

        for (i = 0; i < n; i++)
        {
                out[i] = p;

                if (clear)
                {
                        nsize = sep_align(sizes[0]);
                }
                else
                {
                        nsize = sep_align(sizes[i]);
                }
                total_size -= nsize;

                /* Update local size */
                b->s.size = nsize;

                p = shift(p, nsize);
                br = CONTAINER(btree, data, p);

                /* Create offset part of right seperator */
                offset += nsize;
                if (i != n - 1) br->s.bs_offset = offset;

                b = br;
        }

        /* Nothing left - then we are done */
        if (!total_size)
        {
                test_all(tl);
                return out;
        }

        /* Resize last element to have the slack */
        p = out[n - 1];
        b = CONTAINER(btree, data, p);

        b->s.size += total_size;
        check_sep(b);

        /* How big is last allocation? */
        if (clear)
        {
                nsize = sep_align(sizes[0]);
        }
        else
        {
                nsize = sep_align(sizes[n - 1]);
        }

        /* Split off excess if too much */
        split_node(tl, b, b->s.size, nsize);

        test_all(tl);
        return out;
}

#if 0
static noinline void **ialloc_aux(size_t n, size_t *sizes, void **chunks, int clear)
{
        atls *tl = init_tls();
        if (!tl) return NULL;

        return ialloc(tl, n, sizes, chunks, clear);
}

void **independent_calloc(size_t n, size_t size, void **chunks)
{
        atls *tl = get_tls();

        if (!tl) return ialloc_aux(n, &size, chunks, 1);

        return ialloc(tl, n, &size, chunks, 1);
}

void **independent_comalloc(size_t n, size_t *sizes, void **chunks)
{
        atls *tl = get_tls();

        if (!tl) return ialloc_aux(n, sizes, chunks, 0);

        return ialloc(tl, n, sizes, chunks, 0);
}
#endif
//#ifndef WINDOWS
#if 0
void *malloc_get_state(void)
{
        abort();

        return NULL;
}

int malloc_set_state(void *p)
{
        (void) p;
        abort();

        return 0;
}
#endif


#ifdef WINDOWS
#ifdef PREFIX
void *PREFIX(_expand)(void *p, size_t size)
#else /* PREFIX */
__attribute__((dllimport)) void *_expand(void *p, size_t size)
#endif /* PREFIX */
{
        DECL_PROF_FUNC;

        atls *tl = get_tls();

        /* paranoia */
        if (!p) return NULL;

        /* Handle expansion into already allocated memory */
        if (malloc_usable_size(p) <= size) return p;

        /* Don't handle slab allocations */
        if (is_slab(p)) return NULL;

        /* Cannot expand a block created by someone else */
        if (!tl) goto nomem;

        p = realloc_aux2(p, size, tl);

        /* Did it work? */
        if (malloc_usable_size(p) >= size) return p;

nomem:
        set_enomem();
        return NULL;
}

/* Nolock functions call their normal functions */
void PREFIX(_free_nolock)(void *p)
{
        PREFIX(free)(p);
}

void *PREFIX(_realloc_nolock)(void *p, size_t size)
{
        return PREFIX(realloc)(p, size);
}

void *PREFIX(_calloc_nolock)(size_t n, size_t size)
{
        return PREFIX(calloc)(n, size);
}

size_t PREFIX(_msize_nolock)(void *p)
{
        return malloc_usable_size(p);
}

#endif