qemu/roms/SLOF/slof/allocator.c

   1 /******************************************************************************
   2  * Copyright (c) 2013 IBM Corporation
   3  * All rights reserved.
   4  * This program and the accompanying materials
   5  * are made available under the terms of the BSD License
   6  * which accompanies this distribution, and is available at
   7  * http://www.opensource.org/licenses/bsd-license.php
   8  *
   9  * Contributors:
  10  *     IBM Corporation - initial implementation
  11  *****************************************************************************/
  12 /*
  13  * All functions concerning interface to slof
  14  */
  15 #include <stdio.h>
  16 #include <string.h>
  17 #include <stdlib.h>
  18 #include <stdbool.h>
  19 #include <helpers.h>
  20
  21 #undef DEBUG
  22 //#define DEBUG
  23 #ifdef DEBUG
  24 #define dprintf(_x ...) do { printf ("%s: ", __func__); printf(_x); } while (0);
  25 #else
  26 #define dprintf(_x ...)
  27 #endif
  28
  29 #define DEFAULT_BLOCK_SIZE 4096
  30
  31 #define BM_WORD_SIZE (sizeof(unsigned long))
  32 #define BM_WORD_BITS (BM_WORD_SIZE * 8)
  33
  34 struct bitmap {
  35         unsigned long start;
  36         unsigned long size;
  37         unsigned long bm_size;
  38         unsigned long block_size;
  39         unsigned long free_blocks;
  40         unsigned long bmw[];
  41 };
  42
  43 #define BIT(x)   (1UL << x)
  44 #define BM_WORD(bmw, n) (bmw[n/BM_WORD_BITS])
  45 #define BM_WORD_MODULO(n)  (n % BM_WORD_BITS)
  46 #define BM_NUM_BITS(reqsize, bsize)     ((reqsize / bsize) + (reqsize % bsize? 1 : 0))
  47
  48 void bm_clear_bit(unsigned long *bmw, int n)
  49 {
  50         BM_WORD(bmw, n) &= ~BIT(BM_WORD_MODULO(n));
  51 }
  52
  53 void bm_set_bit(unsigned long *bmw, int n)
  54 {
  55         BM_WORD(bmw, n) |= BIT(BM_WORD_MODULO(n));
  56 }
  57
  58 bool bm_test_bit(unsigned long *bmw, int n)
  59 {
  60 #ifdef DEBUG
  61         //printf("BMW %x, bitpos %d, value %d\n", &BM_WORD(bmw, n), n, !!(BM_WORD(bmw, n) & BIT(BM_WORD_MODULO(n))));
  62 #endif
  63         return !!(BM_WORD(bmw, n) & BIT(BM_WORD_MODULO(n)));
  64 }
  65
  66 /* Improvement: can use FFS routines to get faster results */
  67 int bm_find_bits(struct bitmap *bm, unsigned int n_bits)
  68 {
  69         unsigned int i, j, total_bits;
  70         int found = -1;
  71         dprintf("Finding %d bits set\n", n_bits);
  72         total_bits = BM_NUM_BITS(bm->size, bm->block_size);
  73         for(i = 0; i < total_bits; i++) {
  74                 if (!bm_test_bit(bm->bmw, i))
  75                         continue;
  76                 /* we have hit the boundary now, give up */
  77                 if (i + n_bits > total_bits)
  78                         break;
  79                 /* Lets find if we have consecutive bits set */
  80                 for(j = i; j < (i + n_bits); j++) {
  81                         if (!bm_test_bit(bm->bmw, (j)))
  82                                 break;
  83                 }
  84                 /* Got it */
  85                 if (j == (i + n_bits)) {
  86                         found = i;
  87                         break;
  88                 }
  89         }
  90         return found;
  91 }
  92
  93 void SLOF_bm_print(unsigned long handle)
  94 {
  95         struct bitmap *bm;
  96         unsigned int i;
  97
  98         if (!handle)
  99                 return;
 100
 101         bm = (struct bitmap *) handle;
 102         printf("BITMAP: start %lx, size %ld, blocksize %ld\n\n",
 103                 bm->start, bm->size, bm->block_size);
 104         printf("0                 16                32                48              63\n");
 105         for(i = 0; i < BM_NUM_BITS(bm->size, bm->block_size); i++) {
 106                 if (i > 0 && (i % 64 == 0))
 107                         printf("\n");
 108                 else if (i > 0 && (i % 8 == 0))
 109                         printf(" ");
 110                 printf("%d", bm_test_bit(bm->bmw, i));
 111         }
 112         printf("\n\n");
 113 }
 114
 115 unsigned long SLOF_bm_allocator_init(unsigned long start, unsigned long size,
 116                                 unsigned long blocksize)
 117 {
 118         struct bitmap *bm;
 119         unsigned long alloc_size, bm_size, n_bits;
 120
 121         dprintf("enter start %x, size %d, block-size %d\n", start, size, blocksize);
 122
 123         if (!size)
 124                 return 0;
 125         if (!blocksize)
 126                 blocksize = DEFAULT_BLOCK_SIZE;
 127
 128         n_bits = BM_NUM_BITS(size, blocksize);
 129         bm_size = (n_bits / BM_WORD_BITS) + ((n_bits % BM_WORD_BITS)? 1 : 0);
 130         alloc_size = sizeof(struct bitmap) + bm_size * BM_WORD_SIZE;
 131         dprintf("Size %ld, blocksize %ld, bm_size %ld, alloc_size %ld\n",
 132                 size, blocksize, bm_size, alloc_size);
 133         bm = (struct bitmap *) SLOF_alloc_mem(alloc_size);
 134         if (!bm)
 135                 return 0;
 136         bm->start = start;
 137         bm->size = size;
 138         bm->bm_size = bm_size;
 139         bm->block_size = blocksize;
 140         bm->free_blocks = n_bits;
 141         memset(bm->bmw, 0xFF, bm_size*BM_WORD_SIZE);
 142         return (unsigned long)bm;
 143 }
 144
 145 unsigned long SLOF_bm_alloc(unsigned long handle, unsigned long size)
 146 {
 147         struct bitmap *bm;
 148         unsigned long n_bits;
 149         unsigned long addr;
 150         unsigned int i;
 151         int bitpos;
 152
 153         if (!handle)
 154                 return -1;
 155
 156         bm = (struct bitmap *) handle;
 157
 158         n_bits = BM_NUM_BITS(size, bm->block_size);
 159         if (n_bits > bm->free_blocks)
 160                 return -1;
 161
 162         bitpos = bm_find_bits(bm, n_bits);
 163         if (bitpos == -1)
 164                 return -1;
 165
 166         dprintf("BMW %d, bitpos %d\n", i, bitpos);
 167         dprintf("size %d, block_size %d, n_bits %d\n", size, bm->block_size, n_bits);
 168         for(i = bitpos; i < (bitpos + n_bits); i++) {
 169 #ifdef DEBUG
 170                 if (!bm_test_bit(bm->bmw, i))
 171                         dprintf("Warning: Bit already in use: %d\n", i);
 172 #endif
 173                 bm_clear_bit(bm->bmw, i);
 174         }
 175         bm->free_blocks -= n_bits;
 176         addr = bm->start + bitpos * bm->block_size;
 177         dprintf("BMW %d, bitpos %d addr %lx free_blocks %d\n", i, bitpos, addr, bm->free_blocks);
 178         return addr;
 179 }
 180
 181 void SLOF_bm_free(unsigned long handle, unsigned long ptr, unsigned long size)
 182 {
 183         struct bitmap *bm;
 184         unsigned long bitpos, n_bits;
 185         unsigned long addr;
 186         unsigned int i;
 187
 188         if (!handle)
 189                 return;
 190
 191         bm = (struct bitmap *) handle;
 192         addr = (unsigned long ) ptr;
 193         n_bits = BM_NUM_BITS(size, bm->block_size);
 194         if (addr < bm->start || (bm->start + bm->size) < (addr + size)) {
 195                 printf("Error: Bitmap start %lx, size %ld, requested address %lx, size %ld\n",
 196                         bm->start, bm->size, addr, size);
 197                 return;
 198         }
 199         bitpos = (addr - bm->start) / bm->block_size;
 200         bm->free_blocks += n_bits;
 201
 202 #ifdef DEBUG
 203         dprintf("addr %lx, bitpos %d\n", addr, bitpos);
 204         dprintf("size %d, block_size %d, n_bits %d, free_blocks %d\n", size, bm->block_size, n_bits, bm->free_blocks);
 205         if (addr % bm->block_size) {
 206                 dprintf("Warning: Address not aligned addr %lx\n", addr);
 207         }
 208 #endif
 209
 210         for(i = bitpos; i < (bitpos + n_bits); i++) {
 211 #ifdef DEBUG
 212                 if (bm_test_bit(bm->bmw, i))
 213                         dprintf("Warning: Bit already set: %d\n", i);
 214 #endif
 215                 bm_set_bit(bm->bmw, i);
 216         }
 217
 218         return;
 219 }