Subversion Repositories Kolibri OS

Rev

Rev 4103 | Rev 5270 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. /*
  2.  * 2002-10-18  written by Jim Houston jim.houston@ccur.com
  3.  *      Copyright (C) 2002 by Concurrent Computer Corporation
  4.  *      Distributed under the GNU GPL license version 2.
  5.  *
  6.  * Modified by George Anzinger to reuse immediately and to use
  7.  * find bit instructions.  Also removed _irq on spinlocks.
  8.  *
  9.  * Modified by Nadia Derbey to make it RCU safe.
  10.  *
  11.  * Small id to pointer translation service.
  12.  *
  13.  * It uses a radix tree like structure as a sparse array indexed
  14.  * by the id to obtain the pointer.  The bitmap makes allocating
  15.  * a new id quick.
  16.  *
  17.  * You call it to allocate an id (an int) an associate with that id a
  18.  * pointer or what ever, we treat it as a (void *).  You can pass this
  19.  * id to a user for him to pass back at a later time.  You then pass
  20.  * that id to this code and it returns your pointer.
  21.  */
  22.  
  23. #include <linux/kernel.h>
  24. #include <linux/export.h>
  25. #include <linux/string.h>
  26. #include <linux/bitops.h>
  27. #include <linux/idr.h>
  28. //#include <stdlib.h>
  29.  
  30. static inline void * __must_check ERR_PTR(long error)
  31. {
  32.         return (void *) error;
  33. }
  34.  
  35. unsigned long find_next_zero_bit(const unsigned long *addr, unsigned long size,
  36.                                  unsigned long offset);
  37.  
  38.  
  39. #define MAX_IDR_SHIFT           (sizeof(int) * 8 - 1)
  40. #define MAX_IDR_BIT             (1U << MAX_IDR_SHIFT)
  41.  
  42. /* Leave the possibility of an incomplete final layer */
  43. #define MAX_IDR_LEVEL ((MAX_IDR_SHIFT + IDR_BITS - 1) / IDR_BITS)
  44.  
  45. /* Number of id_layer structs to leave in free list */
  46. #define MAX_IDR_FREE (MAX_IDR_LEVEL * 2)
  47.  
  48. static struct idr_layer *idr_preload_head;
  49. static int idr_preload_cnt;
  50.  
  51. static DEFINE_SPINLOCK(simple_ida_lock);
  52.  
  53. /* the maximum ID which can be allocated given idr->layers */
  54. static int idr_max(int layers)
  55. {
  56.         int bits = min_t(int, layers * IDR_BITS, MAX_IDR_SHIFT);
  57.  
  58.         return (1 << bits) - 1;
  59. }
  60.  
  61. /*
  62.  * Prefix mask for an idr_layer at @layer.  For layer 0, the prefix mask is
  63.  * all bits except for the lower IDR_BITS.  For layer 1, 2 * IDR_BITS, and
  64.  * so on.
  65.  */
  66. static int idr_layer_prefix_mask(int layer)
  67. {
  68.         return ~idr_max(layer + 1);
  69. }
  70.  
  71. static struct idr_layer *get_from_free_list(struct idr *idp)
  72. {
  73.         struct idr_layer *p;
  74.         unsigned long flags;
  75.  
  76.         spin_lock_irqsave(&idp->lock, flags);
  77.         if ((p = idp->id_free)) {
  78.                 idp->id_free = p->ary[0];
  79.                 idp->id_free_cnt--;
  80.                 p->ary[0] = NULL;
  81.         }
  82.         spin_unlock_irqrestore(&idp->lock, flags);
  83.         return(p);
  84. }
  85.  
  86. /**
  87.  * idr_layer_alloc - allocate a new idr_layer
  88.  * @gfp_mask: allocation mask
  89.  * @layer_idr: optional idr to allocate from
  90.  *
  91.  * If @layer_idr is %NULL, directly allocate one using @gfp_mask or fetch
  92.  * one from the per-cpu preload buffer.  If @layer_idr is not %NULL, fetch
  93.  * an idr_layer from @idr->id_free.
  94.  *
  95.  * @layer_idr is to maintain backward compatibility with the old alloc
  96.  * interface - idr_pre_get() and idr_get_new*() - and will be removed
  97.  * together with per-pool preload buffer.
  98.  */
  99. static struct idr_layer *idr_layer_alloc(gfp_t gfp_mask, struct idr *layer_idr)
  100. {
  101.         struct idr_layer *new;
  102.  
  103.         /* this is the old path, bypass to get_from_free_list() */
  104.         if (layer_idr)
  105.                 return get_from_free_list(layer_idr);
  106.  
  107.         /* try to allocate directly from kmem_cache */
  108.         new = kzalloc(sizeof(struct idr_layer), gfp_mask);
  109.         if (new)
  110.                 return new;
  111.  
  112.  
  113.         new = idr_preload_head;
  114.         if (new) {
  115.                 idr_preload_head = new->ary[0];
  116.                 idr_preload_cnt--;
  117.                 new->ary[0] = NULL;
  118.         }
  119.         preempt_enable();
  120.         return new;
  121. }
  122.  
  123. static void idr_layer_rcu_free(struct rcu_head *head)
  124. {
  125.         struct idr_layer *layer;
  126.  
  127.     layer = container_of(head, struct idr_layer, rcu_head);
  128.     kfree(layer);
  129. }
  130.  
  131. static inline void free_layer(struct idr *idr, struct idr_layer *p)
  132. {
  133.         if (idr->hint == p)
  134.                 RCU_INIT_POINTER(idr->hint, NULL);
  135.     idr_layer_rcu_free(&p->rcu_head);
  136. }
  137.  
  138. /* only called when idp->lock is held */
  139. static void __move_to_free_list(struct idr *idp, struct idr_layer *p)
  140. {
  141.         p->ary[0] = idp->id_free;
  142.         idp->id_free = p;
  143.         idp->id_free_cnt++;
  144. }
  145.  
  146. static void move_to_free_list(struct idr *idp, struct idr_layer *p)
  147. {
  148.         unsigned long flags;
  149.  
  150.         /*
  151.          * Depends on the return element being zeroed.
  152.          */
  153.         spin_lock_irqsave(&idp->lock, flags);
  154.         __move_to_free_list(idp, p);
  155.         spin_unlock_irqrestore(&idp->lock, flags);
  156. }
  157.  
  158. static void idr_mark_full(struct idr_layer **pa, int id)
  159. {
  160.         struct idr_layer *p = pa[0];
  161.         int l = 0;
  162.  
  163.         __set_bit(id & IDR_MASK, p->bitmap);
  164.         /*
  165.          * If this layer is full mark the bit in the layer above to
  166.          * show that this part of the radix tree is full.  This may
  167.          * complete the layer above and require walking up the radix
  168.          * tree.
  169.          */
  170.         while (bitmap_full(p->bitmap, IDR_SIZE)) {
  171.                 if (!(p = pa[++l]))
  172.                         break;
  173.                 id = id >> IDR_BITS;
  174.                 __set_bit((id & IDR_MASK), p->bitmap);
  175.         }
  176. }
  177.  
  178. static int __idr_pre_get(struct idr *idp, gfp_t gfp_mask)
  179. {
  180.         while (idp->id_free_cnt < MAX_IDR_FREE) {
  181.        struct idr_layer *new;
  182.        new = kzalloc(sizeof(struct idr_layer), gfp_mask);
  183.        if (new == NULL)
  184.            return (0);
  185.        move_to_free_list(idp, new);
  186.    }
  187.    return 1;
  188. }
  189.  
  190. /**
  191.  * sub_alloc - try to allocate an id without growing the tree depth
  192.  * @idp: idr handle
  193.  * @starting_id: id to start search at
  194.  * @pa: idr_layer[MAX_IDR_LEVEL] used as backtrack buffer
  195.  * @gfp_mask: allocation mask for idr_layer_alloc()
  196.  * @layer_idr: optional idr passed to idr_layer_alloc()
  197.  *
  198.  * Allocate an id in range [@starting_id, INT_MAX] from @idp without
  199.  * growing its depth.  Returns
  200.  *
  201.  *  the allocated id >= 0 if successful,
  202.  *  -EAGAIN if the tree needs to grow for allocation to succeed,
  203.  *  -ENOSPC if the id space is exhausted,
  204.  *  -ENOMEM if more idr_layers need to be allocated.
  205.  */
  206. static int sub_alloc(struct idr *idp, int *starting_id, struct idr_layer **pa,
  207.                      gfp_t gfp_mask, struct idr *layer_idr)
  208. {
  209.         int n, m, sh;
  210.         struct idr_layer *p, *new;
  211.         int l, id, oid;
  212.  
  213.         id = *starting_id;
  214.  restart:
  215.         p = idp->top;
  216.         l = idp->layers;
  217.         pa[l--] = NULL;
  218.         while (1) {
  219.                 /*
  220.                  * We run around this while until we reach the leaf node...
  221.                  */
  222.                 n = (id >> (IDR_BITS*l)) & IDR_MASK;
  223.                 m = find_next_zero_bit(p->bitmap, IDR_SIZE, n);
  224.                 if (m == IDR_SIZE) {
  225.                         /* no space available go back to previous layer. */
  226.                         l++;
  227.                         oid = id;
  228.                         id = (id | ((1 << (IDR_BITS * l)) - 1)) + 1;
  229.  
  230.                         /* if already at the top layer, we need to grow */
  231.                         if (id > idr_max(idp->layers)) {
  232.                                 *starting_id = id;
  233.                                 return -EAGAIN;
  234.                         }
  235.                         p = pa[l];
  236.                         BUG_ON(!p);
  237.  
  238.                         /* If we need to go up one layer, continue the
  239.                          * loop; otherwise, restart from the top.
  240.                          */
  241.                         sh = IDR_BITS * (l + 1);
  242.                         if (oid >> sh == id >> sh)
  243.                                 continue;
  244.                         else
  245.                                 goto restart;
  246.                 }
  247.                 if (m != n) {
  248.                         sh = IDR_BITS*l;
  249.                         id = ((id >> sh) ^ n ^ m) << sh;
  250.                 }
  251.                 if ((id >= MAX_IDR_BIT) || (id < 0))
  252.                         return -ENOSPC;
  253.                 if (l == 0)
  254.                         break;
  255.                 /*
  256.                  * Create the layer below if it is missing.
  257.                  */
  258.                 if (!p->ary[m]) {
  259.                         new = idr_layer_alloc(gfp_mask, layer_idr);
  260.                         if (!new)
  261.                                 return -ENOMEM;
  262.                         new->layer = l-1;
  263.                         new->prefix = id & idr_layer_prefix_mask(new->layer);
  264.                         rcu_assign_pointer(p->ary[m], new);
  265.                         p->count++;
  266.                 }
  267.                 pa[l--] = p;
  268.                 p = p->ary[m];
  269.         }
  270.  
  271.         pa[l] = p;
  272.         return id;
  273. }
  274.  
  275. static int idr_get_empty_slot(struct idr *idp, int starting_id,
  276.                               struct idr_layer **pa, gfp_t gfp_mask,
  277.                               struct idr *layer_idr)
  278. {
  279.         struct idr_layer *p, *new;
  280.         int layers, v, id;
  281.         unsigned long flags;
  282.  
  283.         id = starting_id;
  284. build_up:
  285.         p = idp->top;
  286.         layers = idp->layers;
  287.         if (unlikely(!p)) {
  288.                 if (!(p = idr_layer_alloc(gfp_mask, layer_idr)))
  289.                         return -ENOMEM;
  290.                 p->layer = 0;
  291.                 layers = 1;
  292.         }
  293.         /*
  294.          * Add a new layer to the top of the tree if the requested
  295.          * id is larger than the currently allocated space.
  296.          */
  297.         while (id > idr_max(layers)) {
  298.                 layers++;
  299.                 if (!p->count) {
  300.                         /* special case: if the tree is currently empty,
  301.                          * then we grow the tree by moving the top node
  302.                          * upwards.
  303.                          */
  304.                         p->layer++;
  305.                         WARN_ON_ONCE(p->prefix);
  306.                         continue;
  307.                 }
  308.                 if (!(new = idr_layer_alloc(gfp_mask, layer_idr))) {
  309.                         /*
  310.                          * The allocation failed.  If we built part of
  311.                          * the structure tear it down.
  312.                          */
  313.                         spin_lock_irqsave(&idp->lock, flags);
  314.                         for (new = p; p && p != idp->top; new = p) {
  315.                                 p = p->ary[0];
  316.                                 new->ary[0] = NULL;
  317.                                 new->count = 0;
  318.                                 bitmap_clear(new->bitmap, 0, IDR_SIZE);
  319.                                 __move_to_free_list(idp, new);
  320.                         }
  321.                         spin_unlock_irqrestore(&idp->lock, flags);
  322.                         return -ENOMEM;
  323.                 }
  324.                 new->ary[0] = p;
  325.                 new->count = 1;
  326.                 new->layer = layers-1;
  327.                 new->prefix = id & idr_layer_prefix_mask(new->layer);
  328.                 if (bitmap_full(p->bitmap, IDR_SIZE))
  329.                         __set_bit(0, new->bitmap);
  330.                 p = new;
  331.         }
  332.         rcu_assign_pointer(idp->top, p);
  333.         idp->layers = layers;
  334.         v = sub_alloc(idp, &id, pa, gfp_mask, layer_idr);
  335.         if (v == -EAGAIN)
  336.                 goto build_up;
  337.         return(v);
  338. }
  339.  
  340. /*
  341.  * @id and @pa are from a successful allocation from idr_get_empty_slot().
  342.  * Install the user pointer @ptr and mark the slot full.
  343.  */
  344. static void idr_fill_slot(struct idr *idr, void *ptr, int id,
  345.                           struct idr_layer **pa)
  346. {
  347.         /* update hint used for lookup, cleared from free_layer() */
  348.         rcu_assign_pointer(idr->hint, pa[0]);
  349.  
  350.         rcu_assign_pointer(pa[0]->ary[id & IDR_MASK], (struct idr_layer *)ptr);
  351.                 pa[0]->count++;
  352.                 idr_mark_full(pa, id);
  353. }
  354.  
  355.  
  356. /**
  357.  * idr_preload - preload for idr_alloc()
  358.  * @gfp_mask: allocation mask to use for preloading
  359.  *
  360.  * Preload per-cpu layer buffer for idr_alloc().  Can only be used from
  361.  * process context and each idr_preload() invocation should be matched with
  362.  * idr_preload_end().  Note that preemption is disabled while preloaded.
  363.  *
  364.  * The first idr_alloc() in the preloaded section can be treated as if it
  365.  * were invoked with @gfp_mask used for preloading.  This allows using more
  366.  * permissive allocation masks for idrs protected by spinlocks.
  367.  *
  368.  * For example, if idr_alloc() below fails, the failure can be treated as
  369.  * if idr_alloc() were called with GFP_KERNEL rather than GFP_NOWAIT.
  370.  *
  371.  *      idr_preload(GFP_KERNEL);
  372.  *      spin_lock(lock);
  373.  *
  374.  *      id = idr_alloc(idr, ptr, start, end, GFP_NOWAIT);
  375.  *
  376.  *      spin_unlock(lock);
  377.  *      idr_preload_end();
  378.  *      if (id < 0)
  379.  *              error;
  380.  */
  381. void idr_preload(gfp_t gfp_mask)
  382. {
  383.  
  384.         /*
  385.          * idr_alloc() is likely to succeed w/o full idr_layer buffer and
  386.          * return value from idr_alloc() needs to be checked for failure
  387.          * anyway.  Silently give up if allocation fails.  The caller can
  388.          * treat failures from idr_alloc() as if idr_alloc() were called
  389.          * with @gfp_mask which should be enough.
  390.          */
  391.         while (idr_preload_cnt < MAX_IDR_FREE) {
  392.                 struct idr_layer *new;
  393.  
  394.                 new = kzalloc(sizeof(struct idr_layer), gfp_mask);
  395.                 if (!new)
  396.                         break;
  397.  
  398.                 /* link the new one to per-cpu preload list */
  399.                 new->ary[0] = idr_preload_head;
  400.                 idr_preload_head = new;
  401.                 idr_preload_cnt++;
  402.         }
  403. }
  404. EXPORT_SYMBOL(idr_preload);
  405.  
  406. /**
  407.  * idr_alloc - allocate new idr entry
  408.  * @idr: the (initialized) idr
  409.  * @ptr: pointer to be associated with the new id
  410.  * @start: the minimum id (inclusive)
  411.  * @end: the maximum id (exclusive, <= 0 for max)
  412.  * @gfp_mask: memory allocation flags
  413.  *
  414.  * Allocate an id in [start, end) and associate it with @ptr.  If no ID is
  415.  * available in the specified range, returns -ENOSPC.  On memory allocation
  416.  * failure, returns -ENOMEM.
  417.  *
  418.  * Note that @end is treated as max when <= 0.  This is to always allow
  419.  * using @start + N as @end as long as N is inside integer range.
  420.  *
  421.  * The user is responsible for exclusively synchronizing all operations
  422.  * which may modify @idr.  However, read-only accesses such as idr_find()
  423.  * or iteration can be performed under RCU read lock provided the user
  424.  * destroys @ptr in RCU-safe way after removal from idr.
  425.  */
  426. int idr_alloc(struct idr *idr, void *ptr, int start, int end, gfp_t gfp_mask)
  427. {
  428.         int max = end > 0 ? end - 1 : INT_MAX;  /* inclusive upper limit */
  429.         struct idr_layer *pa[MAX_IDR_LEVEL + 1];
  430.         int id;
  431.  
  432.         /* sanity checks */
  433.         if (WARN_ON_ONCE(start < 0))
  434.                 return -EINVAL;
  435.         if (unlikely(max < start))
  436.                 return -ENOSPC;
  437.  
  438.         /* allocate id */
  439.         id = idr_get_empty_slot(idr, start, pa, gfp_mask, NULL);
  440.         if (unlikely(id < 0))
  441.                 return id;
  442.         if (unlikely(id > max))
  443.                 return -ENOSPC;
  444.  
  445.         idr_fill_slot(idr, ptr, id, pa);
  446.         return id;
  447. }
  448. EXPORT_SYMBOL_GPL(idr_alloc);
  449.  
  450. /**
  451.  * idr_alloc_cyclic - allocate new idr entry in a cyclical fashion
  452.  * @idr: the (initialized) idr
  453.  * @ptr: pointer to be associated with the new id
  454.  * @start: the minimum id (inclusive)
  455.  * @end: the maximum id (exclusive, <= 0 for max)
  456.  * @gfp_mask: memory allocation flags
  457.  *
  458.  * Essentially the same as idr_alloc, but prefers to allocate progressively
  459.  * higher ids if it can. If the "cur" counter wraps, then it will start again
  460.  * at the "start" end of the range and allocate one that has already been used.
  461.  */
  462. int idr_alloc_cyclic(struct idr *idr, void *ptr, int start, int end,
  463.                         gfp_t gfp_mask)
  464. {
  465.         int id;
  466.  
  467.         id = idr_alloc(idr, ptr, max(start, idr->cur), end, gfp_mask);
  468.         if (id == -ENOSPC)
  469.                 id = idr_alloc(idr, ptr, start, end, gfp_mask);
  470.  
  471.         if (likely(id >= 0))
  472.                 idr->cur = id + 1;
  473.         return id;
  474. }
  475. EXPORT_SYMBOL(idr_alloc_cyclic);
  476.  
  477. static void idr_remove_warning(int id)
  478. {
  479.         WARN(1, "idr_remove called for id=%d which is not allocated.\n", id);
  480. }
  481.  
  482. static void sub_remove(struct idr *idp, int shift, int id)
  483. {
  484.         struct idr_layer *p = idp->top;
  485.         struct idr_layer **pa[MAX_IDR_LEVEL + 1];
  486.         struct idr_layer ***paa = &pa[0];
  487.         struct idr_layer *to_free;
  488.         int n;
  489.  
  490.         *paa = NULL;
  491.         *++paa = &idp->top;
  492.  
  493.         while ((shift > 0) && p) {
  494.                 n = (id >> shift) & IDR_MASK;
  495.                 __clear_bit(n, p->bitmap);
  496.                 *++paa = &p->ary[n];
  497.                 p = p->ary[n];
  498.                 shift -= IDR_BITS;
  499.         }
  500.         n = id & IDR_MASK;
  501.         if (likely(p != NULL && test_bit(n, p->bitmap))) {
  502.                 __clear_bit(n, p->bitmap);
  503.                 rcu_assign_pointer(p->ary[n], NULL);
  504.                 to_free = NULL;
  505.                 while(*paa && ! --((**paa)->count)){
  506.                         if (to_free)
  507.                                 free_layer(idp, to_free);
  508.                         to_free = **paa;
  509.                         **paa-- = NULL;
  510.                 }
  511.                 if (!*paa)
  512.                         idp->layers = 0;
  513.                 if (to_free)
  514.                         free_layer(idp, to_free);
  515.         } else
  516.                 idr_remove_warning(id);
  517. }
  518.  
  519. /**
  520.  * idr_remove - remove the given id and free its slot
  521.  * @idp: idr handle
  522.  * @id: unique key
  523.  */
  524. void idr_remove(struct idr *idp, int id)
  525. {
  526.         struct idr_layer *p;
  527.         struct idr_layer *to_free;
  528.  
  529.         if (id < 0)
  530.                 return;
  531.  
  532.         if (id > idr_max(idp->layers)) {
  533.                 idr_remove_warning(id);
  534.                 return;
  535.         }
  536.  
  537.         sub_remove(idp, (idp->layers - 1) * IDR_BITS, id);
  538.         if (idp->top && idp->top->count == 1 && (idp->layers > 1) &&
  539.             idp->top->ary[0]) {
  540.                 /*
  541.                  * Single child at leftmost slot: we can shrink the tree.
  542.                  * This level is not needed anymore since when layers are
  543.                  * inserted, they are inserted at the top of the existing
  544.                  * tree.
  545.                  */
  546.                 to_free = idp->top;
  547.                 p = idp->top->ary[0];
  548.                 rcu_assign_pointer(idp->top, p);
  549.                 --idp->layers;
  550.                 to_free->count = 0;
  551.                 bitmap_clear(to_free->bitmap, 0, IDR_SIZE);
  552.                 free_layer(idp, to_free);
  553.         }
  554. }
  555. EXPORT_SYMBOL(idr_remove);
  556.  
  557. static void __idr_remove_all(struct idr *idp)
  558. {
  559.         int n, id, max;
  560.         int bt_mask;
  561.         struct idr_layer *p;
  562.         struct idr_layer *pa[MAX_IDR_LEVEL + 1];
  563.         struct idr_layer **paa = &pa[0];
  564.  
  565.         n = idp->layers * IDR_BITS;
  566.         *paa = idp->top;
  567.         rcu_assign_pointer(idp->top, NULL);
  568.         max = idr_max(idp->layers);
  569.  
  570.         id = 0;
  571.         while (id >= 0 && id <= max) {
  572.                 p = *paa;
  573.                 while (n > IDR_BITS && p) {
  574.                         n -= IDR_BITS;
  575.                         p = p->ary[(id >> n) & IDR_MASK];
  576.                         *++paa = p;
  577.                 }
  578.  
  579.                 bt_mask = id;
  580.                 id += 1 << n;
  581.                 /* Get the highest bit that the above add changed from 0->1. */
  582.                 while (n < fls(id ^ bt_mask)) {
  583.                         if (*paa)
  584.                                 free_layer(idp, *paa);
  585.                         n += IDR_BITS;
  586.                         --paa;
  587.                 }
  588.         }
  589.         idp->layers = 0;
  590. }
  591.  
  592. /**
  593.  * idr_destroy - release all cached layers within an idr tree
  594.  * @idp: idr handle
  595.  *
  596.  * Free all id mappings and all idp_layers.  After this function, @idp is
  597.  * completely unused and can be freed / recycled.  The caller is
  598.  * responsible for ensuring that no one else accesses @idp during or after
  599.  * idr_destroy().
  600.  *
  601.  * A typical clean-up sequence for objects stored in an idr tree will use
  602.  * idr_for_each() to free all objects, if necessay, then idr_destroy() to
  603.  * free up the id mappings and cached idr_layers.
  604.  */
  605. void idr_destroy(struct idr *idp)
  606. {
  607.         __idr_remove_all(idp);
  608.  
  609.         while (idp->id_free_cnt) {
  610.                 struct idr_layer *p = get_from_free_list(idp);
  611.         kfree(p);
  612.         }
  613. }
  614. EXPORT_SYMBOL(idr_destroy);
  615.  
  616. void *idr_find_slowpath(struct idr *idp, int id)
  617. {
  618.         int n;
  619.         struct idr_layer *p;
  620.  
  621.         if (id < 0)
  622.                 return NULL;
  623.  
  624.         p = rcu_dereference_raw(idp->top);
  625.         if (!p)
  626.                 return NULL;
  627.         n = (p->layer+1) * IDR_BITS;
  628.  
  629.         if (id > idr_max(p->layer + 1))
  630.                 return NULL;
  631.         BUG_ON(n == 0);
  632.  
  633.         while (n > 0 && p) {
  634.                 n -= IDR_BITS;
  635.                 BUG_ON(n != p->layer*IDR_BITS);
  636.                 p = rcu_dereference_raw(p->ary[(id >> n) & IDR_MASK]);
  637.         }
  638.         return((void *)p);
  639. }
  640. EXPORT_SYMBOL(idr_find_slowpath);
  641.  
  642. /**
  643.  * idr_for_each - iterate through all stored pointers
  644.  * @idp: idr handle
  645.  * @fn: function to be called for each pointer
  646.  * @data: data passed back to callback function
  647.  *
  648.  * Iterate over the pointers registered with the given idr.  The
  649.  * callback function will be called for each pointer currently
  650.  * registered, passing the id, the pointer and the data pointer passed
  651.  * to this function.  It is not safe to modify the idr tree while in
  652.  * the callback, so functions such as idr_get_new and idr_remove are
  653.  * not allowed.
  654.  *
  655.  * We check the return of @fn each time. If it returns anything other
  656.  * than %0, we break out and return that value.
  657.  *
  658.  * The caller must serialize idr_for_each() vs idr_get_new() and idr_remove().
  659.  */
  660. int idr_for_each(struct idr *idp,
  661.                  int (*fn)(int id, void *p, void *data), void *data)
  662. {
  663.         int n, id, max, error = 0;
  664.         struct idr_layer *p;
  665.         struct idr_layer *pa[MAX_IDR_LEVEL + 1];
  666.         struct idr_layer **paa = &pa[0];
  667.  
  668.         n = idp->layers * IDR_BITS;
  669.         *paa = rcu_dereference_raw(idp->top);
  670.         max = idr_max(idp->layers);
  671.  
  672.         id = 0;
  673.         while (id >= 0 && id <= max) {
  674.                 p = *paa;
  675.                 while (n > 0 && p) {
  676.                         n -= IDR_BITS;
  677.                         p = rcu_dereference_raw(p->ary[(id >> n) & IDR_MASK]);
  678.                         *++paa = p;
  679.                 }
  680.  
  681.                 if (p) {
  682.                         error = fn(id, (void *)p, data);
  683.                         if (error)
  684.                                 break;
  685.                 }
  686.  
  687.                 id += 1 << n;
  688.                 while (n < fls(id)) {
  689.                         n += IDR_BITS;
  690.                         --paa;
  691.                 }
  692.         }
  693.  
  694.         return error;
  695. }
  696. EXPORT_SYMBOL(idr_for_each);
  697.  
  698. /**
  699.  * idr_get_next - lookup next object of id to given id.
  700.  * @idp: idr handle
  701.  * @nextidp:  pointer to lookup key
  702.  *
  703.  * Returns pointer to registered object with id, which is next number to
  704.  * given id. After being looked up, *@nextidp will be updated for the next
  705.  * iteration.
  706.  *
  707.  * This function can be called under rcu_read_lock(), given that the leaf
  708.  * pointers lifetimes are correctly managed.
  709.  */
  710. void *idr_get_next(struct idr *idp, int *nextidp)
  711. {
  712.         struct idr_layer *p, *pa[MAX_IDR_LEVEL + 1];
  713.         struct idr_layer **paa = &pa[0];
  714.         int id = *nextidp;
  715.         int n, max;
  716.  
  717.         /* find first ent */
  718.         p = *paa = rcu_dereference_raw(idp->top);
  719.         if (!p)
  720.                 return NULL;
  721.         n = (p->layer + 1) * IDR_BITS;
  722.         max = idr_max(p->layer + 1);
  723.  
  724.         while (id >= 0 && id <= max) {
  725.                 p = *paa;
  726.                 while (n > 0 && p) {
  727.                         n -= IDR_BITS;
  728.                         p = rcu_dereference_raw(p->ary[(id >> n) & IDR_MASK]);
  729.                         *++paa = p;
  730.                 }
  731.  
  732.                 if (p) {
  733.                         *nextidp = id;
  734.                         return p;
  735.                 }
  736.  
  737.                 /*
  738.                  * Proceed to the next layer at the current level.  Unlike
  739.                  * idr_for_each(), @id isn't guaranteed to be aligned to
  740.                  * layer boundary at this point and adding 1 << n may
  741.                  * incorrectly skip IDs.  Make sure we jump to the
  742.                  * beginning of the next layer using round_up().
  743.                  */
  744.                 id = round_up(id + 1, 1 << n);
  745.                 while (n < fls(id)) {
  746.                         n += IDR_BITS;
  747.                         --paa;
  748.                 }
  749.         }
  750.         return NULL;
  751. }
  752. EXPORT_SYMBOL(idr_get_next);
  753.  
  754.  
  755. /**
  756.  * idr_replace - replace pointer for given id
  757.  * @idp: idr handle
  758.  * @ptr: pointer you want associated with the id
  759.  * @id: lookup key
  760.  *
  761.  * Replace the pointer registered with an id and return the old value.
  762.  * A %-ENOENT return indicates that @id was not found.
  763.  * A %-EINVAL return indicates that @id was not within valid constraints.
  764.  *
  765.  * The caller must serialize with writers.
  766.  */
  767. void *idr_replace(struct idr *idp, void *ptr, int id)
  768. {
  769.         int n;
  770.         struct idr_layer *p, *old_p;
  771.  
  772.         if (id < 0)
  773.                 return ERR_PTR(-EINVAL);
  774.  
  775.         p = idp->top;
  776.         if (!p)
  777.                 return ERR_PTR(-ENOENT);
  778.  
  779.         if (id > idr_max(p->layer + 1))
  780.                 return ERR_PTR(-ENOENT);
  781.  
  782.         n = p->layer * IDR_BITS;
  783.         while ((n > 0) && p) {
  784.                 p = p->ary[(id >> n) & IDR_MASK];
  785.                 n -= IDR_BITS;
  786.         }
  787.  
  788.         n = id & IDR_MASK;
  789.         if (unlikely(p == NULL || !test_bit(n, p->bitmap)))
  790.                 return ERR_PTR(-ENOENT);
  791.  
  792.         old_p = p->ary[n];
  793.         rcu_assign_pointer(p->ary[n], ptr);
  794.  
  795.         return old_p;
  796. }
  797. EXPORT_SYMBOL(idr_replace);
  798.  
  799. void __init idr_init_cache(void)
  800. {
  801.     //idr_layer_cache = kmem_cache_create("idr_layer_cache",
  802.     //           sizeof(struct idr_layer), 0, SLAB_PANIC, NULL);
  803. }
  804.  
  805. /**
  806.  * idr_init - initialize idr handle
  807.  * @idp:        idr handle
  808.  *
  809.  * This function is use to set up the handle (@idp) that you will pass
  810.  * to the rest of the functions.
  811.  */
  812. void idr_init(struct idr *idp)
  813. {
  814.         memset(idp, 0, sizeof(struct idr));
  815.         spin_lock_init(&idp->lock);
  816. }
  817. EXPORT_SYMBOL(idr_init);
  818.  
  819. static int idr_has_entry(int id, void *p, void *data)
  820. {
  821.         return 1;
  822. }
  823.  
  824. bool idr_is_empty(struct idr *idp)
  825. {
  826.         return !idr_for_each(idp, idr_has_entry, NULL);
  827. }
  828. EXPORT_SYMBOL(idr_is_empty);
  829.  
  830. /**
  831.  * DOC: IDA description
  832.  * IDA - IDR based ID allocator
  833.  *
  834.  * This is id allocator without id -> pointer translation.  Memory
  835.  * usage is much lower than full blown idr because each id only
  836.  * occupies a bit.  ida uses a custom leaf node which contains
  837.  * IDA_BITMAP_BITS slots.
  838.  *
  839.  * 2007-04-25  written by Tejun Heo <htejun@gmail.com>
  840.  */
  841.  
  842. static void free_bitmap(struct ida *ida, struct ida_bitmap *bitmap)
  843. {
  844.         unsigned long flags;
  845.  
  846.         if (!ida->free_bitmap) {
  847.                 spin_lock_irqsave(&ida->idr.lock, flags);
  848.                 if (!ida->free_bitmap) {
  849.                         ida->free_bitmap = bitmap;
  850.                         bitmap = NULL;
  851.                 }
  852.                 spin_unlock_irqrestore(&ida->idr.lock, flags);
  853.         }
  854.  
  855.         kfree(bitmap);
  856. }
  857.  
  858. /**
  859.  * ida_pre_get - reserve resources for ida allocation
  860.  * @ida:        ida handle
  861.  * @gfp_mask:   memory allocation flag
  862.  *
  863.  * This function should be called prior to locking and calling the
  864.  * following function.  It preallocates enough memory to satisfy the
  865.  * worst possible allocation.
  866.  *
  867.  * If the system is REALLY out of memory this function returns %0,
  868.  * otherwise %1.
  869.  */
  870. int ida_pre_get(struct ida *ida, gfp_t gfp_mask)
  871. {
  872.         /* allocate idr_layers */
  873.         if (!__idr_pre_get(&ida->idr, gfp_mask))
  874.                 return 0;
  875.  
  876.         /* allocate free_bitmap */
  877.         if (!ida->free_bitmap) {
  878.                 struct ida_bitmap *bitmap;
  879.  
  880.                 bitmap = kmalloc(sizeof(struct ida_bitmap), gfp_mask);
  881.                 if (!bitmap)
  882.                         return 0;
  883.  
  884.                 free_bitmap(ida, bitmap);
  885.         }
  886.  
  887.         return 1;
  888. }
  889. EXPORT_SYMBOL(ida_pre_get);
  890.  
  891. /**
  892.  * ida_get_new_above - allocate new ID above or equal to a start id
  893.  * @ida:        ida handle
  894.  * @starting_id: id to start search at
  895.  * @p_id:       pointer to the allocated handle
  896.  *
  897.  * Allocate new ID above or equal to @starting_id.  It should be called
  898.  * with any required locks.
  899.  *
  900.  * If memory is required, it will return %-EAGAIN, you should unlock
  901.  * and go back to the ida_pre_get() call.  If the ida is full, it will
  902.  * return %-ENOSPC.
  903.  *
  904.  * @p_id returns a value in the range @starting_id ... %0x7fffffff.
  905.  */
  906. int ida_get_new_above(struct ida *ida, int starting_id, int *p_id)
  907. {
  908.         struct idr_layer *pa[MAX_IDR_LEVEL + 1];
  909.         struct ida_bitmap *bitmap;
  910.         unsigned long flags;
  911.         int idr_id = starting_id / IDA_BITMAP_BITS;
  912.         int offset = starting_id % IDA_BITMAP_BITS;
  913.         int t, id;
  914.  
  915.  restart:
  916.         /* get vacant slot */
  917.         t = idr_get_empty_slot(&ida->idr, idr_id, pa, 0, &ida->idr);
  918.         if (t < 0)
  919.                 return t == -ENOMEM ? -EAGAIN : t;
  920.  
  921.         if (t * IDA_BITMAP_BITS >= MAX_IDR_BIT)
  922.                 return -ENOSPC;
  923.  
  924.         if (t != idr_id)
  925.                 offset = 0;
  926.         idr_id = t;
  927.  
  928.         /* if bitmap isn't there, create a new one */
  929.         bitmap = (void *)pa[0]->ary[idr_id & IDR_MASK];
  930.         if (!bitmap) {
  931.                 spin_lock_irqsave(&ida->idr.lock, flags);
  932.                 bitmap = ida->free_bitmap;
  933.                 ida->free_bitmap = NULL;
  934.                 spin_unlock_irqrestore(&ida->idr.lock, flags);
  935.  
  936.                 if (!bitmap)
  937.                         return -EAGAIN;
  938.  
  939.                 memset(bitmap, 0, sizeof(struct ida_bitmap));
  940.                 rcu_assign_pointer(pa[0]->ary[idr_id & IDR_MASK],
  941.                                 (void *)bitmap);
  942.                 pa[0]->count++;
  943.         }
  944.  
  945.         /* lookup for empty slot */
  946.         t = find_next_zero_bit(bitmap->bitmap, IDA_BITMAP_BITS, offset);
  947.         if (t == IDA_BITMAP_BITS) {
  948.                 /* no empty slot after offset, continue to the next chunk */
  949.                 idr_id++;
  950.                 offset = 0;
  951.                 goto restart;
  952.         }
  953.  
  954.         id = idr_id * IDA_BITMAP_BITS + t;
  955.         if (id >= MAX_IDR_BIT)
  956.                 return -ENOSPC;
  957.  
  958.         __set_bit(t, bitmap->bitmap);
  959.         if (++bitmap->nr_busy == IDA_BITMAP_BITS)
  960.                 idr_mark_full(pa, idr_id);
  961.  
  962.         *p_id = id;
  963.  
  964.         /* Each leaf node can handle nearly a thousand slots and the
  965.          * whole idea of ida is to have small memory foot print.
  966.          * Throw away extra resources one by one after each successful
  967.          * allocation.
  968.          */
  969.         if (ida->idr.id_free_cnt || ida->free_bitmap) {
  970.                 struct idr_layer *p = get_from_free_list(&ida->idr);
  971.                 if (p)
  972.                         kfree(p);
  973.         }
  974.  
  975.         return 0;
  976. }
  977. EXPORT_SYMBOL(ida_get_new_above);
  978.  
  979. /**
  980.  * ida_remove - remove the given ID
  981.  * @ida:        ida handle
  982.  * @id:         ID to free
  983.  */
  984. void ida_remove(struct ida *ida, int id)
  985. {
  986.         struct idr_layer *p = ida->idr.top;
  987.         int shift = (ida->idr.layers - 1) * IDR_BITS;
  988.         int idr_id = id / IDA_BITMAP_BITS;
  989.         int offset = id % IDA_BITMAP_BITS;
  990.         int n;
  991.         struct ida_bitmap *bitmap;
  992.  
  993.         if (idr_id > idr_max(ida->idr.layers))
  994.                 goto err;
  995.  
  996.         /* clear full bits while looking up the leaf idr_layer */
  997.         while ((shift > 0) && p) {
  998.                 n = (idr_id >> shift) & IDR_MASK;
  999.                 __clear_bit(n, p->bitmap);
  1000.                 p = p->ary[n];
  1001.                 shift -= IDR_BITS;
  1002.         }
  1003.  
  1004.         if (p == NULL)
  1005.                 goto err;
  1006.  
  1007.         n = idr_id & IDR_MASK;
  1008.         __clear_bit(n, p->bitmap);
  1009.  
  1010.         bitmap = (void *)p->ary[n];
  1011.         if (!bitmap || !test_bit(offset, bitmap->bitmap))
  1012.                 goto err;
  1013.  
  1014.         /* update bitmap and remove it if empty */
  1015.         __clear_bit(offset, bitmap->bitmap);
  1016.         if (--bitmap->nr_busy == 0) {
  1017.                 __set_bit(n, p->bitmap);        /* to please idr_remove() */
  1018.                 idr_remove(&ida->idr, idr_id);
  1019.                 free_bitmap(ida, bitmap);
  1020.         }
  1021.  
  1022.         return;
  1023.  
  1024.  err:
  1025.         WARN(1, "ida_remove called for id=%d which is not allocated.\n", id);
  1026. }
  1027. EXPORT_SYMBOL(ida_remove);
  1028.  
  1029. /**
  1030.  * ida_destroy - release all cached layers within an ida tree
  1031.  * @ida:                ida handle
  1032.  */
  1033. void ida_destroy(struct ida *ida)
  1034. {
  1035.         idr_destroy(&ida->idr);
  1036.         kfree(ida->free_bitmap);
  1037. }
  1038. EXPORT_SYMBOL(ida_destroy);
  1039.  
  1040. /**
  1041.  * ida_simple_get - get a new id.
  1042.  * @ida: the (initialized) ida.
  1043.  * @start: the minimum id (inclusive, < 0x8000000)
  1044.  * @end: the maximum id (exclusive, < 0x8000000 or 0)
  1045.  * @gfp_mask: memory allocation flags
  1046.  *
  1047.  * Allocates an id in the range start <= id < end, or returns -ENOSPC.
  1048.  * On memory allocation failure, returns -ENOMEM.
  1049.  *
  1050.  * Use ida_simple_remove() to get rid of an id.
  1051.  */
  1052. int ida_simple_get(struct ida *ida, unsigned int start, unsigned int end,
  1053.                    gfp_t gfp_mask)
  1054. {
  1055.         int ret, id;
  1056.         unsigned int max;
  1057.         unsigned long flags;
  1058.  
  1059.         BUG_ON((int)start < 0);
  1060.         BUG_ON((int)end < 0);
  1061.  
  1062.         if (end == 0)
  1063.                 max = 0x80000000;
  1064.         else {
  1065.                 BUG_ON(end < start);
  1066.                 max = end - 1;
  1067.         }
  1068.  
  1069. again:
  1070.         if (!ida_pre_get(ida, gfp_mask))
  1071.                 return -ENOMEM;
  1072.  
  1073.         spin_lock_irqsave(&simple_ida_lock, flags);
  1074.         ret = ida_get_new_above(ida, start, &id);
  1075.         if (!ret) {
  1076.                 if (id > max) {
  1077.                         ida_remove(ida, id);
  1078.                         ret = -ENOSPC;
  1079.                 } else {
  1080.                         ret = id;
  1081.                 }
  1082.         }
  1083.         spin_unlock_irqrestore(&simple_ida_lock, flags);
  1084.  
  1085.         if (unlikely(ret == -EAGAIN))
  1086.                 goto again;
  1087.  
  1088.         return ret;
  1089. }
  1090. EXPORT_SYMBOL(ida_simple_get);
  1091.  
  1092. /**
  1093.  * ida_simple_remove - remove an allocated id.
  1094.  * @ida: the (initialized) ida.
  1095.  * @id: the id returned by ida_simple_get.
  1096.  */
  1097. void ida_simple_remove(struct ida *ida, unsigned int id)
  1098. {
  1099.         unsigned long flags;
  1100.  
  1101.         BUG_ON((int)id < 0);
  1102.         spin_lock_irqsave(&simple_ida_lock, flags);
  1103.         ida_remove(ida, id);
  1104.         spin_unlock_irqrestore(&simple_ida_lock, flags);
  1105. }
  1106. EXPORT_SYMBOL(ida_simple_remove);
  1107.  
  1108. /**
  1109.  * ida_init - initialize ida handle
  1110.  * @ida:        ida handle
  1111.  *
  1112.  * This function is use to set up the handle (@ida) that you will pass
  1113.  * to the rest of the functions.
  1114.  */
  1115. void ida_init(struct ida *ida)
  1116. {
  1117.         memset(ida, 0, sizeof(struct ida));
  1118.         idr_init(&ida->idr);
  1119.  
  1120. }
  1121. EXPORT_SYMBOL(ida_init);
  1122.  
  1123.  
  1124.  
  1125. unsigned long find_first_bit(const unsigned long *addr, unsigned long size)
  1126. {
  1127.         const unsigned long *p = addr;
  1128.         unsigned long result = 0;
  1129.         unsigned long tmp;
  1130.  
  1131.         while (size & ~(BITS_PER_LONG-1)) {
  1132.                 if ((tmp = *(p++)))
  1133.                         goto found;
  1134.                 result += BITS_PER_LONG;
  1135.                 size -= BITS_PER_LONG;
  1136.         }
  1137.         if (!size)
  1138.                 return result;
  1139.  
  1140.         tmp = (*p) & (~0UL >> (BITS_PER_LONG - size));
  1141.         if (tmp == 0UL)         /* Are any bits set? */
  1142.                 return result + size;   /* Nope. */
  1143. found:
  1144.         return result + __ffs(tmp);
  1145. }
  1146.  
  1147. unsigned long find_next_bit(const unsigned long *addr, unsigned long size,
  1148.                             unsigned long offset)
  1149. {
  1150.         const unsigned long *p = addr + BITOP_WORD(offset);
  1151.         unsigned long result = offset & ~(BITS_PER_LONG-1);
  1152.         unsigned long tmp;
  1153.  
  1154.         if (offset >= size)
  1155.                 return size;
  1156.         size -= result;
  1157.         offset %= BITS_PER_LONG;
  1158.         if (offset) {
  1159.                 tmp = *(p++);
  1160.                 tmp &= (~0UL << offset);
  1161.                 if (size < BITS_PER_LONG)
  1162.                         goto found_first;
  1163.                 if (tmp)
  1164.                         goto found_middle;
  1165.                 size -= BITS_PER_LONG;
  1166.                 result += BITS_PER_LONG;
  1167.         }
  1168.         while (size & ~(BITS_PER_LONG-1)) {
  1169.                 if ((tmp = *(p++)))
  1170.                         goto found_middle;
  1171.                 result += BITS_PER_LONG;
  1172.                 size -= BITS_PER_LONG;
  1173.         }
  1174.         if (!size)
  1175.                 return result;
  1176.         tmp = *p;
  1177.  
  1178. found_first:
  1179.         tmp &= (~0UL >> (BITS_PER_LONG - size));
  1180.         if (tmp == 0UL)         /* Are any bits set? */
  1181.                 return result + size;   /* Nope. */
  1182. found_middle:
  1183.         return result + __ffs(tmp);
  1184. }
  1185.  
  1186. unsigned long find_next_zero_bit(const unsigned long *addr, unsigned long size,
  1187.                                  unsigned long offset)
  1188. {
  1189.         const unsigned long *p = addr + BITOP_WORD(offset);
  1190.         unsigned long result = offset & ~(BITS_PER_LONG-1);
  1191.         unsigned long tmp;
  1192.  
  1193.         if (offset >= size)
  1194.                 return size;
  1195.         size -= result;
  1196.         offset %= BITS_PER_LONG;
  1197.         if (offset) {
  1198.                 tmp = *(p++);
  1199.                 tmp |= ~0UL >> (BITS_PER_LONG - offset);
  1200.                 if (size < BITS_PER_LONG)
  1201.                         goto found_first;
  1202.                 if (~tmp)
  1203.                         goto found_middle;
  1204.                 size -= BITS_PER_LONG;
  1205.                 result += BITS_PER_LONG;
  1206.         }
  1207.         while (size & ~(BITS_PER_LONG-1)) {
  1208.                 if (~(tmp = *(p++)))
  1209.                         goto found_middle;
  1210.                 result += BITS_PER_LONG;
  1211.                 size -= BITS_PER_LONG;
  1212.         }
  1213.         if (!size)
  1214.                 return result;
  1215.         tmp = *p;
  1216.  
  1217. found_first:
  1218.         tmp |= ~0UL << size;
  1219.         if (tmp == ~0UL)        /* Are any bits zero? */
  1220.                 return result + size;   /* Nope. */
  1221. found_middle:
  1222.         return result + ffz(tmp);
  1223. }
  1224.  
  1225. unsigned int hweight32(unsigned int w)
  1226. {
  1227.         unsigned int res = w - ((w >> 1) & 0x55555555);
  1228.         res = (res & 0x33333333) + ((res >> 2) & 0x33333333);
  1229.         res = (res + (res >> 4)) & 0x0F0F0F0F;
  1230.         res = res + (res >> 8);
  1231.         return (res + (res >> 16)) & 0x000000FF;
  1232. }
  1233.  
  1234. unsigned long hweight64(__u64 w)
  1235. {
  1236. #if BITS_PER_LONG == 32
  1237.         return hweight32((unsigned int)(w >> 32)) + hweight32((unsigned int)w);
  1238. #elif BITS_PER_LONG == 64
  1239.         __u64 res = w - ((w >> 1) & 0x5555555555555555ul);
  1240.         res = (res & 0x3333333333333333ul) + ((res >> 2) & 0x3333333333333333ul);
  1241.         res = (res + (res >> 4)) & 0x0F0F0F0F0F0F0F0Ful;
  1242.         res = res + (res >> 8);
  1243.         res = res + (res >> 16);
  1244.         return (res + (res >> 32)) & 0x00000000000000FFul;
  1245. #endif
  1246. }
  1247.  
  1248.