+#define ALIGN_TO(val,align) ((((guint64)val) + ((align) - 1)) & ~((align) - 1))
+#define ALIGN_PTR_TO(ptr,align) (gpointer)((((gssize)(ptr)) + (align - 1)) & (~(align - 1)))
+
+static LockFreeMempool*
+lock_free_mempool_new (void)
+{
+ return g_new0 (LockFreeMempool, 1);
+}
+
+static void
+lock_free_mempool_free (LockFreeMempool *mp)
+{
+ LockFreeMempoolChunk *chunk, *next;
+
+ chunk = mp->chunks;
+ while (chunk) {
+ next = chunk->prev;
+ mono_vfree (chunk, mono_pagesize ());
+ chunk = next;
+ }
+ g_free (mp);
+}
+
+/*
+ * This is async safe
+ */
+static LockFreeMempoolChunk*
+lock_free_mempool_chunk_new (LockFreeMempool *mp, int len)
+{
+ LockFreeMempoolChunk *chunk, *prev;
+ int size;
+
+ size = mono_pagesize ();
+ while (size - sizeof (LockFreeMempoolChunk) < len)
+ size += mono_pagesize ();
+ chunk = mono_valloc (0, size, MONO_MMAP_READ|MONO_MMAP_WRITE);
+ g_assert (chunk);
+ chunk->mem = ALIGN_PTR_TO ((char*)chunk + sizeof (LockFreeMempoolChunk), 16);
+ chunk->size = ((char*)chunk + size) - (char*)chunk->mem;
+ chunk->pos = 0;
+
+ /* Add to list of chunks lock-free */
+ while (TRUE) {
+ prev = mp->chunks;
+ if (InterlockedCompareExchangePointer ((volatile gpointer*)&mp->chunks, chunk, prev) == prev)
+ break;
+ }
+ chunk->prev = prev;
+
+ return chunk;
+}
+
+/*
+ * This is async safe
+ */
+static gpointer
+lock_free_mempool_alloc0 (LockFreeMempool *mp, guint size)
+{
+ LockFreeMempoolChunk *chunk;
+ gpointer res;
+ int oldpos;
+
+ // FIXME: Free the allocator
+
+ size = ALIGN_TO (size, 8);
+ chunk = mp->current;
+ if (!chunk) {
+ chunk = lock_free_mempool_chunk_new (mp, size);
+ mono_memory_barrier ();
+ /* Publish */
+ mp->current = chunk;
+ }
+
+ /* The code below is lock-free, 'chunk' is shared state */
+ oldpos = InterlockedExchangeAdd (&chunk->pos, size);
+ if (oldpos + size > chunk->size) {
+ chunk = lock_free_mempool_chunk_new (mp, size);
+ g_assert (chunk->pos + size <= chunk->size);
+ res = chunk->mem;
+ chunk->pos += size;
+ mono_memory_barrier ();
+ mp->current = chunk;
+ } else {
+ res = (char*)chunk->mem + oldpos;
+ }
+
+ return res;
+}
+