/*
 * $Id: kern_mem.c,v 1.1.1.1 2007/10/16 08:41:04 init Exp $
 */
#include <sys/types.h>
#include <sys/mem.h>
#include <libkern/printf.h>

#define KMEM_DEBUG

#ifdef KMEM_DEBUG
#define DPRINTF(x...) 	do { printf(x); } while (0)
#else
#define DPRINTF(x...) 	{ }
#endif
 	
/*
 * Kernel memory.
 *
 * Our memory map looks like that (at least for testarm with 32MB):
 * (0MB)      0x00000000 | interrupt vectors (actually kernel text)
 *            ...
 *            0x00000020 | kernel text (continue)
 *            ...
 *            0x000fffff | END of kernel text
 *
 * (1MB)      0x00100000 | kernel data
 *            ... (1MB of kernel data)
 *            0x001fffff | END of kernel data
 *
 * (2MB)      0x00200000 | heap start (grows downwards)
 *            ... (28MB of heap)
 *
 * (30MB)     0x01e00000 <---- brk address
 * 
 *            ... (2MB of stack)
 * (32MB)     0x01ffffff | stack pointer (grows upwards)
 *
 * XXX very-very tiny memory model for my at91sam7s64
 */

/* config.c exports us ammount of available physical memory. */
extern uint32_t	 physmem;

/* break address */
//uint32_t 	brkaddr = (KMEM_HEAPBEGIN + (physmem - KMEM_HEAPBEGIN) / 2) 
//						+ (physmem - KMEM_HEAPBEGIN)
uint32_t 	brkaddr;

/* from where to allocate memory in kmalloc() */
uint32_t 	allocbase;

/* stack pointer */
uint32_t 	sp;

/* memory bit map pointer and its length */
uint8_t 	*membmap;
uint32_t 	membmaplen;

/* pages statistics */
uint32_t 	totalpages;
uint32_t 	freepages;


void
kmem_init(void)
{
	/*
	 * Initialize kernel memory (de)allocation subsystem.
	 * This allocates bit map, each BIT of it represents one kmem page.
	 * kmem page is KMEM_PAGESIZE bytes length and may be 0 (free) or 1 (not free) in membmap.
	 */
	int32_t 	n;
	uint8_t 	*bytep;

	/* set break address */
 	brkaddr = physmem - (physmem / 16); /* XXX top 2MB in testarm */
	
	/* bytes needed to hold bitmap */
	membmaplen = ((brkaddr - KMEM_HEAPBEGIN) / KMEM_PAGESIZE) / 8;

	/* total pages that will be available to kernel */
	totalpages = (brkaddr - (KMEM_HEAPBEGIN + membmaplen)) / KMEM_PAGESIZE; /* XXX what about potential modulo? */
	freepages = totalpages;

	/* allocate space for membmap in the beginning of KMEM_HEAPBEGIN */
	bytep = (uint8_t *) KMEM_HEAPBEGIN;
	n = membmaplen;

	while(n) {
		/* XXX what about errors? */
		*bytep = 0;
		bytep++;

		n--;
	}
 
	membmap = (uint8_t *)KMEM_HEAPBEGIN;

	/* skip our map; all allocations will be done starting from this */
	allocbase = KMEM_HEAPBEGIN + membmaplen;

	printf("kmem_init: membmap @0x%x size=%d pagesize=%d; %d total pages available starting from 0x%x\n", 
				membmap, membmaplen, KMEM_PAGESIZE, totalpages, allocbase);
}


void
*kmalloc(uint32_t nbytes)
{
	/*
	 * Allocate nbytes bytes of memory
	 */
	uint32_t 	pagesfound, npages, nmbmbytes, firstmbmbit;
	uint8_t 	*mbmbyte, mask, nmbmbits, i, offbits;
	uint32_t 	*firstpageptr, *lastpageptr, mbmbit, offbytes;


	/* kmalloc(0) should return NULL */
	if (! nbytes)
		return(NULL);

	/* pages needed to hold requested bytes */
	npages = nbytes / KMEM_PAGESIZE + (nbytes % KMEM_PAGESIZE != 0 ? 1 : 0);

	DPRINTF("kmalloc: requested %d bytes (%d pages) of %d free pages; ", nbytes, npages, freepages);

	/* check if we have enough free pages */
	if (npages >= freepages) {
		DPRINTF("not enough free pages\n");
		return(NULL);
	}

	pagesfound = 0;
	nmbmbytes = 0;
	/* look for contiguously available pages in memory bit map */
	for (mbmbyte = membmap; nmbmbytes < membmaplen; nmbmbytes++) {

		mask = 0x01;
		nmbmbits = 0; /* counts "free page" bits inside mbmbyte */

		/* check all 8 bits in mbmbyte */
		while(mask != 0) {

//			printf("0x%x @ 0x%x & %x\n", *mbmbyte, mbmbyte, mask);
			if ((mbmbyte[nmbmbytes] & mask) == 0) {
				/* page is free */
				pagesfound++;

				/* see if we need not to search more */
				if (pagesfound == npages) {
					/*
					 * Requested amount of pages have been found.
					 */

					/* calculate pointer to this (last) page */
					lastpageptr = (uint32_t *)((allocbase + (8 * nmbmbytes * KMEM_PAGESIZE)) + (KMEM_PAGESIZE * nmbmbits)); 
					DPRINTF("lastpageptr=0x%x\n", lastpageptr);

					/* rewind pointer to the first byte */
					firstpageptr = lastpageptr - ((npages - 1));
					DPRINTF("firstpageptr=0x%x\n", firstpageptr);

					offbits = mask ? mask - 1 : mask;	/* XXX */
					/* fill this (npages pages) region with zeroes */
					while(firstpageptr <= lastpageptr) {

						/* TODO invalidate page in membmap */
						offbytes = ((uint32_t)firstpageptr - (uint32_t)allocbase) / (KMEM_PAGESIZE * 8);
//						printf("offbytes=%d\n", offbytes);
//						printf("membmap[%d] |= 1 << %d\n", offbytes, offbits);
						membmap[offbytes] |= 1 << offbits;

						/* zerofy page */
						for (i = 0; i < KMEM_PAGESIZE; i++) {
							*(uint8_t *)firstpageptr = 0xaa;
							((uint8_t *)firstpageptr)++;
						}
						/* TODO rework offbits calculation */
						offbits++;
						if(offbits == 8)
							/* reset to the first bit */
							offbits = 0;
					}

					/* rewind pointer again */
					firstpageptr = lastpageptr - ((npages - 1));

					/* got it */
					DPRINTF("allocated, addr=0x%x\n", firstpageptr);

					/* decrement system free pages */
					freepages -= npages;

					/* return pointer to the first byte of the allocated region */
					return(firstpageptr);
				}
			} else {
				/* encountered non-free page; reset pagesfound */
				pagesfound = 0;
			}

			/* shift to check next bit */
			mask <<= 1;
			nmbmbits++;
		}
			
	}

	/* free pages haven't been found */
	DPRINTF("not found contiguous region\n");

	return(NULL);
}


void
kfree(void *p, uint32_t nbytes)
{
	/*
	 * XXX Free nbytes bytes
	*/
}