page_allocator.c

Go to the documentation of this file.

#include <sys/types.h>
#include <stdio.h>
#include <stddef.h>
#include <stdint.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>

#include <page_allocator.h>
#include <debug.h>

struct page_allocator {
        unsigned long base;
        unsigned long nr_of_pages; 
        unsigned long max_order; 
        unsigned long bitmap_sz;

        unsigned int page_size_bits;

        char bitmap[];
};

static inline int page_allocator_handle_size(unsigned int nr_of_pages)
{
        return sizeof(struct page_allocator) + (nr_of_pages << 1);
}

#define BYTE_IDX(j)     ((j) >> 3)
#define BIT_IDX(j)      ((j) & 0x07)
#define BIT_MASK(j)     (0x01 << BIT_IDX(j))

static inline void initialize_header(struct page_allocator *handle, unsigned int base, unsigned int size)
{
        unsigned int i;
        unsigned int bit_count;

        /* base & nr_of_pages will be updated after initiate the bitmap */
        handle->base = (unsigned int)base;
        handle->nr_of_pages = (size >> handle->page_size_bits);

        for (bit_count = 0, i = 1; i < handle->nr_of_pages; i <<= 1, bit_count++);

        handle->bitmap_sz = i >> 2;

        handle->max_order = bit_count;
        return;
}

static inline void initialize_bitmap(struct page_allocator *handle)
{
        unsigned int nr_of_pages;
        unsigned int order;
        unsigned int idx;
        unsigned int tmp;
        unsigned int start_idx;

        memset(handle->bitmap, 0, handle->bitmap_sz << 1);

        nr_of_pages = handle->nr_of_pages;
        order = handle->max_order;

        start_idx = 0;
        do {
                tmp = 0x01 << order;
                idx = (0x01 << (handle->max_order - order));
                idx--;

                while (tmp <= nr_of_pages) {
                        handle->bitmap[BYTE_IDX(idx + start_idx)] |= BIT_MASK(idx + start_idx);
                        nr_of_pages -= tmp;
                        start_idx++;
                }

                order--;
                start_idx <<= 0x01;
        } while (nr_of_pages > 0);
}

#if defined(_DEBUG)
static inline void debug_dump_bitmap(struct page_allocator *handle)
{
        unsigned int i;
        int bit_idx;
        unsigned int cnt;
        unsigned int line;
        unsigned int order;

        dbg_printf("Available bitmap\n");
        cnt = 0;
        line = 0x01;
        order = handle->max_order;
        dbg_printf("%d: ", order);
        for (i = 0; i < handle->bitmap_sz; i++) {
                for (bit_idx = 0; bit_idx < 8; bit_idx++) {
                        dbg_printf("%d", !!(handle->bitmap[i] & (0x01 << bit_idx)));
                        if (cnt++ == (line - 1)) {
                                order--;
                                dbg_printf("\n%d: ", order);
                                line = line << 0x01;
                                cnt = 0;
                        }
                }
        }
        dbg_printf("\n");

        dbg_printf("Allocated bitmap\n");
        cnt = 0;
        line = 0x01;
        order = handle->max_order;
        dbg_printf("%d: ", order);
        for (i = 0; i < handle->bitmap_sz; i++) {
                for (bit_idx = 0; bit_idx < 8; bit_idx++) {
                        dbg_printf("%d", !!(handle->bitmap[handle->bitmap_sz + i] & (0x01 << bit_idx)));
                        if (cnt++ == (line - 1)) {
                                order--;
                                dbg_printf("\n%d: ", order);
                                line = line << 0x01;
                                cnt = 0;
                        }
                }
        }
        dbg_printf("\n");
}
#endif

struct page_allocator *page_allocator_init(void *info, void *addr, unsigned int size)
{
        struct page_allocator *handle;

        handle = (struct page_allocator *)info;
        for (handle->page_size_bits = 0; (0x01 << handle->page_size_bits) < sysconf(_SC_PAGESIZE); handle->page_size_bits++);

        if (size & (handle->page_size_bits - 1)) {
                return NULL;
        }

        if ((unsigned int)addr & (handle->page_size_bits - 1)) {
                return NULL;
        }

        /* Initiate bitmap */
        initialize_header(handle, (unsigned int)addr, size);
        initialize_bitmap(handle);

        return handle;
}

int page_allocator_fini(struct page_allocator *handle)
{
        if (!handle) {
                return -EINVAL;
        }

        return 0;
}

static inline int validate_power_of_two(unsigned int nr_of_pages, unsigned int *order)
{
        unsigned int i;
        unsigned int _order;
        int ret;

        for (_order = 0, i = 1; i < nr_of_pages; i <<= 1, _order++) {
                /* Do nothing */
        }

        ret = (i == nr_of_pages);

        if (ret) {
                if (order) {
                        *order = _order;
                }
        }

        return ret;
}

void *page_allocator_alloc(struct page_allocator *handle, unsigned int nr_of_pages)
{
        unsigned int order;
        unsigned int idx;
        unsigned int i;
        unsigned int j;
        unsigned int total;

        if (!handle) {
                dbg_printf("Invalid handle\n");
                return (void *)-1;
        }

        if (!validate_power_of_two(nr_of_pages, &order)) {
                dbg_printf("Invalid nr_of_pages\n");
                return (void *)-1;
        }

        idx = 0x01 << (handle->max_order - order);
        total = idx << 1;
        idx--;
        total--;

        for (i = idx; i < total; i++) {
                if (handle->bitmap[BYTE_IDX(i)] & BIT_MASK(i)) {
                        handle->bitmap[BYTE_IDX(i)] &= ~BIT_MASK(i);
                        handle->bitmap[handle->bitmap_sz + BYTE_IDX(i)] |= BIT_MASK(i);
                        i = (i - idx) << order;

                        if (BYTE_IDX(i) >= handle->bitmap_sz) {
                                dbg_printf("Out of buffer\n");
                                return (void *)-1;
                        }
                        return (void *)(handle->base + (i << handle->page_size_bits));
                }
        }

        /* Try to split more bigger bit-group */
        j = order;
        while (j < handle->max_order) {
                j++;

                idx = 0x01 << (handle->max_order - j);
                total = idx << 1;
                idx--;
                total--;

                for (i = idx; i < total; i++) {
                        if (handle->bitmap[BYTE_IDX(i)] & BIT_MASK(i)) {
                                handle->bitmap[BYTE_IDX(i)] &= ~BIT_MASK(i);
                                break;
                        }
                }

                if (i == total) {
                        continue;
                }

                do {
                        i -= idx; /* Get the pure index. */
                        j--; /* Go to the lower order */

                        idx = (0x01 << (handle->max_order - j)) - 1;
                        i = idx + (i << 1);
                        handle->bitmap[BYTE_IDX(i + 1)] |= BIT_MASK(i + 1);
                } while (j != order);
                handle->bitmap[handle->bitmap_sz + BYTE_IDX(i)] |= BIT_MASK(i);
                i = (i - idx) << order;

                if (BYTE_IDX(i) >= handle->bitmap_sz) {
                        dbg_printf("Out of buffer\n");
                        return (void *)-1;
                }
                return (void *)(handle->base + (i << handle->page_size_bits));
        }

        dbg_printf("Failed to allocate\n");
        return (void *)-1;
}

int page_allocator_free(struct page_allocator *handle, void *addr)
{
        unsigned int order;
        unsigned int idx;
        unsigned int i;
        int ret = -ENOENT;

        if (!handle) {
                return -EINVAL;
        }

        for (order = 0; order <= handle->max_order; order++) {
                i = ((unsigned int)addr - handle->base) >> (handle->page_size_bits + order);
                idx = (0x01 << (handle->max_order - order)) - 1;

                if (handle->bitmap[handle->bitmap_sz + BYTE_IDX(idx + i)] & BIT_MASK(idx + i)) {
                        unsigned int buddy;

                        handle->bitmap[handle->bitmap_sz + BYTE_IDX(idx + i)] &= ~BIT_MASK(idx + i);

                        do {
                                buddy = idx + i + (((idx + i) & 0x01) ? 1 : -1);
                                if (buddy != (unsigned int)-1 && handle->bitmap[BYTE_IDX(buddy)] & BIT_MASK(buddy)) {
                                        handle->bitmap[BYTE_IDX(buddy)] &= ~BIT_MASK(buddy);
                                } else {
                                        handle->bitmap[BYTE_IDX(idx + i)] |= BIT_MASK(idx + i);
                                        ret = 0;
                                        break;
                                }

                                order++;
                                i = ((unsigned int)addr - handle->base) >> (handle->page_size_bits + order);
                                idx = (0x01 << (handle->max_order - order)) - 1;
                        } while (order < handle->max_order);

                        if (order == handle->max_order) {
                                handle->bitmap[BYTE_IDX(idx + i)] |= BIT_MASK(idx + i);
                                ret = 0;
                        }

                        break;
                }
        }

        /*
        if (ret != 0 && order == handle->max_order) {
                if (handle->bitmap[handle->bitmap_sz + BYTE_IDX(0)] & BIT_MASK(0)) {
                        handle->bitmap[handle->bitmap_sz + BYTE_IDX(0)] &= ~BIT_MASK(0);
                        handle->bitmap[BYTE_IDX(0)] |= BIT_MASK(0);
                        ret = 0;
                }
        }
        */

        return ret;
}

void page_allocator_dump_header(struct page_allocator *handle)
{
        dbg_printf("base: %x\n", handle->base);
        dbg_printf("The number of pages: %u\n", handle->nr_of_pages);
        dbg_printf("Max order: %u\n", handle->max_order);
        dbg_printf("Size of bitmap block: %u\n", handle->bitmap_sz);
#if defined(_DEBUG)
        debug_dump_bitmap(handle);
#endif
}

unsigned int page_allocator_info_size(unsigned int size)
{
        unsigned int nr_of_pages;
        unsigned int i;
        unsigned int page_size_bits;

        for (page_size_bits = 0; (0x01 << page_size_bits) < sysconf(_SC_PAGESIZE); page_size_bits++);

        if (size & (page_size_bits - 1)) {
                return 0;
        }

        nr_of_pages = size >> page_size_bits;
        for (i = 1; i < nr_of_pages; i <<= 1) {
        }

        return (i >> 1) + sizeof(struct page_allocator);
}

int page_allocator_get_region(struct page_allocator *handle, unsigned int *start, unsigned int *end)
{
        if (!handle) {
                return -EINVAL;
        }

        *start = handle->base;
        *end = handle->base + handle->nr_of_pages * sysconf(_SC_PAGESIZE);
        return 0;
}

/* End of a file */