utcb handling helper routines for the thread library.

This is one of the steps we need in the process of providing a similar interface
for thread creation which can be found in the mainstream operating systems like
Linux.

conts/libl4thread/SConscript

@@ -15,8 +15,11 @@ from config.projpaths import *
 
 Import('env')
 
+LIBMEM_RELDIR = 'conts/libmem'
+LIBMEM_DIR = join(PROJROOT, LIBMEM_RELDIR)
+
 e = env.Clone()
-e.Append(CPPPATH = ['include/l4thread'])
+e.Append(CPPPATH = ['include/l4thread', LIBMEM_DIR])
 
 source = [Glob('*.[cS]') + Glob('src/*.[cS]')]
 objects = e.StaticObject(source)

conts/libl4thread/include/l4thread/addr.h

@@ -0,0 +1,26 @@
+/*
+ * Address allocation pool
+ *
+ * Copyright (C) 2007 Bahadir Balban
+ */
+#ifndef __ADDR_H__
+#define __ADDR_H__
+
+#include <idpool.h>
+
+/* Address pool to allocate from a range of addresses */
+struct address_pool {
+	struct id_pool *idpool;
+	unsigned long start;
+	unsigned long end;
+};
+
+int address_pool_init_with_idpool(struct address_pool *pool,
+				  struct id_pool *idpool,
+				  unsigned long start, unsigned long end);
+int address_pool_init(struct address_pool *pool, unsigned long start,
+		      unsigned long end);
+void *address_new(struct address_pool *pool, int npages);
+int address_del(struct address_pool *, void *addr, int npages);
+
+#endif /* __ADDR_H__ */

conts/libl4thread/include/l4thread/bit.h

@@ -0,0 +1,42 @@
+#ifndef __BIT_H__
+#define __BIT_H__
+
+unsigned int __clz(unsigned int bitvector);
+int find_and_set_first_free_bit(u32 *word, unsigned int lastbit);
+int find_and_set_first_free_contig_bits(u32 *word,  unsigned int limit,
+					int nbits);
+int check_and_clear_bit(u32 *word, int bit);
+int check_and_clear_contig_bits(u32 *word, int first, int nbits);
+
+int check_and_set_bit(u32 *word, int bit);
+
+/* Set */
+static inline void setbit(unsigned int *w, unsigned int flags)
+{
+	*w |= flags;
+}
+
+
+/* Clear */
+static inline void clrbit(unsigned int *w, unsigned int flags)
+{
+	*w &= ~flags;
+}
+
+/* Test */
+static inline int tstbit(unsigned int *w, unsigned int flags)
+{
+	return *w & flags;
+}
+
+/* Test and clear */
+static inline int tstclr(unsigned int *w, unsigned int flags)
+{
+	int res = tstbit(w, flags);
+
+	clrbit(w, flags);
+
+	return res;
+}
+
+#endif /* __BIT_H__ */

conts/libl4thread/include/l4thread/idpool.h

@@ -0,0 +1,30 @@
+#ifndef __IDPOOL_H__
+#define __IDPOOL_H__
+
+#include <bit.h>
+#include <string.h>
+#include INC_GLUE(memory.h)
+
+struct id_pool {
+	int nwords;
+	int bitlimit;
+	u32 bitmap[];
+};
+
+/* Copy one id pool to another by calculating its size */
+static inline void id_pool_copy(struct id_pool *to, struct id_pool *from, int totalbits)
+{
+	int nwords = BITWISE_GETWORD(totalbits);
+
+	memcpy(to, from, nwords * SZ_WORD + sizeof(struct id_pool));
+}
+
+struct id_pool *id_pool_new_init(int mapsize);
+int id_new(struct id_pool *pool);
+int id_del(struct id_pool *pool, int id);
+int id_get(struct id_pool *pool, int id);
+int id_is_empty(struct id_pool *pool);
+int ids_new_contiguous(struct id_pool *pool, int numids);
+int ids_del_contiguous(struct id_pool *pool, int first, int numids);
+
+#endif /* __IDPOOL_H__ */

conts/libl4thread/include/l4thread/utcb.h

@@ -0,0 +1,25 @@
+/*
+ * UTCB handling helper routines
+ *
+ * Copyright (C) 2009 B Labs Ltd.
+ */
+#ifndef __UTCB_H__
+#define __UTCB_H__
+
+#include <l4/lib/list.h>
+
+struct utcb_desc {
+	struct link list;
+	unsigned long utcb_base;
+	struct id_pool *slots;
+};
+
+int utcb_pool_init(unsigned long utcb_start, unsigned long utcb_end);
+
+unsigned long utcb_new_slot(struct utcb_desc *desc);
+int utcb_delete_slot(struct utcb_desc *desc, unsigned long address);
+
+struct utcb_desc *utcb_new_desc(void);
+int utcb_delete_desc(struct utcb_desc *desc);
+
+#endif /* __UTCB_H__ */

conts/libl4thread/src/addr.c

@@ -0,0 +1,55 @@
+/*
+ * This module allocates an unused address range from
+ * a given memory region defined as the pool range.
+ *
+ * Copyright (C) 2007 Bahadir Balban
+ */
+#include <addr.h>
+#include <stdio.h>
+
+/*
+ * Initializes an address pool, but uses an already
+ * allocated id pool for it.
+ */
+int address_pool_init_with_idpool(struct address_pool *pool,
+				  struct id_pool *idpool,
+				  unsigned long start, unsigned long end)
+{
+	pool->idpool = idpool;
+	pool->start = start;
+	pool->end = end;
+
+	return 0;
+}
+
+int address_pool_init(struct address_pool *pool, unsigned long start, unsigned long end)
+{
+	if ((pool->idpool = id_pool_new_init(__pfn(end - start))) < 0)
+		return (int)pool->idpool;
+	pool->start = start;
+	pool->end = end;
+	return 0;
+}
+
+void *address_new(struct address_pool *pool, int npages)
+{
+	unsigned int pfn;
+
+	if ((int)(pfn = ids_new_contiguous(pool->idpool, npages)) < 0)
+		return 0;
+
+	return (void *)__pfn_to_addr(pfn) + pool->start;
+}
+
+int address_del(struct address_pool *pool, void *addr, int npages)
+{
+	unsigned long pfn = __pfn(page_align(addr) - pool->start);
+
+	if (ids_del_contiguous(pool->idpool, pfn, npages) < 0) {
+		printf("%s: Invalid address range returned to "
+		       "virtual address pool.\n", __FUNCTION__);
+		return -1;
+	}
+	return 0;
+}
+

conts/libl4thread/src/bit.c

@@ -0,0 +1,109 @@
+/*
+ * Bit manipulation functions.
+ *
+ * Copyright (C) 2007 Bahadir Balban
+ */
+#include <bit.h>
+#include <stdio.h>
+#include INC_GLUE(memory.h)
+
+/* Emulation of ARM's CLZ (count leading zeroes) instruction */
+unsigned int __clz(unsigned int bitvector)
+{
+	unsigned int x = 0;
+	while((!(bitvector & ((unsigned)1 << 31))) && (x < 32)) {
+		bitvector <<= 1;
+		x++;
+	}
+	return x;
+}
+
+int find_and_set_first_free_bit(u32 *word, unsigned int limit)
+{
+	int success = 0;
+	int i;
+
+	for(i = 0; i < limit; i++) {
+		/* Find first unset bit */
+		if (!(word[BITWISE_GETWORD(i)] & BITWISE_GETBIT(i))) {
+			/* Set it */
+			word[BITWISE_GETWORD(i)] |= BITWISE_GETBIT(i);
+			success = 1;
+			break;
+		}
+	}
+	/* Return bit just set */
+	if (success)
+		return i;
+	else
+		return -1;
+}
+
+int find_and_set_first_free_contig_bits(u32 *word,  unsigned int limit,
+					int nbits)
+{
+	int i = 0, first = 0, last = 0, found = 0;
+
+	/* Can't allocate more than the limit */
+	if (nbits > limit)
+		return -1;
+
+	/* This is a state machine that checks n contiguous free bits. */
+	/* FIXME: It should be <= instead of <. Fix & test in a single patch */
+	while (i + nbits < limit) {
+		first = i;
+		last  = i;
+		while (!(word[BITWISE_GETWORD(last)] & BITWISE_GETBIT(last))) {
+			last++;
+			i++;
+			if (last == first + nbits) {
+				found = 1;
+				break;
+			}
+		}
+		if (found)
+			break;
+		i++;
+	}
+
+	/* If found, set the bits */
+	if (found) {
+		for (int x = first; x < first + nbits; x++)
+			word[BITWISE_GETWORD(x)] |= BITWISE_GETBIT(x);
+		return first;
+	} else
+		return -1;
+}
+
+int check_and_clear_bit(u32 *word, int bit)
+{
+	/* Check that bit was set */
+	if (word[BITWISE_GETWORD(bit)] & BITWISE_GETBIT(bit)) {
+		word[BITWISE_GETWORD(bit)] &= ~BITWISE_GETBIT(bit);
+		return 0;
+	} else {
+		printf("Trying to clear already clear bit\n");
+		return -1;
+	}
+}
+
+int check_and_set_bit(u32 *word, int bit)
+{
+	/* Check that bit was clear */
+	if (!(word[BITWISE_GETWORD(bit)] & BITWISE_GETBIT(bit))) {
+		word[BITWISE_GETWORD(bit)] |= BITWISE_GETBIT(bit);
+		return 0;
+	} else {
+		//printf("Trying to set already set bit\n");
+		return -1;
+	}
+}
+
+int check_and_clear_contig_bits(u32 *word, int first, int nbits)
+{
+	for (int i = first; i < first + nbits; i++)
+		if (check_and_clear_bit(word, i) < 0)
+			return -1;
+	return 0;
+}
+

conts/libl4thread/src/idpool.c

@@ -0,0 +1,86 @@
+/*
+ * Used for thread and space ids, and also for utcbs.
+ *
+ * Copyright (C) 2009 B Labs Ltd.
+ */
+#include <idpool.h>
+#include <stdio.h>
+#include <l4/api/errno.h>
+#include <malloc/malloc.h>
+
+struct id_pool *id_pool_new_init(int totalbits)
+{
+	int nwords = BITWISE_GETWORD(totalbits) + 1;
+	struct id_pool *new = kzalloc((nwords * SZ_WORD)
+				      + sizeof(struct id_pool));
+	if (!new)
+		return PTR_ERR(-ENOMEM);
+
+	new->nwords = nwords;
+	new->bitlimit = totalbits;
+
+	return new;
+}
+
+/* Search for a free slot up to the limit given */
+int id_new(struct id_pool *pool)
+{
+	return find_and_set_first_free_bit(pool->bitmap, pool->bitlimit);
+}
+
+/* This finds n contiguous free ids, allocates and returns the first one */
+int ids_new_contiguous(struct id_pool *pool, int numids)
+{
+	int id = find_and_set_first_free_contig_bits(pool->bitmap,
+						     pool->bitlimit,
+						     numids);
+	if (id < 0)
+		printf("%s: Warning! New id alloc failed\n", __FUNCTION__);
+	return id;
+}
+
+/* This deletes a list of contiguous ids given the first one and number of ids */
+int ids_del_contiguous(struct id_pool *pool, int first, int numids)
+{
+	int ret;
+
+	if (pool->nwords * WORD_BITS < first + numids)
+		return -1;
+	if ((ret = check_and_clear_contig_bits(pool->bitmap, first, numids)))
+		printf("%s: Error: Invalid argument range.\n", __FUNCTION__);
+	return ret;
+}
+
+int id_del(struct id_pool *pool, int id)
+{
+	int ret;
+
+	if (pool->nwords * WORD_BITS < id)
+		return -1;
+
+	if ((ret = check_and_clear_bit(pool->bitmap, id) < 0))
+		printf("%s: Error: Could not delete id.\n", __FUNCTION__);
+	return ret;
+}
+
+/* Return a specific id, if available */
+int id_get(struct id_pool *pool, int id)
+{
+	int ret;
+
+	ret = check_and_set_bit(pool->bitmap, id);
+
+	if (ret < 0)
+		return ret;
+	else
+		return id;
+}
+
+int id_is_empty(struct id_pool *pool)
+{
+	for (int i = 0; i < pool->nwords; i++)
+		if (pool->bitmap[i])
+			return 0;
+	return 1;
+}
+

conts/libl4thread/src/thread.c

@@ -1,9 +1,10 @@
 /*
  *
  */
-
 #include <stdio.h>
 
+extern void utcb_test(unsigned long utcb_start, unsigned long utcb_end);
+
 void l4thread_print(void)
 {
 	printf("Hello world from libl4thread!\n");

conts/libl4thread/src/utcb.c

@@ -0,0 +1,88 @@
+/*
+ * UTCB management in Codezero
+ *
+ * Copyright © 2009 B Labs Ltd
+ */
+#include <stdio.h>
+#include <addr.h>
+#include <utcb.h>
+#include <malloc/malloc.h>
+
+/* Globally disjoint utcb virtual region pool */
+static struct address_pool utcb_region_pool;
+
+int utcb_pool_init(unsigned long utcb_start, unsigned long utcb_end)
+{
+	int err;
+
+	/* Initialise the global utcb virtual address pool */
+	if ((err = address_pool_init(&utcb_region_pool,
+					utcb_start, utcb_end) < 0)) {
+		printf("UTCB address pool initialisation failed.\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static inline void *utcb_new_address(int npages)
+{
+	return address_new(&utcb_region_pool, npages);
+}
+
+static inline int utcb_delete_address(void *utcb_address, int npages)
+{
+	return address_del(&utcb_region_pool, utcb_address, npages);
+}
+
+/* Return an empty utcb slot in this descriptor */
+unsigned long utcb_new_slot(struct utcb_desc *desc)
+{
+	int slot;
+
+	if ((slot = id_new(desc->slots)) < 0)
+		return 0;
+	else
+		return desc->utcb_base + (unsigned long)slot * UTCB_SIZE;
+}
+
+int utcb_delete_slot(struct utcb_desc *desc, unsigned long address)
+{
+	BUG_ON(id_del(desc->slots, (address - desc->utcb_base)
+		      / UTCB_SIZE) < 0);
+	return 0;
+}
+
+struct utcb_desc *utcb_new_desc(void)
+{
+	struct utcb_desc *d;
+
+	/* Allocate a new descriptor */
+	if (!(d	= kzalloc(sizeof(*d))))
+		return 0;
+
+	link_init(&d->list);
+
+	/* We currently assume UTCB is smaller than PAGE_SIZE */
+	BUG_ON(UTCB_SIZE > PAGE_SIZE);
+
+	/* Initialise utcb slots */
+	d->slots = id_pool_new_init(PAGE_SIZE / UTCB_SIZE);
+
+	/* Obtain a new and unique utcb base */
+	/* FIXME: Use variable size than a page */
+	d->utcb_base = (unsigned long)utcb_new_address(1);
+
+	return d;
+}
+
+int utcb_delete_desc(struct utcb_desc *desc)
+{
+	/* Return descriptor address */
+	utcb_delete_address((void *)desc->utcb_base, 1);
+
+	/* Free the descriptor */
+	kfree(desc);
+
+	return 0;
+}

conts/libl4thread/src/utcb_test.c

@@ -0,0 +1,55 @@
+/*
+ * This is for testing utcb port from POSIX
+ */
+#include <stdio.h>
+#include <utcb.h>
+
+void utcb_test(unsigned long utcb_start, unsigned long utcb_end)
+{
+	struct utcb_desc *udesc[20];
+	unsigned long addr;
+	int idx = -1;
+
+	printf("%s\n", __FUNCTION__);
+
+	utcb_pool_init(utcb_start, utcb_end);
+
+	// Test-1
+	for (int i = 0; i < 10; i++)
+		udesc[++idx] = utcb_new_desc();
+	for (int j = 0; j < 20; ++j)
+		addr = utcb_new_slot(udesc[idx]);
+
+	// Test 2
+	for (int i = 0; i < 3; i++)
+		udesc[++idx] = utcb_new_desc();
+	for (int j = 0; j < 10; ++j)
+		addr = utcb_new_slot(udesc[idx]);
+	for (int j = 0; j < 8; j++) {
+		utcb_delete_slot(udesc[idx], addr);
+		addr -= 0x100;
+	}
+	for (int j = 0; j < 2; ++j)
+		addr = utcb_new_slot(udesc[idx]);
+	for (int j = 0; j < 4; j++) {
+		utcb_delete_slot(udesc[idx], addr);
+		addr -= 0x100;
+	}
+	// Test 3
+	for (int i = 0; i < 7; i++) {
+		utcb_delete_desc(udesc[idx]);
+		--idx;
+	}
+	for (int j = 0; j < 4; ++j)
+		addr = utcb_new_slot(udesc[idx]);
+	for (int j = 0; j < 2; j++) {
+		utcb_delete_slot(udesc[idx], addr);
+		addr -= 0x100;
+	}
+	for (int i = 0; i < 3; i++)
+		udesc[++idx] = utcb_new_desc();
+	for (int i = 0; i < 7; i++) {
+		utcb_delete_desc(udesc[idx]);
+		--idx;
+	}
+}