drivers/net/wireless/rockchip_wlan/esp8089/esp_premalloc/esp_slab.c

   1 #ifdef ESP_SLAB
   2
   3 #include <linux/spinlock.h>
   4 #include <linux/slab.h>
   5 #include <linux/spinlock.h>
   6 #include <linux/module.h>
   7 #include "esp_slab.h"
   8 #include "esp_log.h"
   9
  10 static void  *gl_mem_p;
  11 static struct esp_mem_mgmt gl_mm;
  12
  13 int set_gl_mem_p(void *mem_p)
  14 {
  15         gl_mem_p = mem_p;
  16
  17         return 0;
  18 }
  19
  20 void *get_gl_mem_p(void)
  21 {
  22         return gl_mem_p;
  23 }
  24
  25 static inline int esp_clz(u32 x)
  26 {
  27         return __builtin_clz(x);
  28 }
  29
  30 static inline int esp_ctz(u32 x)
  31 {
  32         return __builtin_ctz(x);
  33 }
  34
  35 static inline int esp_popcount(u32 x)
  36 {
  37         int i;
  38
  39         i = 0;
  40         while (x) {
  41                 if (x&0x1)
  42                         i++;
  43                 x = x>>1;
  44         }
  45         return i;
  46         //return __builtin_popcount(x);
  47 }
  48
  49 /* bit_no value must be 0 */
  50 static int get_next_empty_num(u32* bit_map, int bit_map_size, int bit_no)
  51 {
  52         int i, x, offset;
  53         int sum;
  54
  55         i = (bit_no&0x111111e0)>>5;
  56         offset = (bit_no&0x0000001f);
  57
  58         x = esp_ctz(bit_map[i] >> offset);
  59         if (x < 32 - offset) {
  60                 return x;
  61         } else {
  62                 sum = 32 - offset;
  63                 i++;
  64         }
  65
  66         while (i < bit_map_size) {
  67                 x = esp_ctz(bit_map[i]);
  68                 if (x < 32) {
  69                         sum += x;
  70                         break;
  71                 } else {
  72                         sum += x;
  73                 }
  74                 i++;
  75         }
  76
  77         if (i == bit_map_size)
  78                 return -ERANGE;
  79
  80         return sum;
  81 }
  82
  83 /* this inline for ignore warning of unused */
  84 static inline int get_prev_empty_num(u32* bit_map, int bit_map_size, int bit_no)
  85 {
  86         int i, x, offset;
  87         int sum;
  88
  89         i = (bit_no&0x111111e0)>>5;
  90         offset = (bit_no&0x0000001f);
  91
  92         x = esp_clz(bit_map[i] << (32 - offset));
  93         if (x < offset) {
  94                 return x;
  95         } else {
  96                 sum = offset;
  97                 i--;
  98         }
  99
 100         while (i >= 0) {
 101                 x = esp_clz(bit_map[i]);
 102                 if (x < 32) {
 103                         sum += x;
 104                         break;
 105                 } else {
 106                         sum += x;
 107                 }
 108                 i--;
 109         }
 110
 111         if (i == bit_map_size)
 112                 return -ERANGE;
 113
 114         return sum;
 115 }
 116
 117
 118 static int find_1st_empty_pos(u32* bit_map, int bit_map_size, int start_bit_no)
 119 {
 120         int i, x, offset;
 121         int pos;
 122
 123         i = (start_bit_no&0x111111e0)>>5;  /* integer div 32 */
 124         offset = (start_bit_no&0x0000001f); /* mod 32 */
 125
 126         if (esp_popcount((u32)(bit_map[i]>>offset)) < 32 - offset) {
 127                 while ((x = esp_ctz(bit_map[i]>>offset)) == 0) {
 128                         offset++;
 129                 }
 130                 return (i<<5) + offset;
 131         } else {
 132                 i++;
 133                 pos = i<<5;
 134         }
 135
 136         while (i < bit_map_size) {
 137                 if (esp_popcount(bit_map[i]) < 32) {
 138                         offset = 0;
 139                         while ((x = esp_ctz(bit_map[i]>>offset)) == 0) {
 140                                 offset++;
 141                         }
 142                         pos += offset;
 143                         break;
 144                 } else {
 145                         i++;
 146                         pos = i<<5;
 147                 }
 148         }
 149
 150         if (i == bit_map_size)
 151                 return -ERANGE;
 152
 153         return pos;
 154 }
 155
 156 /* start_bit must be 0 */
 157 static void set_next_full(u32 *bit_map, int bit_map_size, int start_bit_no, int bit_count)
 158 {
 159         int i, offset;
 160
 161         i = (start_bit_no&0x111111e0)>>5;  /* integer div 32 */
 162         offset = (start_bit_no&0x0000001f); /* mod 32 */
 163
 164         if (bit_count > 32 - offset) {
 165                 bit_map[i] |= ~((1<<offset) - 1);
 166                 bit_count -= (bit_count - (32 - offset));
 167                 while (bit_count > 0) {
 168                         i++;
 169                         if (bit_count >= 32) {
 170                                 bit_map[i] |= 0xffffffff;
 171                         } else {
 172                                 bit_map[i] |= ((1<<bit_count) -1);
 173                         }
 174                         bit_count -= 32;
 175                 }
 176         } else {
 177                 bit_map[i] |= (((1<<bit_count) - 1) << offset);
 178         }
 179 }
 180
 181 /* start_bit must be 1 */
 182 static void set_next_empty(u32 *bit_map, int bit_map_size, int start_bit_no, int bit_count)
 183 {
 184         int i, offset;
 185
 186         i = (start_bit_no&0x111111e0)>>5;  /* integer div 32 */
 187         offset = (start_bit_no&0x0000001f); /* mod 32 */
 188
 189         if (bit_count > 32 - offset) {
 190                 bit_map[i] &= ((1<<offset) - 1);
 191                 bit_count -= (bit_count - (32 - offset));
 192                 while (bit_count > 0) {
 193                         i++;
 194                         if (bit_count >= 32) {
 195                                 bit_map[i] &= 0x00000000;
 196                         } else {
 197                                 bit_map[i] &= ~((1<<bit_count) -1);
 198                         }
 199                         bit_count -= 32;
 200                 }
 201         } else {
 202                 bit_map[i] &= ~(((1<<bit_count) - 1) << offset);
 203         }
 204 }
 205
 206
 207
 208 static inline int bin_roundup(int n, unsigned int div) /*div must be 2^n */
 209 {
 210         int x;
 211
 212         x = esp_ctz(div);
 213
 214         if (__builtin_popcount(div) > 1)
 215                 return -EINVAL;
 216
 217         return  (n & ((1<<x)-1) ? ((n>>x) + 1)<<x : n);
 218 }
 219
 220
 221 /* must use hash to make it faster, when have time*/
 222 static int set_access(struct esp_mem_mgmt_per *mmp, void *point, int bit_count)
 223 {
 224         int i;
 225
 226         if (!mmp)
 227                 return -EINVAL;
 228
 229         for (i = 0; i < DEFAULT_MAX_ACCESSES_PER; i++) {
 230                 if (mmp->macs[i].p == NULL) {
 231                         mmp->macs[i].p = point;
 232                         mmp->macs[i].pieces = bit_count;
 233                         break;
 234                 }
 235         }
 236
 237         if (i == 128)
 238                 return -ERANGE;
 239         return 0;
 240 }
 241
 242 static int get_clear_access(struct esp_mem_mgmt_per *mmp, void *point)
 243 {
 244         int pieces, i;
 245
 246         if (!mmp)
 247                 return -EINVAL;
 248
 249         for (i = 0; i < DEFAULT_MAX_ACCESSES_PER; i++) {
 250                 if (mmp->macs[i].p == point) {
 251                         pieces = mmp->macs[i].pieces;
 252                         mmp->macs[i].p = NULL;
 253                         mmp->macs[i].pieces = 0;
 254                         return pieces;
 255                 }
 256         }
 257
 258         return -ERANGE;
 259 }
 260
 261 static inline void *_esp_malloc(size_t size)
 262 {
 263         int pos;
 264         int empty_num;
 265         int roundup_size;
 266         void *p;
 267
 268         if (size >= LITTLE_LIMIT) {                        /* use large mem */
 269                 spin_lock_irqsave(&gl_mm.large_mmp.spin_lock, gl_mm.large_mmp.lock_flags);
 270                 pos = 0;
 271                 while (pos < LARGE_PIECES_NUM) {
 272                         if ((pos = find_1st_empty_pos(gl_mm.large_mmp.bit_map, LARGE_BIT_MAP_SIZE, pos)) >= 0) {
 273                                 logd("large find pos [%d]\n", pos);
 274                                 empty_num = get_next_empty_num(gl_mm.large_mmp.bit_map, LARGE_BIT_MAP_SIZE, pos);
 275                                 if ((empty_num<<LARGE_PIECE_SIZE_SHIFT) >= size) {
 276                                         roundup_size = bin_roundup(size, LARGE_PIECE_SIZE);
 277                                         logd("large roundup_size [%d]\n", roundup_size);
 278                                         set_next_full(gl_mm.large_mmp.bit_map, LARGE_BIT_MAP_SIZE, pos, roundup_size>>LARGE_PIECE_SIZE_SHIFT);
 279                                         p = (void *)((pos<<LARGE_PIECE_SIZE_SHIFT) + (u8 *)gl_mm.large_mmp.start_p);
 280                                         set_access(&gl_mm.large_mmp, p, roundup_size>>LARGE_PIECE_SIZE_SHIFT);
 281                                         spin_unlock_irqrestore(&gl_mm.large_mmp.spin_lock, gl_mm.large_mmp.lock_flags);
 282                                         return p;
 283                                 } else {
 284                                         pos += empty_num;
 285                                 }
 286                         } else {                           /* no enough memory */
 287                                 spin_unlock_irqrestore(&gl_mm.large_mmp.spin_lock, gl_mm.large_mmp.lock_flags);
 288                                 return NULL;
 289                         }
 290                 }
 291                 spin_unlock_irqrestore(&gl_mm.large_mmp.spin_lock, gl_mm.large_mmp.lock_flags);
 292
 293         } else {                                           /* use little mem */
 294                 spin_lock_irqsave(&gl_mm.little_mmp.spin_lock, gl_mm.little_mmp.lock_flags);
 295                 pos = 0;
 296                 while (pos < LITTLE_PIECES_NUM) {
 297                         if ((pos = find_1st_empty_pos(gl_mm.little_mmp.bit_map, LITTLE_BIT_MAP_SIZE, pos)) >= 0) {
 298                                 logd("little find pos [%d]\n", pos);
 299                                 empty_num = get_next_empty_num(gl_mm.little_mmp.bit_map, LITTLE_BIT_MAP_SIZE, pos);
 300                                 if ((empty_num<<LITTLE_PIECE_SIZE_SHIFT) >= size) {
 301                                         roundup_size = bin_roundup(size, LITTLE_PIECE_SIZE);
 302                                         logd("little roundup_size [%d]\n", roundup_size);
 303                                         set_next_full(gl_mm.little_mmp.bit_map, LITTLE_BIT_MAP_SIZE, pos, roundup_size>>LITTLE_PIECE_SIZE_SHIFT);
 304                                         p = (void *)((pos<<LITTLE_PIECE_SIZE_SHIFT) + (u8 *)gl_mm.little_mmp.start_p);
 305                                         set_access(&gl_mm.little_mmp, p, roundup_size>>LITTLE_PIECE_SIZE_SHIFT);
 306                                         spin_unlock_irqrestore(&gl_mm.little_mmp.spin_lock, gl_mm.little_mmp.lock_flags);
 307                                         return p;
 308                                 } else {
 309                                         pos += empty_num;
 310                                 }
 311                         } else {                           /* no enough memory */
 312                                 spin_unlock_irqrestore(&gl_mm.little_mmp.spin_lock, gl_mm.little_mmp.lock_flags);
 313                                 return NULL;
 314                         }
 315                 }
 316                 spin_unlock_irqrestore(&gl_mm.little_mmp.spin_lock, gl_mm.little_mmp.lock_flags);
 317         }
 318
 319         return NULL;
 320 }
 321
 322 static inline void _esp_free(void *p)
 323 {
 324         struct esp_mem_mgmt_per *mmp;
 325         int pieces;
 326         int start_pos;
 327
 328
 329         mmp = &gl_mm.large_mmp;
 330         spin_lock_irqsave(&mmp->spin_lock, mmp->lock_flags);
 331         pieces = get_clear_access(&gl_mm.large_mmp, p);
 332         logd("%s large pieces %d\n", __func__, pieces);
 333         if (pieces > 0) {
 334                 start_pos = ((p - mmp->start_p)>>LARGE_PIECE_SIZE_SHIFT);
 335                 logd("%s large pos %d\n", __func__, start_pos);
 336                 set_next_empty(mmp->bit_map, LARGE_BIT_MAP_SIZE, start_pos, pieces);
 337                 spin_unlock_irqrestore(&mmp->spin_lock, mmp->lock_flags);
 338                 return;
 339         } else
 340                 spin_unlock_irqrestore(&mmp->spin_lock, mmp->lock_flags);
 341
 342         mmp = &gl_mm.little_mmp;
 343         spin_lock_irqsave(&mmp->spin_lock, mmp->lock_flags);
 344         pieces = get_clear_access(&gl_mm.little_mmp, p);
 345         logd(KERN_ERR "%s little pieces %d\n", __func__, pieces);
 346         if (pieces > 0) {
 347                 start_pos = ((p - mmp->start_p)>>LITTLE_PIECE_SIZE_SHIFT);
 348                 logd(KERN_ERR "%s little pos %d\n", __func__, start_pos);
 349                 set_next_empty(mmp->bit_map, LITTLE_BIT_MAP_SIZE, start_pos, pieces);
 350                 spin_unlock_irqrestore(&mmp->spin_lock, mmp->lock_flags);
 351                 return;
 352         } else
 353                 spin_unlock_irqrestore(&mmp->spin_lock, mmp->lock_flags);
 354
 355         loge(KERN_ERR "%s point is not alloc from esp prealloc program\n", __func__);
 356
 357         return;
 358 }
 359
 360 #ifdef ESP_DEBUG
 361 void show_bitmap(void)
 362 {
 363         int i;
 364
 365         logi("-----LARGE BIT MAP-----\n");
 366         for (i = 0; i < LARGE_BIT_MAP_SIZE; i++) {
 367                 logi("%04d: 0x%08x\n", i, gl_mm.large_mmp.bit_map[i]);
 368         }
 369
 370         logi("-----LITTLE BIT MAP-----\n");
 371         for (i = 0; i < LITTLE_BIT_MAP_SIZE; i++) {
 372                 logi("%04d: 0x%08x\n", i, gl_mm.little_mmp.bit_map[i]);
 373         }
 374 }
 375 #endif
 376
 377 void *esp_malloc(size_t size)
 378 {
 379 #ifdef ESP_DEBUG
 380         void *p = _esp_malloc(size);
 381         show_bitmap();
 382         return p;
 383 #else
 384         return _esp_malloc(size);
 385 #endif
 386
 387 }
 388 EXPORT_SYMBOL(esp_malloc);
 389
 390 void esp_free(void *p)
 391 {
 392         _esp_free(p);
 393 #ifdef ESP_DEBUG
 394         show_bitmap();
 395 #endif
 396 }
 397 EXPORT_SYMBOL(esp_free);
 398
 399
 400 void *esp_pre_malloc(void)
 401 {
 402         int po;
 403
 404         po = get_order(PREMALLOC_SIZE);
 405         gl_mem_p = (void *)__get_free_pages(GFP_ATOMIC, po);
 406
 407         if (gl_mem_p == NULL) {
 408                 loge("%s no mem for premalloc! \n", __func__);
 409                 return NULL;
 410         }
 411
 412         return gl_mem_p;
 413 }
 414
 415 void esp_pre_free(void)
 416 {
 417         int po;
 418
 419         if (gl_mem_p == NULL) {
 420                 loge("%s no mem for prefree! \n", __func__);
 421                 return;
 422         }
 423
 424         po = get_order(PREMALLOC_SIZE);
 425         free_pages((unsigned long)gl_mem_p, po);
 426         gl_mem_p = NULL;
 427 }
 428
 429 int esp_mm_init(void)
 430 {
 431         memset(&gl_mm, 0x00, sizeof(struct esp_mem_mgmt));
 432
 433         spin_lock_init(&gl_mm.large_mmp.spin_lock);
 434         spin_lock_init(&gl_mm.little_mmp.spin_lock);
 435
 436         gl_mm.large_mmp.start_p = gl_mem_p; /* large */
 437         gl_mm.little_mmp.start_p = gl_mem_p + PREMALLOC_LARGE_SIZE; /* large */
 438
 439         if ((gl_mm.large_mmp.bit_map = (u32*)kzalloc(sizeof(u32)*LARGE_BIT_MAP_SIZE, GFP_KERNEL)) == NULL)
 440                 return -ENOMEM;
 441
 442         if ((gl_mm.little_mmp.bit_map = (u32*)kzalloc(sizeof(u32)*LITTLE_BIT_MAP_SIZE, GFP_KERNEL)) == NULL) {
 443                 kfree(gl_mm.large_mmp.bit_map);
 444                 return -ENOMEM;
 445         }
 446
 447 #ifdef ESP_DEBUG
 448         show_bitmap();
 449 #endif
 450         return 0;
 451 }
 452
 453 void esp_mm_deinit(void)
 454 {
 455         if (gl_mm.large_mmp.bit_map)
 456                 kfree(gl_mm.large_mmp.bit_map);
 457
 458         if (gl_mm.little_mmp.bit_map)
 459                 kfree(gl_mm.little_mmp.bit_map);
 460 }
 461
 462 int esp_slab_init(void)
 463 {
 464         int err = 0;
 465         if (esp_pre_malloc() ==  NULL)
 466                 err = -ENOMEM;
 467
 468         /*TODO:other mem mgr list , but i have no time to do this*/
 469         err = esp_mm_init();
 470
 471         return err;
 472 }
 473
 474 void esp_slab_deinit(void)
 475 {
 476         esp_pre_free();
 477         /*TODO:other mem mgr list , but i have no time to do this*/
 478         esp_mm_init();
 479 }
 480
 481 #endif /* ESP_SLAB */