Index: generic/include/bitops.h
===================================================================
--- generic/include/bitops.h	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
+++ generic/include/bitops.h	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2006 Ondrej Palkovsky
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _BITOPS_H_
+#define _BITOPS_H_
+
+#include <typedefs.h>
+
+
+/** Return position of first non-zero bit from left.
+ *
+ * If number is zero, it returns 0
+ */
+static inline int fnzb32(__u32 arg)
+{
+	int n = 0;
+
+	if (arg & 0xffff0000) { arg >>= 16;n += 16;}
+	if (arg & 0xff00) { arg >>= 8; n += 8;}
+	if (arg & 0xf0) { arg >>= 4; n += 4;}
+	if (arg & 0xc) { arg >>= 2; n+=2;}
+	if (arg & 0x2) { arg >>= 1; n+=1;}
+	return n;
+}
+
+static inline int fnzb64(__u64 arg)
+{
+	int n = 0;
+
+	/* This is because mips complains about big numbers,
+	 * other platforms should optimize it out */
+	__u64 oper = 0xffffffff;
+	oper <<= 32;
+
+	if (arg & oper) { arg >>= 32;n += 32;}
+	return n + fnzb32((__u32) arg);
+}
+
+#define fnzb(x) fnzb32(x)
+
+#endif
Index: generic/include/mm/heap.h
===================================================================
--- generic/include/mm/heap.h	(revision e1888f914c2c146f7fa8e416f6465393c32303d9)
+++ generic/include/mm/heap.h	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
@@ -32,7 +32,10 @@
 #include <arch/types.h>
 #include <typedefs.h>
+#include <mm/slab.h>
 
-#define malloc(size)		early_malloc(size)
-#define free(ptr)		early_free(ptr)
+//#define malloc(size)		early_malloc(size)
+//#define free(ptr)		early_free(ptr)
+#define malloc(size)		kalloc(size,0)
+#define free(ptr)		kfree(ptr)
 
 struct chunk {
Index: generic/include/mm/slab.h
===================================================================
--- generic/include/mm/slab.h	(revision e1888f914c2c146f7fa8e416f6465393c32303d9)
+++ generic/include/mm/slab.h	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
@@ -34,4 +34,10 @@
 #include <arch/atomic.h>
 
+/** Minimum size to be allocated by malloc */
+#define SLAB_MIN_MALLOC_W 3
+
+/** Maximum size to be allocated by malloc */
+#define SLAB_MAX_MALLOC_W 17
+
 /** Initial Magazine size (TODO: dynamically growing magazines) */
 #define SLAB_MAG_SIZE  4
@@ -41,5 +47,5 @@
 
 /** Maximum wasted space we allow for cache */
-#define SLAB_MAX_BADNESS(cache)   ((PAGE_SIZE << (cache)->order >> 2))
+#define SLAB_MAX_BADNESS(cache)   ((PAGE_SIZE << (cache)->order) >> 2)
 
 /* slab_reclaim constants */
@@ -110,3 +116,7 @@
 extern void slab_print_list(void);
 
+/* Malloc support */
+extern void * kalloc(unsigned int size, int flags);
+extern void kfree(void *obj);
+
 #endif
Index: generic/src/mm/as.c
===================================================================
--- generic/src/mm/as.c	(revision e1888f914c2c146f7fa8e416f6465393c32303d9)
+++ generic/src/mm/as.c	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
@@ -79,5 +79,5 @@
 	as_t *as;
 
-	as = (as_t *) malloc(sizeof(as_t));
+	as = (as_t *) early_malloc(sizeof(as_t));
 	if (as) {
 		list_initialize(&as->as_with_asid_link);
Index: generic/src/mm/slab.c
===================================================================
--- generic/src/mm/slab.c	(revision e1888f914c2c146f7fa8e416f6465393c32303d9)
+++ generic/src/mm/slab.c	(revision c352c2ec8a56ea52cad23e37a7b9b1757422863c)
@@ -93,4 +93,5 @@
 #include <panic.h>
 #include <debug.h>
+#include <bitops.h>
 
 SPINLOCK_INITIALIZE(slab_cache_lock);
@@ -109,4 +110,12 @@
  */
 static slab_cache_t *slab_extern_cache;
+/** Caches for malloc */
+static slab_cache_t *malloc_caches[SLAB_MAX_MALLOC_W-SLAB_MIN_MALLOC_W+1];
+char *malloc_names[] =  {
+	"malloc-8","malloc-16","malloc-32","malloc-64","malloc-128",
+	"malloc-256","malloc-512","malloc-1K","malloc-2K",
+	"malloc-4K","malloc-8K","malloc-16K","malloc-32K",
+	"malloc-64K","malloc-128K"
+};
 
 /** Slab descriptor */
@@ -479,4 +488,5 @@
 {
 	int i;
+	int pages;
 
 	memsetb((__address)cache, sizeof(*cache), 0);
@@ -508,5 +518,6 @@
 
 	/* Minimum slab order */
-	cache->order = (cache->size-1) >> PAGE_WIDTH;
+	pages = ((cache->size-1) >> PAGE_WIDTH) + 1;
+	cache->order = fnzb(pages);
 
 	while (badness(cache) > SLAB_MAX_BADNESS(cache)) {
@@ -633,6 +644,6 @@
 	/* Disable interrupts to avoid deadlocks with interrupt handlers */
 	ipl = interrupts_disable();
-	
-	if (!(cache->flags & SLAB_CACHE_NOMAGAZINE))
+
+	if (!(cache->flags & SLAB_CACHE_NOMAGAZINE) && CPU)
 		result = magazine_obj_get(cache);
 
@@ -651,6 +662,6 @@
 }
 
-/** Return object to cache  */
-void slab_free(slab_cache_t *cache, void *obj)
+/** Return object to cache, use slab if known  */
+static void _slab_free(slab_cache_t *cache, void *obj, slab_t *slab)
 {
 	ipl_t ipl;
@@ -659,12 +670,19 @@
 
 	if ((cache->flags & SLAB_CACHE_NOMAGAZINE) \
+	    || !CPU \
 	    || magazine_obj_put(cache, obj)) {
 		
 		spinlock_lock(&cache->lock);
-		slab_obj_destroy(cache, obj, NULL);
+		slab_obj_destroy(cache, obj, slab);
 		spinlock_unlock(&cache->lock);
 	}
 	interrupts_restore(ipl);
 	atomic_dec(&cache->allocated_objs);
+}
+
+/** Return slab object to cache */
+void slab_free(slab_cache_t *cache, void *obj)
+{
+	_slab_free(cache,obj,NULL);
 }
 
@@ -711,4 +729,6 @@
 void slab_cache_init(void)
 {
+	int i, size;
+
 	/* Initialize magazine cache */
 	_slab_cache_create(&mag_cache,
@@ -732,3 +752,34 @@
 
 	/* Initialize structures for malloc */
-}
+	for (i=0, size=(1<<SLAB_MIN_MALLOC_W);
+	     i < (SLAB_MAX_MALLOC_W-SLAB_MIN_MALLOC_W+1);
+	     i++, size <<= 1) {
+		malloc_caches[i] = slab_cache_create(malloc_names[i],
+						     size, 0,
+						     NULL,NULL,0);
+	}
+}
+
+/**************************************/
+/* kalloc/kfree functions             */
+void * kalloc(unsigned int size, int flags)
+{
+	int idx;
+
+	ASSERT( size && size <= (1 << SLAB_MAX_MALLOC_W));
+	
+	if (size < (1 << SLAB_MIN_MALLOC_W))
+		size = (1 << SLAB_MIN_MALLOC_W);
+
+	idx = fnzb(size-1) - SLAB_MIN_MALLOC_W + 1;
+
+	return slab_alloc(malloc_caches[idx], flags);
+}
+
+
+void kfree(void *obj)
+{
+	slab_t *slab = obj2slab(obj);
+	
+	_slab_free(slab->cache, obj, slab);
+}
