1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054 |
- #include <common/idr.h>
- #include <mm/slab.h>
- static void __swap(struct idr_layer **a, struct idr_layer **b)
- {
- struct idr_layer *t = *a;
- *a = *b, *b = t;
- }
- void idr_init(struct idr *idp)
- {
- memset(idp, 0, sizeof(struct idr));
- spin_init(&idp->lock);
- }
- static void __move_to_free_list(struct idr *idp, struct idr_layer *p)
- {
- unsigned long flags;
- spin_lock_irqsave(&idp->lock, flags);
-
- p->ary[0] = idp->free_list;
- io_sfence();
- idp->free_list = p;
- io_sfence();
- ++(idp->id_free_cnt);
- spin_unlock_irqrestore(&idp->lock, flags);
- }
- static void *__get_from_free_list(struct idr *idp)
- {
- if (idp->id_free_cnt == 0)
- {
- if (idr_preload(idp, 0) != 0)
- {
- kBUG("idr-module find a BUG: get free node fail.(Possible ENOMEM error)");
- return NULL;
- }
- }
- unsigned long flags;
- spin_lock_irqsave(&idp->lock, flags);
-
- struct idr_layer *item = idp->free_list;
- if (item == NULL)
- {
- BUG_ON(1);
- }
- io_sfence();
- idp->free_list = idp->free_list->ary[0];
- io_sfence();
- item->ary[0] = NULL;
- io_sfence();
- --(idp->id_free_cnt);
- spin_unlock_irqrestore(&idp->lock, flags);
- return item;
- }
- int idr_preload(struct idr *idp, gfp_t gfp_mask)
- {
- int timer = 0;
- while (idp->id_free_cnt < IDR_FREE_MAX)
- {
- struct idr_layer *new_one;
- new_one = kzalloc(sizeof(struct idr_layer), gfp_mask);
- if (unlikely(new_one == NULL))
- return -ENOMEM;
- __move_to_free_list(idp, new_one);
- timer++;
- }
- return 0;
- }
- static void __idr_layer_free(struct idr_layer *p)
- {
- kfree(p);
- }
- static int __idr_grow(struct idr *idp)
- {
- struct idr_layer *new_node = __get_from_free_list(idp);
- if (NULL == new_node)
- return -ENOMEM;
- __swap(&new_node, &idp->top);
- idp->top->ary[0] = new_node;
- idp->top->layer = new_node ? (new_node->layer + 1) : 0;
- idp->top->bitmap = 0;
- idp->top->full = 0;
- if (new_node != NULL)
- {
- idp->top->bitmap = 1;
- }
- if (new_node != NULL && new_node->full == IDR_FULL)
- {
- idp->top->full = 1;
- }
- return 0;
- }
- static int __idr_get_empty_slot(struct idr *idp, struct idr_layer **stk)
- {
-
- while (NULL == idp->top || idp->top->full == IDR_FULL)
- if (__idr_grow(idp) != 0)
- return -ENOMEM;
- int64_t id = 0;
- int layer = idp->top->layer;
- BUG_ON(layer + 1 >= 7);
- stk[layer + 1] = NULL;
- struct idr_layer *cur_layer = idp->top;
- while (layer >= 0)
- {
- stk[layer] = cur_layer;
- int pos = __lowbit_id(~cur_layer->full);
- if (unlikely(pos < 0))
- {
- kBUG("Value 'cur_layer->full' had been full;"
- "but __idr_get_empty_slot still try to insert a value.");
- }
- id = (id << IDR_BITS) | pos;
- cur_layer = cur_layer->ary[pos];
- if (layer > 0 && NULL == cur_layer)
- {
-
- cur_layer = __get_from_free_list(idp);
- if (NULL == cur_layer)
- return -ENOMEM;
- cur_layer->layer = layer - 1;
- cur_layer->full = 0;
- cur_layer->bitmap = 0;
- stk[layer]->ary[pos] = cur_layer;
- }
- --layer;
- }
- return id;
- }
- static __always_inline void __idr_mark_full(struct idr *idp, int id, struct idr_layer **stk, int mark)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(NULL == stk[0] || NULL == idp->top))
- {
- kBUG("idr-module find a BUG: idp->top can't be NULL.");
- return;
- }
-
- int64_t layer_id = __id & IDR_MASK;
- if (mark == 2)
- stk[0]->full |= (1ull << layer_id);
- if (mark >= 1)
- stk[0]->bitmap |= (1ull << layer_id);
- for (int i = 1; stk[i]; ++i)
- {
- __id >>= IDR_BITS;
- layer_id = __id & IDR_MASK;
- stk[i]->bitmap |= (1ull << layer_id);
- if (stk[i - 1]->full == IDR_FULL)
- stk[i]->full |= (1ull << layer_id);
- }
- }
- static __always_inline int __idr_get_path(struct idr *idp, int id, struct idr_layer **stk)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(idp->top == NULL || __id < 0))
- {
- kBUG("idr-module find a BUG: idp->top can't be NULL and id must be non-negative.");
- return 0;
- }
- struct idr_layer *cur_layer = idp->top;
- int layer = cur_layer->layer;
- stk[layer + 1] = NULL;
- if (unlikely((__id >> ((layer + 1ull) * IDR_BITS)) > 0))
- {
- kBUG("idr-module find a BUG: id is invalid.");
- return 0;
- }
-
- while (layer >= 0)
- {
- stk[layer] = cur_layer;
- int64_t layer_id = (__id >> (layer * IDR_BITS)) & IDR_MASK;
- if (unlikely(((cur_layer->bitmap >> layer_id) & 1) == 0))
- {
- kBUG("idr-module find a BUG: no-such son.");
- return 0;
- }
- cur_layer = cur_layer->ary[layer_id];
- --layer;
- }
- return 1;
- }
- static __always_inline void __idr_erase_full(struct idr *idp, int id, struct idr_layer **stk, int mark)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(NULL == stk[0] || NULL == idp->top))
- {
- kBUG("idr-module find a BUG: idp->top can't be NULL.");
- return;
- }
-
- int64_t layer_id = __id & IDR_MASK;
- if (mark == 0)
- {
- stk[0]->ary[layer_id] = NULL;
- stk[0]->bitmap ^= (1ull << layer_id);
- }
- if (mark != 2 && ((stk[0]->full >> layer_id) & 1))
- stk[0]->full ^= (1ull << layer_id);
-
- for (int layer = 1; stk[layer]; ++layer)
- {
- __id >>= IDR_BITS;
- layer_id = __id & IDR_MASK;
- if (NULL == stk[layer - 1]->bitmap)
- {
- stk[layer]->ary[layer_id] = NULL;
- stk[layer]->bitmap ^= (1ull << layer_id);
- if ((stk[layer]->full >> layer_id) & 1)
- stk[layer]->full ^= (1ull << layer_id);
- __idr_layer_free(stk[layer - 1]);
- stk[layer - 1] = NULL;
- }
- else if (stk[layer - 1]->full != IDR_FULL)
- {
- if ((stk[layer]->full >> layer_id) & 1)
- stk[layer]->full ^= (1ull << layer_id);
- }
- }
-
-
-
- while (idp->top != NULL && ((idp->top->bitmap <= 1 && idp->top->layer > 0) ||
- (idp->top->layer == 0 && idp->top->bitmap == 0)))
- {
- struct idr_layer *t = idp->top->layer ? idp->top->ary[0] : NULL;
- __idr_layer_free(idp->top);
- idp->top = t;
- }
- }
- static int __idr_get_new_above_int(struct idr *idp, void *ptr, int starting_id)
- {
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
-
-
-
- int64_t id = __idr_get_empty_slot(idp, stk);
- if (id >= 0)
- {
- stk[0]->ary[IDR_MASK & id] = ptr;
- __idr_mark_full(idp, id, stk, 2);
- }
- return id;
- }
- int idr_alloc(struct idr *idp, void *ptr, int *id)
- {
- int rv = __idr_get_new_above_int(idp, ptr, 0);
- if (rv < 0)
- return rv;
- *id = rv;
- return 0;
- }
- void *idr_remove(struct idr *idp, int id)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(idp->top == NULL || __id < 0))
- return NULL;
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
- if (0 == __idr_get_path(idp, __id, stk))
- return NULL;
- void *ret = stk[0]->ary[__id & IDR_MASK];
- __idr_erase_full(idp, __id, stk, 0);
- return ret;
- }
- static void __idr_remove_all_with_free(struct idr *idp, bool free)
- {
- if (unlikely(NULL == idp->top))
- {
- kBUG("idr-module find a BUG: idp->top can't be NULL.");
- return;
- }
- int sz = sizeof(struct idr_layer);
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
- struct idr_layer *cur_layer = idp->top;
- int layer = cur_layer->layer;
- BUG_ON(layer + 1 >= 7);
- stk[layer + 1] = NULL;
- while (cur_layer != NULL)
- {
- if (layer > 0 && cur_layer->bitmap)
- {
- stk[layer] = cur_layer;
- int64_t id = __lowbit_id(cur_layer->bitmap);
- cur_layer->bitmap ^= (1ull << id);
- cur_layer = cur_layer->ary[id];
- stk[layer]->ary[id] = NULL;
- --layer;
- }
- else
- {
- if (free)
- {
- for (int i = 0; i < IDR_SIZE; i++)
- {
- kfree(cur_layer->ary[i]);
- cur_layer->ary[i] = NULL;
- }
- }
- __idr_layer_free(cur_layer);
- ++layer;
- cur_layer = stk[layer];
- }
- }
- idp->top = NULL;
- }
- static void __idr_destroy_with_free(struct idr *idp)
- {
- if (likely(idp->top))
- __idr_remove_all_with_free(idp, 1);
- idp->top = NULL;
- while (idp->id_free_cnt)
- __idr_layer_free(__get_from_free_list(idp));
- idp->free_list = NULL;
- }
- void idr_remove_all(struct idr *idp)
- {
- if (unlikely(NULL == idp->top))
- return;
- __idr_remove_all_with_free(idp, 0);
- }
- void idr_destroy(struct idr *idp)
- {
- idr_remove_all(idp);
- idp->top = NULL;
- while (idp->id_free_cnt)
- __idr_layer_free(__get_from_free_list(idp));
- idp->free_list = NULL;
- }
- void *idr_find(struct idr *idp, int id)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(idp->top == NULL || __id < 0))
- {
-
- return NULL;
- }
- struct idr_layer *cur_layer = idp->top;
- int layer = cur_layer->layer;
- barrier();
-
- if ((__id >> ((layer + 1) * IDR_BITS)) > 0)
- return NULL;
- barrier();
- barrier();
- int64_t layer_id = 0;
- while (layer >= 0 && cur_layer != NULL)
- {
- barrier();
- layer_id = (__id >> (IDR_BITS * layer)) & IDR_MASK;
- barrier();
- cur_layer = cur_layer->ary[layer_id];
- --layer;
- }
- return cur_layer;
- }
- void *idr_find_next_getid(struct idr *idp, int64_t start_id, int *nextid)
- {
- BUG_ON(nextid == NULL);
- if (unlikely(idp->top == NULL))
- {
- *nextid = -1;
- return NULL;
- }
- ++start_id;
- start_id = max(0, start_id);
- *nextid = 0;
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
-
- bool state[MAX_LEVEL + 1] = {0};
- int pos_i[MAX_LEVEL + 1] = {0};
-
-
- struct idr_layer *cur_layer = idp->top;
- bool cur_state = false;
- bool init_flag = true;
- int layer = cur_layer->layer;
- BUG_ON(layer + 1 >= 7);
- stk[layer + 1] = NULL;
-
- if ((start_id >> ((layer + 1) * IDR_BITS)) > 0)
- {
- *nextid = -1;
- return NULL;
- }
- while (cur_layer)
- {
- BUG_ON(layer < 0);
- if (init_flag)
- {
- stk[layer] = cur_layer;
- state[layer] = cur_state;
- pos_i[layer] = cur_state ? 0 : ((start_id >> (layer * IDR_BITS)) & IDR_MASK);
- }
- else
- {
- pos_i[layer]++;
- state[layer] = cur_state = true;
- }
- BUG_ON(pos_i[layer] >= 64);
- unsigned long t_bitmap = (cur_layer->bitmap >> pos_i[layer]);
- if (t_bitmap)
- {
- int64_t layer_id = __lowbit_id(t_bitmap) + pos_i[layer];
-
- if ((cur_state == false) && (layer_id > pos_i[layer] > 0))
- cur_state = true;
- pos_i[layer] = layer_id;
- *nextid = (((uint64_t)*nextid) << IDR_BITS) | layer_id;
- if (layer == 0)
- {
-
- return cur_layer->ary[layer_id];
- }
- cur_layer = cur_layer->ary[layer_id];
- init_flag = true;
- --layer;
- }
- else
- {
- (*nextid) >>= IDR_BITS;
- ++layer;
- cur_layer = stk[layer];
- init_flag = false;
- }
- }
- *nextid = -1;
- return NULL;
- }
- void *idr_find_next(struct idr *idp, int start_id)
- {
- int nextid;
- void *ptr = idr_find_next_getid(idp, start_id, &nextid);
- return ptr;
- }
- int idr_replace_get_old(struct idr *idp, void *ptr, int id, void **old_ptr)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(old_ptr == NULL))
- {
- BUG_ON(1);
- return -EINVAL;
- }
- *old_ptr = NULL;
- if (unlikely(idp->top == NULL || __id < 0))
- return -EDOM;
- struct idr_layer *cur_layer = idp->top;
- int64_t layer = cur_layer->layer;
-
- if ((__id >> ((layer + 1) * IDR_BITS)) > 0)
- return -EDOM;
- while (layer > 0)
- {
- int64_t layer_id = (__id >> (layer * IDR_BITS)) & IDR_MASK;
- if (unlikely(NULL == cur_layer->ary[layer_id]))
- return -ENOMEM;
- cur_layer = cur_layer->ary[layer_id];
- layer--;
- }
- __id &= IDR_MASK;
- *old_ptr = cur_layer->ary[__id];
- cur_layer->ary[__id] = ptr;
- return 0;
- }
- int idr_replace(struct idr *idp, void *ptr, int id)
- {
- int64_t __id = (int64_t)id;
- if (__id < 0)
- return -EDOM;
- void *old_ptr;
- int flags = idr_replace_get_old(idp, ptr, __id, &old_ptr);
- return flags;
- }
- bool idr_empty(struct idr *idp)
- {
- if (idp == NULL || idp->top == NULL || !idp->top->bitmap)
- return true;
- return false;
- }
- static bool __idr_cnt_pd(struct idr_layer *cur_layer, int layer_id)
- {
-
- unsigned long flags = ((cur_layer->bitmap) >> layer_id);
- if ((flags % 2) == 0)
- {
- barrier();
- return false;
- }
- return true;
- }
- static bool __idr_cnt(int layer, int id, struct idr_layer *cur_layer)
- {
- int64_t __id = (int64_t)id;
- while (layer >= 0)
- {
- barrier();
- int64_t layer_id = (__id >> (layer * IDR_BITS)) & IDR_MASK;
- barrier();
- if (__idr_cnt_pd(cur_layer, layer_id) == false)
- return false;
- barrier();
- barrier();
- cur_layer = cur_layer->ary[layer_id];
- barrier();
- --layer;
- }
- return true;
- }
- bool idr_count(struct idr *idp, int id)
- {
- int64_t __id = (int64_t)id;
- barrier();
- if (unlikely(idp == NULL || idp->top == NULL || __id < 0))
- return false;
- barrier();
- struct idr_layer *cur_layer = idp->top;
- barrier();
- int layer = cur_layer->layer;
-
- if (unlikely((__id >> ((layer + 1ull) * IDR_BITS)) > 0))
- {
- BUG_ON(1);
- return false;
- }
- barrier();
- return __idr_cnt(layer, id, cur_layer);
- }
- void ida_init(struct ida *ida_p)
- {
- memset(ida_p, 0, sizeof(struct ida));
- idr_init(&ida_p->idr);
- }
- static void __ida_bitmap_free(struct ida_bitmap *bitmap)
- {
- kfree(bitmap);
- }
- int ida_preload(struct ida *ida_p, gfp_t gfp_mask)
- {
- if (idr_preload(&ida_p->idr, gfp_mask) != 0)
- return -ENOMEM;
- spin_lock(&ida_p->idr.lock);
- if (NULL == ida_p->free_list)
- {
- struct ida_bitmap *bitmap;
- bitmap = kzalloc(sizeof(struct ida_bitmap), gfp_mask);
- if (NULL == bitmap)
- {
- spin_unlock(&ida_p->idr.lock);
- return -ENOMEM;
- }
- ida_p->free_list = bitmap;
- }
- spin_unlock(&ida_p->idr.lock);
- return 0;
- }
- static void *__get_ida_bitmap(struct ida *ida_p, gfp_t gfp_mask)
- {
- if (NULL == ida_p->free_list)
- if (ida_preload(ida_p, gfp_mask) < 0)
- {
- kBUG("error : no memory.");
- return NULL;
- }
- struct ida_bitmap *tmp = ida_p->free_list;
- ida_p->free_list = NULL;
- return tmp;
- }
- static int __get_id_from_bitmap(struct ida_bitmap *bmp)
- {
- int ret = 0;
- for (int ary_id = 0; ary_id < IDA_BITMAP_LONGS; ary_id++)
- {
- if (bmp->bitmap[ary_id] != IDR_FULL)
- {
- int bmp_id = __lowbit_id(~bmp->bitmap[ary_id]);
- bmp->bitmap[ary_id] |= (1ull << bmp_id);
- bmp->count++;
- if (unlikely((unsigned long long)ary_id * IDA_BMP_SIZE + bmp_id > INT32_MAX))
- {
- BUG_ON(1);
-
- return -EDOM;
- }
- return ary_id * IDA_BMP_SIZE + bmp_id;
- }
- }
- return -EDOM;
- }
- int ida_alloc(struct ida *ida_p, int *p_id)
- {
- BUG_ON(p_id == NULL);
- *p_id = -1;
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
-
- io_sfence();
- int64_t idr_id = __idr_get_empty_slot(&ida_p->idr, stk);
-
- if (unlikely(NULL == stk[0]))
- return -ENOMEM;
- if (unlikely(idr_id < 0))
- return idr_id;
- int64_t layer_id = idr_id & IDR_MASK;
- if (NULL == stk[0]->ary[layer_id])
- stk[0]->ary[layer_id] = __get_ida_bitmap(ida_p, 0);
- if (unlikely(NULL == stk[0]->ary[layer_id]))
- return -ENOMEM;
- struct ida_bitmap *bmp = (struct ida_bitmap *)stk[0]->ary[layer_id];
- int low_id = __get_id_from_bitmap(bmp);
- if (unlikely(low_id < 0))
- return low_id;
- *p_id = idr_id * IDA_BITMAP_BITS + low_id;
- __idr_mark_full(&ida_p->idr, idr_id, stk, (bmp->count == IDA_FULL ? 2 : 1));
- return 0;
- }
- bool ida_count(struct ida *ida_p, int id)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(NULL == ida_p || NULL == ida_p->idr.top || id < 0))
- return false;
- int idr_id = __id / IDA_BITMAP_BITS;
- int ary_id = (__id % IDA_BITMAP_BITS) / IDA_BMP_SIZE;
- int bmp_id = (__id % IDA_BITMAP_BITS) % IDA_BMP_SIZE;
- struct ida_bitmap *bmp = idr_find(&ida_p->idr, idr_id);
- if (NULL == bmp)
- return false;
- return ((bmp->bitmap[ary_id] >> bmp_id) & 1);
- }
- void ida_remove(struct ida *ida_p, int id)
- {
- int64_t __id = (int64_t)id;
- if (unlikely(NULL == ida_p || NULL == ida_p->idr.top || id < 0))
- return;
- int64_t idr_id = __id / IDA_BITMAP_BITS;
- int64_t ary_id = (__id % IDA_BITMAP_BITS) / IDA_BMP_SIZE;
- int64_t bmp_id = (__id % IDA_BITMAP_BITS) % IDA_BMP_SIZE;
- struct idr_layer *stk[MAX_LEVEL + 1] = {0};
-
- if (0 == __idr_get_path(&ida_p->idr, idr_id, stk))
- return;
- struct ida_bitmap *b_p = (struct ida_bitmap *)(stk[0]->ary[idr_id & IDR_MASK]);
-
- if (unlikely(NULL == b_p || 0 == ((b_p->bitmap[ary_id] >> bmp_id) & 1)))
- return;
- b_p->count--;
- b_p->bitmap[ary_id] ^= (1ull << bmp_id);
- __idr_erase_full(&ida_p->idr, idr_id, stk, (b_p->count > 0 ? 1 : 0));
- if (0 == b_p->count)
- {
- __ida_bitmap_free(b_p);
- if (stk[0])
- stk[0]->ary[idr_id & IDR_MASK] = NULL;
- }
- }
- void ida_destroy(struct ida *ida_p)
- {
- if (unlikely(ida_p == NULL))
- {
- BUG_ON(1);
- return;
- }
- __idr_destroy_with_free(&ida_p->idr);
- ida_p->idr.top = NULL;
- __ida_bitmap_free(ida_p->free_list);
- ida_p->free_list = NULL;
- }
- bool ida_empty(struct ida *ida_p)
- {
- if (ida_p == NULL || ida_p->idr.top == NULL || !ida_p->idr.top->bitmap)
- return true;
- return false;
- }
|