2261136ae0675e4a90b7ff4125c3441c00a03ebb
[platform/kernel/linux-rpi.git] / lib / sbitmap.c
1 /*
2  * Copyright (C) 2016 Facebook
3  * Copyright (C) 2013-2014 Jens Axboe
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public
7  * License v2 as published by the Free Software Foundation.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12  * General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
16  */
17
18 #include <linux/sched.h>
19 #include <linux/random.h>
20 #include <linux/sbitmap.h>
21 #include <linux/seq_file.h>
22
23 int sbitmap_init_node(struct sbitmap *sb, unsigned int depth, int shift,
24                       gfp_t flags, int node)
25 {
26         unsigned int bits_per_word;
27         unsigned int i;
28
29         if (shift < 0) {
30                 shift = ilog2(BITS_PER_LONG);
31                 /*
32                  * If the bitmap is small, shrink the number of bits per word so
33                  * we spread over a few cachelines, at least. If less than 4
34                  * bits, just forget about it, it's not going to work optimally
35                  * anyway.
36                  */
37                 if (depth >= 4) {
38                         while ((4U << shift) > depth)
39                                 shift--;
40                 }
41         }
42         bits_per_word = 1U << shift;
43         if (bits_per_word > BITS_PER_LONG)
44                 return -EINVAL;
45
46         sb->shift = shift;
47         sb->depth = depth;
48         sb->map_nr = DIV_ROUND_UP(sb->depth, bits_per_word);
49
50         if (depth == 0) {
51                 sb->map = NULL;
52                 return 0;
53         }
54
55         sb->map = kcalloc_node(sb->map_nr, sizeof(*sb->map), flags, node);
56         if (!sb->map)
57                 return -ENOMEM;
58
59         for (i = 0; i < sb->map_nr; i++) {
60                 sb->map[i].depth = min(depth, bits_per_word);
61                 depth -= sb->map[i].depth;
62                 spin_lock_init(&sb->map[i].swap_lock);
63         }
64         return 0;
65 }
66 EXPORT_SYMBOL_GPL(sbitmap_init_node);
67
68 void sbitmap_resize(struct sbitmap *sb, unsigned int depth)
69 {
70         unsigned int bits_per_word = 1U << sb->shift;
71         unsigned int i;
72
73         sb->depth = depth;
74         sb->map_nr = DIV_ROUND_UP(sb->depth, bits_per_word);
75
76         for (i = 0; i < sb->map_nr; i++) {
77                 sb->map[i].depth = min(depth, bits_per_word);
78                 depth -= sb->map[i].depth;
79         }
80 }
81 EXPORT_SYMBOL_GPL(sbitmap_resize);
82
83 static int __sbitmap_get_word(unsigned long *word, unsigned long depth,
84                               unsigned int hint, bool wrap)
85 {
86         unsigned int orig_hint = hint;
87         int nr;
88
89         while (1) {
90                 nr = find_next_zero_bit(word, depth, hint);
91                 if (unlikely(nr >= depth)) {
92                         /*
93                          * We started with an offset, and we didn't reset the
94                          * offset to 0 in a failure case, so start from 0 to
95                          * exhaust the map.
96                          */
97                         if (orig_hint && hint && wrap) {
98                                 hint = orig_hint = 0;
99                                 continue;
100                         }
101                         return -1;
102                 }
103
104                 if (!test_and_set_bit_lock(nr, word))
105                         break;
106
107                 hint = nr + 1;
108                 if (hint >= depth - 1)
109                         hint = 0;
110         }
111
112         return nr;
113 }
114
115 /*
116  * See if we have deferred clears that we can batch move
117  */
118 static inline bool sbitmap_deferred_clear(struct sbitmap *sb, int index)
119 {
120         unsigned long mask, val;
121         unsigned long __maybe_unused flags;
122         bool ret = false;
123
124         /* Silence bogus lockdep warning */
125 #if defined(CONFIG_LOCKDEP)
126         local_irq_save(flags);
127 #endif
128         spin_lock(&sb->map[index].swap_lock);
129
130         if (!sb->map[index].cleared)
131                 goto out_unlock;
132
133         /*
134          * First get a stable cleared mask, setting the old mask to 0.
135          */
136         do {
137                 mask = sb->map[index].cleared;
138         } while (cmpxchg(&sb->map[index].cleared, mask, 0) != mask);
139
140         /*
141          * Now clear the masked bits in our free word
142          */
143         do {
144                 val = sb->map[index].word;
145         } while (cmpxchg(&sb->map[index].word, val, val & ~mask) != val);
146
147         ret = true;
148 out_unlock:
149         spin_unlock(&sb->map[index].swap_lock);
150 #if defined(CONFIG_LOCKDEP)
151         local_irq_restore(flags);
152 #endif
153         return ret;
154 }
155
156 static int sbitmap_find_bit_in_index(struct sbitmap *sb, int index,
157                                      unsigned int alloc_hint, bool round_robin)
158 {
159         int nr;
160
161         do {
162                 nr = __sbitmap_get_word(&sb->map[index].word,
163                                         sb->map[index].depth, alloc_hint,
164                                         !round_robin);
165                 if (nr != -1)
166                         break;
167                 if (!sbitmap_deferred_clear(sb, index))
168                         break;
169         } while (1);
170
171         return nr;
172 }
173
174 int sbitmap_get(struct sbitmap *sb, unsigned int alloc_hint, bool round_robin)
175 {
176         unsigned int i, index;
177         int nr = -1;
178
179         index = SB_NR_TO_INDEX(sb, alloc_hint);
180
181         /*
182          * Unless we're doing round robin tag allocation, just use the
183          * alloc_hint to find the right word index. No point in looping
184          * twice in find_next_zero_bit() for that case.
185          */
186         if (round_robin)
187                 alloc_hint = SB_NR_TO_BIT(sb, alloc_hint);
188         else
189                 alloc_hint = 0;
190
191         for (i = 0; i < sb->map_nr; i++) {
192                 nr = sbitmap_find_bit_in_index(sb, index, alloc_hint,
193                                                 round_robin);
194                 if (nr != -1) {
195                         nr += index << sb->shift;
196                         break;
197                 }
198
199                 /* Jump to next index. */
200                 alloc_hint = 0;
201                 if (++index >= sb->map_nr)
202                         index = 0;
203         }
204
205         return nr;
206 }
207 EXPORT_SYMBOL_GPL(sbitmap_get);
208
209 int sbitmap_get_shallow(struct sbitmap *sb, unsigned int alloc_hint,
210                         unsigned long shallow_depth)
211 {
212         unsigned int i, index;
213         int nr = -1;
214
215         index = SB_NR_TO_INDEX(sb, alloc_hint);
216
217         for (i = 0; i < sb->map_nr; i++) {
218                 nr = __sbitmap_get_word(&sb->map[index].word,
219                                         min(sb->map[index].depth, shallow_depth),
220                                         SB_NR_TO_BIT(sb, alloc_hint), true);
221                 if (nr != -1) {
222                         nr += index << sb->shift;
223                         break;
224                 }
225
226                 /* Jump to next index. */
227                 index++;
228                 alloc_hint = index << sb->shift;
229
230                 if (index >= sb->map_nr) {
231                         index = 0;
232                         alloc_hint = 0;
233                 }
234         }
235
236         return nr;
237 }
238 EXPORT_SYMBOL_GPL(sbitmap_get_shallow);
239
240 bool sbitmap_any_bit_set(const struct sbitmap *sb)
241 {
242         unsigned int i;
243
244         for (i = 0; i < sb->map_nr; i++) {
245                 if (sb->map[i].word)
246                         return true;
247         }
248         return false;
249 }
250 EXPORT_SYMBOL_GPL(sbitmap_any_bit_set);
251
252 bool sbitmap_any_bit_clear(const struct sbitmap *sb)
253 {
254         unsigned int i;
255
256         for (i = 0; i < sb->map_nr; i++) {
257                 const struct sbitmap_word *word = &sb->map[i];
258                 unsigned long ret;
259
260                 ret = find_first_zero_bit(&word->word, word->depth);
261                 if (ret < word->depth)
262                         return true;
263         }
264         return false;
265 }
266 EXPORT_SYMBOL_GPL(sbitmap_any_bit_clear);
267
268 static unsigned int __sbitmap_weight(const struct sbitmap *sb, bool set)
269 {
270         unsigned int i, weight = 0;
271
272         for (i = 0; i < sb->map_nr; i++) {
273                 const struct sbitmap_word *word = &sb->map[i];
274
275                 if (set)
276                         weight += bitmap_weight(&word->word, word->depth);
277                 else
278                         weight += bitmap_weight(&word->cleared, word->depth);
279         }
280         return weight;
281 }
282
283 static unsigned int sbitmap_weight(const struct sbitmap *sb)
284 {
285         return __sbitmap_weight(sb, true);
286 }
287
288 static unsigned int sbitmap_cleared(const struct sbitmap *sb)
289 {
290         return __sbitmap_weight(sb, false);
291 }
292
293 void sbitmap_show(struct sbitmap *sb, struct seq_file *m)
294 {
295         seq_printf(m, "depth=%u\n", sb->depth);
296         seq_printf(m, "busy=%u\n", sbitmap_weight(sb) - sbitmap_cleared(sb));
297         seq_printf(m, "cleared=%u\n", sbitmap_cleared(sb));
298         seq_printf(m, "bits_per_word=%u\n", 1U << sb->shift);
299         seq_printf(m, "map_nr=%u\n", sb->map_nr);
300 }
301 EXPORT_SYMBOL_GPL(sbitmap_show);
302
303 static inline void emit_byte(struct seq_file *m, unsigned int offset, u8 byte)
304 {
305         if ((offset & 0xf) == 0) {
306                 if (offset != 0)
307                         seq_putc(m, '\n');
308                 seq_printf(m, "%08x:", offset);
309         }
310         if ((offset & 0x1) == 0)
311                 seq_putc(m, ' ');
312         seq_printf(m, "%02x", byte);
313 }
314
315 void sbitmap_bitmap_show(struct sbitmap *sb, struct seq_file *m)
316 {
317         u8 byte = 0;
318         unsigned int byte_bits = 0;
319         unsigned int offset = 0;
320         int i;
321
322         for (i = 0; i < sb->map_nr; i++) {
323                 unsigned long word = READ_ONCE(sb->map[i].word);
324                 unsigned int word_bits = READ_ONCE(sb->map[i].depth);
325
326                 while (word_bits > 0) {
327                         unsigned int bits = min(8 - byte_bits, word_bits);
328
329                         byte |= (word & (BIT(bits) - 1)) << byte_bits;
330                         byte_bits += bits;
331                         if (byte_bits == 8) {
332                                 emit_byte(m, offset, byte);
333                                 byte = 0;
334                                 byte_bits = 0;
335                                 offset++;
336                         }
337                         word >>= bits;
338                         word_bits -= bits;
339                 }
340         }
341         if (byte_bits) {
342                 emit_byte(m, offset, byte);
343                 offset++;
344         }
345         if (offset)
346                 seq_putc(m, '\n');
347 }
348 EXPORT_SYMBOL_GPL(sbitmap_bitmap_show);
349
350 static unsigned int sbq_calc_wake_batch(struct sbitmap_queue *sbq,
351                                         unsigned int depth)
352 {
353         unsigned int wake_batch;
354         unsigned int shallow_depth;
355
356         /*
357          * For each batch, we wake up one queue. We need to make sure that our
358          * batch size is small enough that the full depth of the bitmap,
359          * potentially limited by a shallow depth, is enough to wake up all of
360          * the queues.
361          *
362          * Each full word of the bitmap has bits_per_word bits, and there might
363          * be a partial word. There are depth / bits_per_word full words and
364          * depth % bits_per_word bits left over. In bitwise arithmetic:
365          *
366          * bits_per_word = 1 << shift
367          * depth / bits_per_word = depth >> shift
368          * depth % bits_per_word = depth & ((1 << shift) - 1)
369          *
370          * Each word can be limited to sbq->min_shallow_depth bits.
371          */
372         shallow_depth = min(1U << sbq->sb.shift, sbq->min_shallow_depth);
373         depth = ((depth >> sbq->sb.shift) * shallow_depth +
374                  min(depth & ((1U << sbq->sb.shift) - 1), shallow_depth));
375         wake_batch = clamp_t(unsigned int, depth / SBQ_WAIT_QUEUES, 1,
376                              SBQ_WAKE_BATCH);
377
378         return wake_batch;
379 }
380
381 int sbitmap_queue_init_node(struct sbitmap_queue *sbq, unsigned int depth,
382                             int shift, bool round_robin, gfp_t flags, int node)
383 {
384         int ret;
385         int i;
386
387         ret = sbitmap_init_node(&sbq->sb, depth, shift, flags, node);
388         if (ret)
389                 return ret;
390
391         sbq->alloc_hint = alloc_percpu_gfp(unsigned int, flags);
392         if (!sbq->alloc_hint) {
393                 sbitmap_free(&sbq->sb);
394                 return -ENOMEM;
395         }
396
397         if (depth && !round_robin) {
398                 for_each_possible_cpu(i)
399                         *per_cpu_ptr(sbq->alloc_hint, i) = prandom_u32() % depth;
400         }
401
402         sbq->min_shallow_depth = UINT_MAX;
403         sbq->wake_batch = sbq_calc_wake_batch(sbq, depth);
404         atomic_set(&sbq->wake_index, 0);
405         atomic_set(&sbq->ws_active, 0);
406
407         sbq->ws = kzalloc_node(SBQ_WAIT_QUEUES * sizeof(*sbq->ws), flags, node);
408         if (!sbq->ws) {
409                 free_percpu(sbq->alloc_hint);
410                 sbitmap_free(&sbq->sb);
411                 return -ENOMEM;
412         }
413
414         for (i = 0; i < SBQ_WAIT_QUEUES; i++) {
415                 init_waitqueue_head(&sbq->ws[i].wait);
416                 atomic_set(&sbq->ws[i].wait_cnt, sbq->wake_batch);
417         }
418
419         sbq->round_robin = round_robin;
420         return 0;
421 }
422 EXPORT_SYMBOL_GPL(sbitmap_queue_init_node);
423
424 static void sbitmap_queue_update_wake_batch(struct sbitmap_queue *sbq,
425                                             unsigned int depth)
426 {
427         unsigned int wake_batch = sbq_calc_wake_batch(sbq, depth);
428         int i;
429
430         if (sbq->wake_batch != wake_batch) {
431                 WRITE_ONCE(sbq->wake_batch, wake_batch);
432                 /*
433                  * Pairs with the memory barrier in sbitmap_queue_wake_up()
434                  * to ensure that the batch size is updated before the wait
435                  * counts.
436                  */
437                 smp_mb__before_atomic();
438                 for (i = 0; i < SBQ_WAIT_QUEUES; i++)
439                         atomic_set(&sbq->ws[i].wait_cnt, 1);
440         }
441 }
442
443 void sbitmap_queue_resize(struct sbitmap_queue *sbq, unsigned int depth)
444 {
445         sbitmap_queue_update_wake_batch(sbq, depth);
446         sbitmap_resize(&sbq->sb, depth);
447 }
448 EXPORT_SYMBOL_GPL(sbitmap_queue_resize);
449
450 int __sbitmap_queue_get(struct sbitmap_queue *sbq)
451 {
452         unsigned int hint, depth;
453         int nr;
454
455         hint = this_cpu_read(*sbq->alloc_hint);
456         depth = READ_ONCE(sbq->sb.depth);
457         if (unlikely(hint >= depth)) {
458                 hint = depth ? prandom_u32() % depth : 0;
459                 this_cpu_write(*sbq->alloc_hint, hint);
460         }
461         nr = sbitmap_get(&sbq->sb, hint, sbq->round_robin);
462
463         if (nr == -1) {
464                 /* If the map is full, a hint won't do us much good. */
465                 this_cpu_write(*sbq->alloc_hint, 0);
466         } else if (nr == hint || unlikely(sbq->round_robin)) {
467                 /* Only update the hint if we used it. */
468                 hint = nr + 1;
469                 if (hint >= depth - 1)
470                         hint = 0;
471                 this_cpu_write(*sbq->alloc_hint, hint);
472         }
473
474         return nr;
475 }
476 EXPORT_SYMBOL_GPL(__sbitmap_queue_get);
477
478 int __sbitmap_queue_get_shallow(struct sbitmap_queue *sbq,
479                                 unsigned int shallow_depth)
480 {
481         unsigned int hint, depth;
482         int nr;
483
484         WARN_ON_ONCE(shallow_depth < sbq->min_shallow_depth);
485
486         hint = this_cpu_read(*sbq->alloc_hint);
487         depth = READ_ONCE(sbq->sb.depth);
488         if (unlikely(hint >= depth)) {
489                 hint = depth ? prandom_u32() % depth : 0;
490                 this_cpu_write(*sbq->alloc_hint, hint);
491         }
492         nr = sbitmap_get_shallow(&sbq->sb, hint, shallow_depth);
493
494         if (nr == -1) {
495                 /* If the map is full, a hint won't do us much good. */
496                 this_cpu_write(*sbq->alloc_hint, 0);
497         } else if (nr == hint || unlikely(sbq->round_robin)) {
498                 /* Only update the hint if we used it. */
499                 hint = nr + 1;
500                 if (hint >= depth - 1)
501                         hint = 0;
502                 this_cpu_write(*sbq->alloc_hint, hint);
503         }
504
505         return nr;
506 }
507 EXPORT_SYMBOL_GPL(__sbitmap_queue_get_shallow);
508
509 void sbitmap_queue_min_shallow_depth(struct sbitmap_queue *sbq,
510                                      unsigned int min_shallow_depth)
511 {
512         sbq->min_shallow_depth = min_shallow_depth;
513         sbitmap_queue_update_wake_batch(sbq, sbq->sb.depth);
514 }
515 EXPORT_SYMBOL_GPL(sbitmap_queue_min_shallow_depth);
516
517 static struct sbq_wait_state *sbq_wake_ptr(struct sbitmap_queue *sbq)
518 {
519         int i, wake_index;
520
521         if (!atomic_read(&sbq->ws_active))
522                 return NULL;
523
524         wake_index = atomic_read(&sbq->wake_index);
525         for (i = 0; i < SBQ_WAIT_QUEUES; i++) {
526                 struct sbq_wait_state *ws = &sbq->ws[wake_index];
527
528                 if (waitqueue_active(&ws->wait)) {
529                         int o = atomic_read(&sbq->wake_index);
530
531                         if (wake_index != o)
532                                 atomic_cmpxchg(&sbq->wake_index, o, wake_index);
533                         return ws;
534                 }
535
536                 wake_index = sbq_index_inc(wake_index);
537         }
538
539         return NULL;
540 }
541
542 static bool __sbq_wake_up(struct sbitmap_queue *sbq)
543 {
544         struct sbq_wait_state *ws;
545         unsigned int wake_batch;
546         int wait_cnt;
547
548         ws = sbq_wake_ptr(sbq);
549         if (!ws)
550                 return false;
551
552         wait_cnt = atomic_dec_return(&ws->wait_cnt);
553         if (wait_cnt <= 0) {
554                 int ret;
555
556                 wake_batch = READ_ONCE(sbq->wake_batch);
557
558                 /*
559                  * Pairs with the memory barrier in sbitmap_queue_resize() to
560                  * ensure that we see the batch size update before the wait
561                  * count is reset.
562                  */
563                 smp_mb__before_atomic();
564
565                 /*
566                  * For concurrent callers of this, the one that failed the
567                  * atomic_cmpxhcg() race should call this function again
568                  * to wakeup a new batch on a different 'ws'.
569                  */
570                 ret = atomic_cmpxchg(&ws->wait_cnt, wait_cnt, wake_batch);
571                 if (ret == wait_cnt) {
572                         sbq_index_atomic_inc(&sbq->wake_index);
573                         wake_up_nr(&ws->wait, wake_batch);
574                         return false;
575                 }
576
577                 return true;
578         }
579
580         return false;
581 }
582
583 void sbitmap_queue_wake_up(struct sbitmap_queue *sbq)
584 {
585         while (__sbq_wake_up(sbq))
586                 ;
587 }
588 EXPORT_SYMBOL_GPL(sbitmap_queue_wake_up);
589
590 void sbitmap_queue_clear(struct sbitmap_queue *sbq, unsigned int nr,
591                          unsigned int cpu)
592 {
593         sbitmap_deferred_clear_bit(&sbq->sb, nr);
594
595         /*
596          * Pairs with the memory barrier in set_current_state() to ensure the
597          * proper ordering of clear_bit_unlock()/waitqueue_active() in the waker
598          * and test_and_set_bit_lock()/prepare_to_wait()/finish_wait() in the
599          * waiter. See the comment on waitqueue_active().
600          */
601         smp_mb__after_atomic();
602         sbitmap_queue_wake_up(sbq);
603
604         if (likely(!sbq->round_robin && nr < sbq->sb.depth))
605                 *per_cpu_ptr(sbq->alloc_hint, cpu) = nr;
606 }
607 EXPORT_SYMBOL_GPL(sbitmap_queue_clear);
608
609 void sbitmap_queue_wake_all(struct sbitmap_queue *sbq)
610 {
611         int i, wake_index;
612
613         /*
614          * Pairs with the memory barrier in set_current_state() like in
615          * sbitmap_queue_wake_up().
616          */
617         smp_mb();
618         wake_index = atomic_read(&sbq->wake_index);
619         for (i = 0; i < SBQ_WAIT_QUEUES; i++) {
620                 struct sbq_wait_state *ws = &sbq->ws[wake_index];
621
622                 if (waitqueue_active(&ws->wait))
623                         wake_up(&ws->wait);
624
625                 wake_index = sbq_index_inc(wake_index);
626         }
627 }
628 EXPORT_SYMBOL_GPL(sbitmap_queue_wake_all);
629
630 void sbitmap_queue_show(struct sbitmap_queue *sbq, struct seq_file *m)
631 {
632         bool first;
633         int i;
634
635         sbitmap_show(&sbq->sb, m);
636
637         seq_puts(m, "alloc_hint={");
638         first = true;
639         for_each_possible_cpu(i) {
640                 if (!first)
641                         seq_puts(m, ", ");
642                 first = false;
643                 seq_printf(m, "%u", *per_cpu_ptr(sbq->alloc_hint, i));
644         }
645         seq_puts(m, "}\n");
646
647         seq_printf(m, "wake_batch=%u\n", sbq->wake_batch);
648         seq_printf(m, "wake_index=%d\n", atomic_read(&sbq->wake_index));
649         seq_printf(m, "ws_active=%d\n", atomic_read(&sbq->ws_active));
650
651         seq_puts(m, "ws={\n");
652         for (i = 0; i < SBQ_WAIT_QUEUES; i++) {
653                 struct sbq_wait_state *ws = &sbq->ws[i];
654
655                 seq_printf(m, "\t{.wait_cnt=%d, .wait=%s},\n",
656                            atomic_read(&ws->wait_cnt),
657                            waitqueue_active(&ws->wait) ? "active" : "inactive");
658         }
659         seq_puts(m, "}\n");
660
661         seq_printf(m, "round_robin=%d\n", sbq->round_robin);
662         seq_printf(m, "min_shallow_depth=%u\n", sbq->min_shallow_depth);
663 }
664 EXPORT_SYMBOL_GPL(sbitmap_queue_show);
665
666 void sbitmap_prepare_to_wait(struct sbitmap_queue *sbq,
667                              struct sbq_wait_state *ws,
668                              struct sbq_wait *sbq_wait, int state)
669 {
670         if (!sbq_wait->accounted) {
671                 atomic_inc(&sbq->ws_active);
672                 sbq_wait->accounted = 1;
673         }
674         prepare_to_wait_exclusive(&ws->wait, &sbq_wait->wait, state);
675 }
676 EXPORT_SYMBOL_GPL(sbitmap_prepare_to_wait);
677
678 void sbitmap_finish_wait(struct sbitmap_queue *sbq, struct sbq_wait_state *ws,
679                          struct sbq_wait *sbq_wait)
680 {
681         finish_wait(&ws->wait, &sbq_wait->wait);
682         if (sbq_wait->accounted) {
683                 atomic_dec(&sbq->ws_active);
684                 sbq_wait->accounted = 0;
685         }
686 }
687 EXPORT_SYMBOL_GPL(sbitmap_finish_wait);