block/blk-mq.c

   1 #include <linux/kernel.h>
   2 #include <linux/module.h>
   3 #include <linux/backing-dev.h>
   4 #include <linux/bio.h>
   5 #include <linux/blkdev.h>
   6 #include <linux/mm.h>
   7 #include <linux/init.h>
   8 #include <linux/slab.h>
   9 #include <linux/workqueue.h>
  10 #include <linux/smp.h>
  11 #include <linux/llist.h>
  12 #include <linux/list_sort.h>
  13 #include <linux/cpu.h>
  14 #include <linux/cache.h>
  15 #include <linux/sched/sysctl.h>
  16 #include <linux/delay.h>
  17
  18 #include <trace/events/block.h>
  19
  20 #include <linux/blk-mq.h>
  21 #include "blk.h"
  22 #include "blk-mq.h"
  23 #include "blk-mq-tag.h"
  24
  25 static DEFINE_MUTEX(all_q_mutex);
  26 static LIST_HEAD(all_q_list);
  27
  28 static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx);
  29
  30 static struct blk_mq_ctx *__blk_mq_get_ctx(struct request_queue *q,
  31                                            unsigned int cpu)
  32 {
  33         return per_cpu_ptr(q->queue_ctx, cpu);
  34 }
  35
  36 /*
  37  * This assumes per-cpu software queueing queues. They could be per-node
  38  * as well, for instance. For now this is hardcoded as-is. Note that we don't
  39  * care about preemption, since we know the ctx's are persistent. This does
  40  * mean that we can't rely on ctx always matching the currently running CPU.
  41  */
  42 static struct blk_mq_ctx *blk_mq_get_ctx(struct request_queue *q)
  43 {
  44         return __blk_mq_get_ctx(q, get_cpu());
  45 }
  46
  47 static void blk_mq_put_ctx(struct blk_mq_ctx *ctx)
  48 {
  49         put_cpu();
  50 }
  51
  52 /*
  53  * Check if any of the ctx's have pending work in this hardware queue
  54  */
  55 static bool blk_mq_hctx_has_pending(struct blk_mq_hw_ctx *hctx)
  56 {
  57         unsigned int i;
  58
  59         for (i = 0; i < hctx->ctx_map.map_size; i++)
  60                 if (hctx->ctx_map.map[i].word)
  61                         return true;
  62
  63         return false;
  64 }
  65
  66 static inline struct blk_align_bitmap *get_bm(struct blk_mq_hw_ctx *hctx,
  67                                               struct blk_mq_ctx *ctx)
  68 {
  69         return &hctx->ctx_map.map[ctx->index_hw / hctx->ctx_map.bits_per_word];
  70 }
  71
  72 #define CTX_TO_BIT(hctx, ctx)   \
  73         ((ctx)->index_hw & ((hctx)->ctx_map.bits_per_word - 1))
  74
  75 /*
  76  * Mark this ctx as having pending work in this hardware queue
  77  */
  78 static void blk_mq_hctx_mark_pending(struct blk_mq_hw_ctx *hctx,
  79                                      struct blk_mq_ctx *ctx)
  80 {
  81         struct blk_align_bitmap *bm = get_bm(hctx, ctx);
  82
  83         if (!test_bit(CTX_TO_BIT(hctx, ctx), &bm->word))
  84                 set_bit(CTX_TO_BIT(hctx, ctx), &bm->word);
  85 }
  86
  87 static void blk_mq_hctx_clear_pending(struct blk_mq_hw_ctx *hctx,
  88                                       struct blk_mq_ctx *ctx)
  89 {
  90         struct blk_align_bitmap *bm = get_bm(hctx, ctx);
  91
  92         clear_bit(CTX_TO_BIT(hctx, ctx), &bm->word);
  93 }
  94
  95 static struct request *__blk_mq_alloc_request(struct blk_mq_hw_ctx *hctx,
  96                                               struct blk_mq_ctx *ctx,
  97                                               gfp_t gfp, bool reserved)
  98 {
  99         struct request *rq;
 100         unsigned int tag;
 101
 102         tag = blk_mq_get_tag(hctx, &ctx->last_tag, gfp, reserved);
 103         if (tag != BLK_MQ_TAG_FAIL) {
 104                 rq = hctx->tags->rqs[tag];
 105
 106                 rq->cmd_flags = 0;
 107                 if (blk_mq_tag_busy(hctx)) {
 108                         rq->cmd_flags = REQ_MQ_INFLIGHT;
 109                         atomic_inc(&hctx->nr_active);
 110                 }
 111
 112                 rq->tag = tag;
 113                 return rq;
 114         }
 115
 116         return NULL;
 117 }
 118
 119 static int blk_mq_queue_enter(struct request_queue *q)
 120 {
 121         int ret;
 122
 123         __percpu_counter_add(&q->mq_usage_counter, 1, 1000000);
 124         smp_wmb();
 125         /* we have problems to freeze the queue if it's initializing */
 126         if (!blk_queue_bypass(q) || !blk_queue_init_done(q))
 127                 return 0;
 128
 129         __percpu_counter_add(&q->mq_usage_counter, -1, 1000000);
 130
 131         spin_lock_irq(q->queue_lock);
 132         ret = wait_event_interruptible_lock_irq(q->mq_freeze_wq,
 133                 !blk_queue_bypass(q) || blk_queue_dying(q),
 134                 *q->queue_lock);
 135         /* inc usage with lock hold to avoid freeze_queue runs here */
 136         if (!ret && !blk_queue_dying(q))
 137                 __percpu_counter_add(&q->mq_usage_counter, 1, 1000000);
 138         else if (blk_queue_dying(q))
 139                 ret = -ENODEV;
 140         spin_unlock_irq(q->queue_lock);
 141
 142         return ret;
 143 }
 144
 145 static void blk_mq_queue_exit(struct request_queue *q)
 146 {
 147         __percpu_counter_add(&q->mq_usage_counter, -1, 1000000);
 148 }
 149
 150 static void __blk_mq_drain_queue(struct request_queue *q)
 151 {
 152         while (true) {
 153                 s64 count;
 154
 155                 spin_lock_irq(q->queue_lock);
 156                 count = percpu_counter_sum(&q->mq_usage_counter);
 157                 spin_unlock_irq(q->queue_lock);
 158
 159                 if (count == 0)
 160                         break;
 161                 blk_mq_run_queues(q, false);
 162                 msleep(10);
 163         }
 164 }
 165
 166 /*
 167  * Guarantee no request is in use, so we can change any data structure of
 168  * the queue afterward.
 169  */
 170 static void blk_mq_freeze_queue(struct request_queue *q)
 171 {
 172         bool drain;
 173
 174         spin_lock_irq(q->queue_lock);
 175         drain = !q->bypass_depth++;
 176         queue_flag_set(QUEUE_FLAG_BYPASS, q);
 177         spin_unlock_irq(q->queue_lock);
 178
 179         if (drain)
 180                 __blk_mq_drain_queue(q);
 181 }
 182
 183 void blk_mq_drain_queue(struct request_queue *q)
 184 {
 185         __blk_mq_drain_queue(q);
 186 }
 187
 188 static void blk_mq_unfreeze_queue(struct request_queue *q)
 189 {
 190         bool wake = false;
 191
 192         spin_lock_irq(q->queue_lock);
 193         if (!--q->bypass_depth) {
 194                 queue_flag_clear(QUEUE_FLAG_BYPASS, q);
 195                 wake = true;
 196         }
 197         WARN_ON_ONCE(q->bypass_depth < 0);
 198         spin_unlock_irq(q->queue_lock);
 199         if (wake)
 200                 wake_up_all(&q->mq_freeze_wq);
 201 }
 202
 203 bool blk_mq_can_queue(struct blk_mq_hw_ctx *hctx)
 204 {
 205         return blk_mq_has_free_tags(hctx->tags);
 206 }
 207 EXPORT_SYMBOL(blk_mq_can_queue);
 208
 209 static void blk_mq_rq_ctx_init(struct request_queue *q, struct blk_mq_ctx *ctx,
 210                                struct request *rq, unsigned int rw_flags)
 211 {
 212         if (blk_queue_io_stat(q))
 213                 rw_flags |= REQ_IO_STAT;
 214
 215         INIT_LIST_HEAD(&rq->queuelist);
 216         /* csd/requeue_work/fifo_time is initialized before use */
 217         rq->q = q;
 218         rq->mq_ctx = ctx;
 219         rq->cmd_flags |= rw_flags;
 220         rq->cmd_type = 0;
 221         /* do not touch atomic flags, it needs atomic ops against the timer */
 222         rq->cpu = -1;
 223         rq->__data_len = 0;
 224         rq->__sector = (sector_t) -1;
 225         rq->bio = NULL;
 226         rq->biotail = NULL;
 227         INIT_HLIST_NODE(&rq->hash);
 228         RB_CLEAR_NODE(&rq->rb_node);
 229         memset(&rq->flush, 0, max(sizeof(rq->flush), sizeof(rq->elv)));
 230         rq->rq_disk = NULL;
 231         rq->part = NULL;
 232         rq->start_time = jiffies;
 233 #ifdef CONFIG_BLK_CGROUP
 234         rq->rl = NULL;
 235         set_start_time_ns(rq);
 236         rq->io_start_time_ns = 0;
 237 #endif
 238         rq->nr_phys_segments = 0;
 239 #if defined(CONFIG_BLK_DEV_INTEGRITY)
 240         rq->nr_integrity_segments = 0;
 241 #endif
 242         rq->ioprio = 0;
 243         rq->special = NULL;
 244         /* tag was already set */
 245         rq->errors = 0;
 246         memset(rq->__cmd, 0, sizeof(rq->__cmd));
 247         rq->cmd = rq->__cmd;
 248         rq->cmd_len = BLK_MAX_CDB;
 249
 250         rq->extra_len = 0;
 251         rq->sense_len = 0;
 252         rq->resid_len = 0;
 253         rq->sense = NULL;
 254
 255         rq->deadline = 0;
 256         INIT_LIST_HEAD(&rq->timeout_list);
 257         rq->timeout = 0;
 258         rq->retries = 0;
 259         rq->end_io = NULL;
 260         rq->end_io_data = NULL;
 261         rq->next_rq = NULL;
 262
 263         ctx->rq_dispatched[rw_is_sync(rw_flags)]++;
 264 }
 265
 266 static struct request *blk_mq_alloc_request_pinned(struct request_queue *q,
 267                                                    int rw, gfp_t gfp,
 268                                                    bool reserved)
 269 {
 270         struct request *rq;
 271
 272         do {
 273                 struct blk_mq_ctx *ctx = blk_mq_get_ctx(q);
 274                 struct blk_mq_hw_ctx *hctx = q->mq_ops->map_queue(q, ctx->cpu);
 275
 276                 rq = __blk_mq_alloc_request(hctx, ctx, gfp & ~__GFP_WAIT,
 277                                                 reserved);
 278                 if (rq) {
 279                         blk_mq_rq_ctx_init(q, ctx, rq, rw);
 280                         break;
 281                 }
 282
 283                 if (gfp & __GFP_WAIT) {
 284                         __blk_mq_run_hw_queue(hctx);
 285                         blk_mq_put_ctx(ctx);
 286                 } else {
 287                         blk_mq_put_ctx(ctx);
 288                         break;
 289                 }
 290
 291                 blk_mq_wait_for_tags(hctx, reserved);
 292         } while (1);
 293
 294         return rq;
 295 }
 296
 297 struct request *blk_mq_alloc_request(struct request_queue *q, int rw, gfp_t gfp)
 298 {
 299         struct request *rq;
 300
 301         if (blk_mq_queue_enter(q))
 302                 return NULL;
 303
 304         rq = blk_mq_alloc_request_pinned(q, rw, gfp, false);
 305         if (rq)
 306                 blk_mq_put_ctx(rq->mq_ctx);
 307         return rq;
 308 }
 309 EXPORT_SYMBOL(blk_mq_alloc_request);
 310
 311 struct request *blk_mq_alloc_reserved_request(struct request_queue *q, int rw,
 312                                               gfp_t gfp)
 313 {
 314         struct request *rq;
 315
 316         if (blk_mq_queue_enter(q))
 317                 return NULL;
 318
 319         rq = blk_mq_alloc_request_pinned(q, rw, gfp, true);
 320         if (rq)
 321                 blk_mq_put_ctx(rq->mq_ctx);
 322         return rq;
 323 }
 324 EXPORT_SYMBOL(blk_mq_alloc_reserved_request);
 325
 326 static void __blk_mq_free_request(struct blk_mq_hw_ctx *hctx,
 327                                   struct blk_mq_ctx *ctx, struct request *rq)
 328 {
 329         const int tag = rq->tag;
 330         struct request_queue *q = rq->q;
 331
 332         if (rq->cmd_flags & REQ_MQ_INFLIGHT)
 333                 atomic_dec(&hctx->nr_active);
 334
 335         clear_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
 336         blk_mq_put_tag(hctx, tag, &ctx->last_tag);
 337         blk_mq_queue_exit(q);
 338 }
 339
 340 void blk_mq_free_request(struct request *rq)
 341 {
 342         struct blk_mq_ctx *ctx = rq->mq_ctx;
 343         struct blk_mq_hw_ctx *hctx;
 344         struct request_queue *q = rq->q;
 345
 346         ctx->rq_completed[rq_is_sync(rq)]++;
 347
 348         hctx = q->mq_ops->map_queue(q, ctx->cpu);
 349         __blk_mq_free_request(hctx, ctx, rq);
 350 }
 351
 352 /*
 353  * Clone all relevant state from a request that has been put on hold in
 354  * the flush state machine into the preallocated flush request that hangs
 355  * off the request queue.
 356  *
 357  * For a driver the flush request should be invisible, that's why we are
 358  * impersonating the original request here.
 359  */
 360 void blk_mq_clone_flush_request(struct request *flush_rq,
 361                 struct request *orig_rq)
 362 {
 363         struct blk_mq_hw_ctx *hctx =
 364                 orig_rq->q->mq_ops->map_queue(orig_rq->q, orig_rq->mq_ctx->cpu);
 365
 366         flush_rq->mq_ctx = orig_rq->mq_ctx;
 367         flush_rq->tag = orig_rq->tag;
 368         memcpy(blk_mq_rq_to_pdu(flush_rq), blk_mq_rq_to_pdu(orig_rq),
 369                 hctx->cmd_size);
 370 }
 371
 372 inline void __blk_mq_end_io(struct request *rq, int error)
 373 {
 374         blk_account_io_done(rq);
 375
 376         if (rq->end_io) {
 377                 rq->end_io(rq, error);
 378         } else {
 379                 if (unlikely(blk_bidi_rq(rq)))
 380                         blk_mq_free_request(rq->next_rq);
 381                 blk_mq_free_request(rq);
 382         }
 383 }
 384 EXPORT_SYMBOL(__blk_mq_end_io);
 385
 386 void blk_mq_end_io(struct request *rq, int error)
 387 {
 388         if (blk_update_request(rq, error, blk_rq_bytes(rq)))
 389                 BUG();
 390         __blk_mq_end_io(rq, error);
 391 }
 392 EXPORT_SYMBOL(blk_mq_end_io);
 393
 394 static void __blk_mq_complete_request_remote(void *data)
 395 {
 396         struct request *rq = data;
 397
 398         rq->q->softirq_done_fn(rq);
 399 }
 400
 401 void __blk_mq_complete_request(struct request *rq)
 402 {
 403         struct blk_mq_ctx *ctx = rq->mq_ctx;
 404         bool shared = false;
 405         int cpu;
 406
 407         if (!test_bit(QUEUE_FLAG_SAME_COMP, &rq->q->queue_flags)) {
 408                 rq->q->softirq_done_fn(rq);
 409                 return;
 410         }
 411
 412         cpu = get_cpu();
 413         if (!test_bit(QUEUE_FLAG_SAME_FORCE, &rq->q->queue_flags))
 414                 shared = cpus_share_cache(cpu, ctx->cpu);
 415
 416         if (cpu != ctx->cpu && !shared && cpu_online(ctx->cpu)) {
 417                 rq->csd.func = __blk_mq_complete_request_remote;
 418                 rq->csd.info = rq;
 419                 rq->csd.flags = 0;
 420                 smp_call_function_single_async(ctx->cpu, &rq->csd);
 421         } else {
 422                 rq->q->softirq_done_fn(rq);
 423         }
 424         put_cpu();
 425 }
 426
 427 /**
 428  * blk_mq_complete_request - end I/O on a request
 429  * @rq:         the request being processed
 430  *
 431  * Description:
 432  *      Ends all I/O on a request. It does not handle partial completions.
 433  *      The actual completion happens out-of-order, through a IPI handler.
 434  **/
 435 void blk_mq_complete_request(struct request *rq)
 436 {
 437         if (unlikely(blk_should_fake_timeout(rq->q)))
 438                 return;
 439         if (!blk_mark_rq_complete(rq))
 440                 __blk_mq_complete_request(rq);
 441 }
 442 EXPORT_SYMBOL(blk_mq_complete_request);
 443
 444 static void blk_mq_start_request(struct request *rq, bool last)
 445 {
 446         struct request_queue *q = rq->q;
 447
 448         trace_block_rq_issue(q, rq);
 449
 450         rq->resid_len = blk_rq_bytes(rq);
 451         if (unlikely(blk_bidi_rq(rq)))
 452                 rq->next_rq->resid_len = blk_rq_bytes(rq->next_rq);
 453
 454         /*
 455          * Just mark start time and set the started bit. Due to memory
 456          * ordering, we know we'll see the correct deadline as long as
 457          * REQ_ATOMIC_STARTED is seen.
 458          */
 459         rq->deadline = jiffies + q->rq_timeout;
 460
 461         /*
 462          * Mark us as started and clear complete. Complete might have been
 463          * set if requeue raced with timeout, which then marked it as
 464          * complete. So be sure to clear complete again when we start
 465          * the request, otherwise we'll ignore the completion event.
 466          */
 467         set_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
 468         clear_bit(REQ_ATOM_COMPLETE, &rq->atomic_flags);
 469
 470         if (q->dma_drain_size && blk_rq_bytes(rq)) {
 471                 /*
 472                  * Make sure space for the drain appears.  We know we can do
 473                  * this because max_hw_segments has been adjusted to be one
 474                  * fewer than the device can handle.
 475                  */
 476                 rq->nr_phys_segments++;
 477         }
 478
 479         /*
 480          * Flag the last request in the series so that drivers know when IO
 481          * should be kicked off, if they don't do it on a per-request basis.
 482          *
 483          * Note: the flag isn't the only condition drivers should do kick off.
 484          * If drive is busy, the last request might not have the bit set.
 485          */
 486         if (last)
 487                 rq->cmd_flags |= REQ_END;
 488 }
 489
 490 static void __blk_mq_requeue_request(struct request *rq)
 491 {
 492         struct request_queue *q = rq->q;
 493
 494         trace_block_rq_requeue(q, rq);
 495         clear_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
 496
 497         rq->cmd_flags &= ~REQ_END;
 498
 499         if (q->dma_drain_size && blk_rq_bytes(rq))
 500                 rq->nr_phys_segments--;
 501 }
 502
 503 void blk_mq_requeue_request(struct request *rq)
 504 {
 505         __blk_mq_requeue_request(rq);
 506         blk_clear_rq_complete(rq);
 507
 508         BUG_ON(blk_queued_rq(rq));
 509         blk_mq_insert_request(rq, true, true, false);
 510 }
 511 EXPORT_SYMBOL(blk_mq_requeue_request);
 512
 513 struct request *blk_mq_tag_to_rq(struct blk_mq_tags *tags, unsigned int tag)
 514 {
 515         return tags->rqs[tag];
 516 }
 517 EXPORT_SYMBOL(blk_mq_tag_to_rq);
 518
 519 struct blk_mq_timeout_data {
 520         struct blk_mq_hw_ctx *hctx;
 521         unsigned long *next;
 522         unsigned int *next_set;
 523 };
 524
 525 static void blk_mq_timeout_check(void *__data, unsigned long *free_tags)
 526 {
 527         struct blk_mq_timeout_data *data = __data;
 528         struct blk_mq_hw_ctx *hctx = data->hctx;
 529         unsigned int tag;
 530
 531          /* It may not be in flight yet (this is where
 532          * the REQ_ATOMIC_STARTED flag comes in). The requests are
 533          * statically allocated, so we know it's always safe to access the
 534          * memory associated with a bit offset into ->rqs[].
 535          */
 536         tag = 0;
 537         do {
 538                 struct request *rq;
 539
 540                 tag = find_next_zero_bit(free_tags, hctx->tags->nr_tags, tag);
 541                 if (tag >= hctx->tags->nr_tags)
 542                         break;
 543
 544                 rq = blk_mq_tag_to_rq(hctx->tags, tag++);
 545                 if (rq->q != hctx->queue)
 546                         continue;
 547                 if (!test_bit(REQ_ATOM_STARTED, &rq->atomic_flags))
 548                         continue;
 549
 550                 blk_rq_check_expired(rq, data->next, data->next_set);
 551         } while (1);
 552 }
 553
 554 static void blk_mq_hw_ctx_check_timeout(struct blk_mq_hw_ctx *hctx,
 555                                         unsigned long *next,
 556                                         unsigned int *next_set)
 557 {
 558         struct blk_mq_timeout_data data = {
 559                 .hctx           = hctx,
 560                 .next           = next,
 561                 .next_set       = next_set,
 562         };
 563
 564         /*
 565          * Ask the tagging code to iterate busy requests, so we can
 566          * check them for timeout.
 567          */
 568         blk_mq_tag_busy_iter(hctx->tags, blk_mq_timeout_check, &data);
 569 }
 570
 571 static enum blk_eh_timer_return blk_mq_rq_timed_out(struct request *rq)
 572 {
 573         struct request_queue *q = rq->q;
 574
 575         /*
 576          * We know that complete is set at this point. If STARTED isn't set
 577          * anymore, then the request isn't active and the "timeout" should
 578          * just be ignored. This can happen due to the bitflag ordering.
 579          * Timeout first checks if STARTED is set, and if it is, assumes
 580          * the request is active. But if we race with completion, then
 581          * we both flags will get cleared. So check here again, and ignore
 582          * a timeout event with a request that isn't active.
 583          */
 584         if (!test_bit(REQ_ATOM_STARTED, &rq->atomic_flags))
 585                 return BLK_EH_NOT_HANDLED;
 586
 587         if (!q->mq_ops->timeout)
 588                 return BLK_EH_RESET_TIMER;
 589
 590         return q->mq_ops->timeout(rq);
 591 }
 592
 593 static void blk_mq_rq_timer(unsigned long data)
 594 {
 595         struct request_queue *q = (struct request_queue *) data;
 596         struct blk_mq_hw_ctx *hctx;
 597         unsigned long next = 0;
 598         int i, next_set = 0;
 599
 600         queue_for_each_hw_ctx(q, hctx, i)
 601                 blk_mq_hw_ctx_check_timeout(hctx, &next, &next_set);
 602
 603         if (next_set) {
 604                 next = blk_rq_timeout(round_jiffies_up(next));
 605                 mod_timer(&q->timeout, next);
 606         } else {
 607                 queue_for_each_hw_ctx(q, hctx, i)
 608                         blk_mq_tag_idle(hctx);
 609         }
 610 }
 611
 612 /*
 613  * Reverse check our software queue for entries that we could potentially
 614  * merge with. Currently includes a hand-wavy stop count of 8, to not spend
 615  * too much time checking for merges.
 616  */
 617 static bool blk_mq_attempt_merge(struct request_queue *q,
 618                                  struct blk_mq_ctx *ctx, struct bio *bio)
 619 {
 620         struct request *rq;
 621         int checked = 8;
 622
 623         list_for_each_entry_reverse(rq, &ctx->rq_list, queuelist) {
 624                 int el_ret;
 625
 626                 if (!checked--)
 627                         break;
 628
 629                 if (!blk_rq_merge_ok(rq, bio))
 630                         continue;
 631
 632                 el_ret = blk_try_merge(rq, bio);
 633                 if (el_ret == ELEVATOR_BACK_MERGE) {
 634                         if (bio_attempt_back_merge(q, rq, bio)) {
 635                                 ctx->rq_merged++;
 636                                 return true;
 637                         }
 638                         break;
 639                 } else if (el_ret == ELEVATOR_FRONT_MERGE) {
 640                         if (bio_attempt_front_merge(q, rq, bio)) {
 641                                 ctx->rq_merged++;
 642                                 return true;
 643                         }
 644                         break;
 645                 }
 646         }
 647
 648         return false;
 649 }
 650
 651 /*
 652  * Process software queues that have been marked busy, splicing them
 653  * to the for-dispatch
 654  */
 655 static void flush_busy_ctxs(struct blk_mq_hw_ctx *hctx, struct list_head *list)
 656 {
 657         struct blk_mq_ctx *ctx;
 658         int i;
 659
 660         for (i = 0; i < hctx->ctx_map.map_size; i++) {
 661                 struct blk_align_bitmap *bm = &hctx->ctx_map.map[i];
 662                 unsigned int off, bit;
 663
 664                 if (!bm->word)
 665                         continue;
 666
 667                 bit = 0;
 668                 off = i * hctx->ctx_map.bits_per_word;
 669                 do {
 670                         bit = find_next_bit(&bm->word, bm->depth, bit);
 671                         if (bit >= bm->depth)
 672                                 break;
 673
 674                         ctx = hctx->ctxs[bit + off];
 675                         clear_bit(bit, &bm->word);
 676                         spin_lock(&ctx->lock);
 677                         list_splice_tail_init(&ctx->rq_list, list);
 678                         spin_unlock(&ctx->lock);
 679
 680                         bit++;
 681                 } while (1);
 682         }
 683 }
 684
 685 /*
 686  * Run this hardware queue, pulling any software queues mapped to it in.
 687  * Note that this function currently has various problems around ordering
 688  * of IO. In particular, we'd like FIFO behaviour on handling existing
 689  * items on the hctx->dispatch list. Ignore that for now.
 690  */
 691 static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 692 {
 693         struct request_queue *q = hctx->queue;
 694         struct request *rq;
 695         LIST_HEAD(rq_list);
 696         int queued;
 697
 698         WARN_ON(!cpumask_test_cpu(raw_smp_processor_id(), hctx->cpumask));
 699
 700         if (unlikely(test_bit(BLK_MQ_S_STOPPED, &hctx->state)))
 701                 return;
 702
 703         hctx->run++;
 704
 705         /*
 706          * Touch any software queue that has pending entries.
 707          */
 708         flush_busy_ctxs(hctx, &rq_list);
 709
 710         /*
 711          * If we have previous entries on our dispatch list, grab them
 712          * and stuff them at the front for more fair dispatch.
 713          */
 714         if (!list_empty_careful(&hctx->dispatch)) {
 715                 spin_lock(&hctx->lock);
 716                 if (!list_empty(&hctx->dispatch))
 717                         list_splice_init(&hctx->dispatch, &rq_list);
 718                 spin_unlock(&hctx->lock);
 719         }
 720
 721         /*
 722          * Now process all the entries, sending them to the driver.
 723          */
 724         queued = 0;
 725         while (!list_empty(&rq_list)) {
 726                 int ret;
 727
 728                 rq = list_first_entry(&rq_list, struct request, queuelist);
 729                 list_del_init(&rq->queuelist);
 730
 731                 blk_mq_start_request(rq, list_empty(&rq_list));
 732
 733                 ret = q->mq_ops->queue_rq(hctx, rq);
 734                 switch (ret) {
 735                 case BLK_MQ_RQ_QUEUE_OK:
 736                         queued++;
 737                         continue;
 738                 case BLK_MQ_RQ_QUEUE_BUSY:
 739                         list_add(&rq->queuelist, &rq_list);
 740                         __blk_mq_requeue_request(rq);
 741                         break;
 742                 default:
 743                         pr_err("blk-mq: bad return on queue: %d\n", ret);
 744                 case BLK_MQ_RQ_QUEUE_ERROR:
 745                         rq->errors = -EIO;
 746                         blk_mq_end_io(rq, rq->errors);
 747                         break;
 748                 }
 749
 750                 if (ret == BLK_MQ_RQ_QUEUE_BUSY)
 751                         break;
 752         }
 753
 754         if (!queued)
 755                 hctx->dispatched[0]++;
 756         else if (queued < (1 << (BLK_MQ_MAX_DISPATCH_ORDER - 1)))
 757                 hctx->dispatched[ilog2(queued) + 1]++;
 758
 759         /*
 760          * Any items that need requeuing? Stuff them into hctx->dispatch,
 761          * that is where we will continue on next queue run.
 762          */
 763         if (!list_empty(&rq_list)) {
 764                 spin_lock(&hctx->lock);
 765                 list_splice(&rq_list, &hctx->dispatch);
 766                 spin_unlock(&hctx->lock);
 767         }
 768 }
 769
 770 /*
 771  * It'd be great if the workqueue API had a way to pass
 772  * in a mask and had some smarts for more clever placement.
 773  * For now we just round-robin here, switching for every
 774  * BLK_MQ_CPU_WORK_BATCH queued items.
 775  */
 776 static int blk_mq_hctx_next_cpu(struct blk_mq_hw_ctx *hctx)
 777 {
 778         int cpu = hctx->next_cpu;
 779
 780         if (--hctx->next_cpu_batch <= 0) {
 781                 int next_cpu;
 782
 783                 next_cpu = cpumask_next(hctx->next_cpu, hctx->cpumask);
 784                 if (next_cpu >= nr_cpu_ids)
 785                         next_cpu = cpumask_first(hctx->cpumask);
 786
 787                 hctx->next_cpu = next_cpu;
 788                 hctx->next_cpu_batch = BLK_MQ_CPU_WORK_BATCH;
 789         }
 790
 791         return cpu;
 792 }
 793
 794 void blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx, bool async)
 795 {
 796         if (unlikely(test_bit(BLK_MQ_S_STOPPED, &hctx->state)))
 797                 return;
 798
 799         if (!async && cpumask_test_cpu(smp_processor_id(), hctx->cpumask))
 800                 __blk_mq_run_hw_queue(hctx);
 801         else if (hctx->queue->nr_hw_queues == 1)
 802                 kblockd_schedule_delayed_work(&hctx->run_work, 0);
 803         else {
 804                 unsigned int cpu;
 805
 806                 cpu = blk_mq_hctx_next_cpu(hctx);
 807                 kblockd_schedule_delayed_work_on(cpu, &hctx->run_work, 0);
 808         }
 809 }
 810
 811 void blk_mq_run_queues(struct request_queue *q, bool async)
 812 {
 813         struct blk_mq_hw_ctx *hctx;
 814         int i;
 815
 816         queue_for_each_hw_ctx(q, hctx, i) {
 817                 if ((!blk_mq_hctx_has_pending(hctx) &&
 818                     list_empty_careful(&hctx->dispatch)) ||
 819                     test_bit(BLK_MQ_S_STOPPED, &hctx->state))
 820                         continue;
 821
 822                 preempt_disable();
 823                 blk_mq_run_hw_queue(hctx, async);
 824                 preempt_enable();
 825         }
 826 }
 827 EXPORT_SYMBOL(blk_mq_run_queues);
 828
 829 void blk_mq_stop_hw_queue(struct blk_mq_hw_ctx *hctx)
 830 {
 831         cancel_delayed_work(&hctx->run_work);
 832         cancel_delayed_work(&hctx->delay_work);
 833         set_bit(BLK_MQ_S_STOPPED, &hctx->state);
 834 }
 835 EXPORT_SYMBOL(blk_mq_stop_hw_queue);
 836
 837 void blk_mq_stop_hw_queues(struct request_queue *q)
 838 {
 839         struct blk_mq_hw_ctx *hctx;
 840         int i;
 841
 842         queue_for_each_hw_ctx(q, hctx, i)
 843                 blk_mq_stop_hw_queue(hctx);
 844 }
 845 EXPORT_SYMBOL(blk_mq_stop_hw_queues);
 846
 847 void blk_mq_start_hw_queue(struct blk_mq_hw_ctx *hctx)
 848 {
 849         clear_bit(BLK_MQ_S_STOPPED, &hctx->state);
 850
 851         preempt_disable();
 852         __blk_mq_run_hw_queue(hctx);
 853         preempt_enable();
 854 }
 855 EXPORT_SYMBOL(blk_mq_start_hw_queue);
 856
 857 void blk_mq_start_hw_queues(struct request_queue *q)
 858 {
 859         struct blk_mq_hw_ctx *hctx;
 860         int i;
 861
 862         queue_for_each_hw_ctx(q, hctx, i)
 863                 blk_mq_start_hw_queue(hctx);
 864 }
 865 EXPORT_SYMBOL(blk_mq_start_hw_queues);
 866
 867
 868 void blk_mq_start_stopped_hw_queues(struct request_queue *q, bool async)
 869 {
 870         struct blk_mq_hw_ctx *hctx;
 871         int i;
 872
 873         queue_for_each_hw_ctx(q, hctx, i) {
 874                 if (!test_bit(BLK_MQ_S_STOPPED, &hctx->state))
 875                         continue;
 876
 877                 clear_bit(BLK_MQ_S_STOPPED, &hctx->state);
 878                 preempt_disable();
 879                 blk_mq_run_hw_queue(hctx, async);
 880                 preempt_enable();
 881         }
 882 }
 883 EXPORT_SYMBOL(blk_mq_start_stopped_hw_queues);
 884
 885 static void blk_mq_run_work_fn(struct work_struct *work)
 886 {
 887         struct blk_mq_hw_ctx *hctx;
 888
 889         hctx = container_of(work, struct blk_mq_hw_ctx, run_work.work);
 890
 891         __blk_mq_run_hw_queue(hctx);
 892 }
 893
 894 static void blk_mq_delay_work_fn(struct work_struct *work)
 895 {
 896         struct blk_mq_hw_ctx *hctx;
 897
 898         hctx = container_of(work, struct blk_mq_hw_ctx, delay_work.work);
 899
 900         if (test_and_clear_bit(BLK_MQ_S_STOPPED, &hctx->state))
 901                 __blk_mq_run_hw_queue(hctx);
 902 }
 903
 904 void blk_mq_delay_queue(struct blk_mq_hw_ctx *hctx, unsigned long msecs)
 905 {
 906         unsigned long tmo = msecs_to_jiffies(msecs);
 907
 908         if (hctx->queue->nr_hw_queues == 1)
 909                 kblockd_schedule_delayed_work(&hctx->delay_work, tmo);
 910         else {
 911                 unsigned int cpu;
 912
 913                 cpu = blk_mq_hctx_next_cpu(hctx);
 914                 kblockd_schedule_delayed_work_on(cpu, &hctx->delay_work, tmo);
 915         }
 916 }
 917 EXPORT_SYMBOL(blk_mq_delay_queue);
 918
 919 static void __blk_mq_insert_request(struct blk_mq_hw_ctx *hctx,
 920                                     struct request *rq, bool at_head)
 921 {
 922         struct blk_mq_ctx *ctx = rq->mq_ctx;
 923
 924         trace_block_rq_insert(hctx->queue, rq);
 925
 926         if (at_head)
 927                 list_add(&rq->queuelist, &ctx->rq_list);
 928         else
 929                 list_add_tail(&rq->queuelist, &ctx->rq_list);
 930
 931         blk_mq_hctx_mark_pending(hctx, ctx);
 932
 933         /*
 934          * We do this early, to ensure we are on the right CPU.
 935          */
 936         blk_add_timer(rq);
 937 }
 938
 939 void blk_mq_insert_request(struct request *rq, bool at_head, bool run_queue,
 940                 bool async)
 941 {
 942         struct request_queue *q = rq->q;
 943         struct blk_mq_hw_ctx *hctx;
 944         struct blk_mq_ctx *ctx = rq->mq_ctx, *current_ctx;
 945
 946         current_ctx = blk_mq_get_ctx(q);
 947         if (!cpu_online(ctx->cpu))
 948                 rq->mq_ctx = ctx = current_ctx;
 949
 950         hctx = q->mq_ops->map_queue(q, ctx->cpu);
 951
 952         if (rq->cmd_flags & (REQ_FLUSH | REQ_FUA) &&
 953             !(rq->cmd_flags & (REQ_FLUSH_SEQ))) {
 954                 blk_insert_flush(rq);
 955         } else {
 956                 spin_lock(&ctx->lock);
 957                 __blk_mq_insert_request(hctx, rq, at_head);
 958                 spin_unlock(&ctx->lock);
 959         }
 960
 961         if (run_queue)
 962                 blk_mq_run_hw_queue(hctx, async);
 963
 964         blk_mq_put_ctx(current_ctx);
 965 }
 966
 967 static void blk_mq_insert_requests(struct request_queue *q,
 968                                      struct blk_mq_ctx *ctx,
 969                                      struct list_head *list,
 970                                      int depth,
 971                                      bool from_schedule)
 972
 973 {
 974         struct blk_mq_hw_ctx *hctx;
 975         struct blk_mq_ctx *current_ctx;
 976
 977         trace_block_unplug(q, depth, !from_schedule);
 978
 979         current_ctx = blk_mq_get_ctx(q);
 980
 981         if (!cpu_online(ctx->cpu))
 982                 ctx = current_ctx;
 983         hctx = q->mq_ops->map_queue(q, ctx->cpu);
 984
 985         /*
 986          * preemption doesn't flush plug list, so it's possible ctx->cpu is
 987          * offline now
 988          */
 989         spin_lock(&ctx->lock);
 990         while (!list_empty(list)) {
 991                 struct request *rq;
 992
 993                 rq = list_first_entry(list, struct request, queuelist);
 994                 list_del_init(&rq->queuelist);
 995                 rq->mq_ctx = ctx;
 996                 __blk_mq_insert_request(hctx, rq, false);
 997         }
 998         spin_unlock(&ctx->lock);
 999
1000         blk_mq_run_hw_queue(hctx, from_schedule);
1001         blk_mq_put_ctx(current_ctx);
1002 }
1003
1004 static int plug_ctx_cmp(void *priv, struct list_head *a, struct list_head *b)
1005 {
1006         struct request *rqa = container_of(a, struct request, queuelist);
1007         struct request *rqb = container_of(b, struct request, queuelist);
1008
1009         return !(rqa->mq_ctx < rqb->mq_ctx ||
1010                  (rqa->mq_ctx == rqb->mq_ctx &&
1011                   blk_rq_pos(rqa) < blk_rq_pos(rqb)));
1012 }
1013
1014 void blk_mq_flush_plug_list(struct blk_plug *plug, bool from_schedule)
1015 {
1016         struct blk_mq_ctx *this_ctx;
1017         struct request_queue *this_q;
1018         struct request *rq;
1019         LIST_HEAD(list);
1020         LIST_HEAD(ctx_list);
1021         unsigned int depth;
1022
1023         list_splice_init(&plug->mq_list, &list);
1024
1025         list_sort(NULL, &list, plug_ctx_cmp);
1026
1027         this_q = NULL;
1028         this_ctx = NULL;
1029         depth = 0;
1030
1031         while (!list_empty(&list)) {
1032                 rq = list_entry_rq(list.next);
1033                 list_del_init(&rq->queuelist);
1034                 BUG_ON(!rq->q);
1035                 if (rq->mq_ctx != this_ctx) {
1036                         if (this_ctx) {
1037                                 blk_mq_insert_requests(this_q, this_ctx,
1038                                                         &ctx_list, depth,
1039                                                         from_schedule);
1040                         }
1041
1042                         this_ctx = rq->mq_ctx;
1043                         this_q = rq->q;
1044                         depth = 0;
1045                 }
1046
1047                 depth++;
1048                 list_add_tail(&rq->queuelist, &ctx_list);
1049         }
1050
1051         /*
1052          * If 'this_ctx' is set, we know we have entries to complete
1053          * on 'ctx_list'. Do those.
1054          */
1055         if (this_ctx) {
1056                 blk_mq_insert_requests(this_q, this_ctx, &ctx_list, depth,
1057                                        from_schedule);
1058         }
1059 }
1060
1061 static void blk_mq_bio_to_request(struct request *rq, struct bio *bio)
1062 {
1063         init_request_from_bio(rq, bio);
1064         blk_account_io_start(rq, 1);
1065 }
1066
1067 static void blk_mq_make_request(struct request_queue *q, struct bio *bio)
1068 {
1069         struct blk_mq_hw_ctx *hctx;
1070         struct blk_mq_ctx *ctx;
1071         const int is_sync = rw_is_sync(bio->bi_rw);
1072         const int is_flush_fua = bio->bi_rw & (REQ_FLUSH | REQ_FUA);
1073         int rw = bio_data_dir(bio);
1074         struct request *rq;
1075         unsigned int use_plug, request_count = 0;
1076
1077         /*
1078          * If we have multiple hardware queues, just go directly to
1079          * one of those for sync IO.
1080          */
1081         use_plug = !is_flush_fua && ((q->nr_hw_queues == 1) || !is_sync);
1082
1083         blk_queue_bounce(q, &bio);
1084
1085         if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
1086                 bio_endio(bio, -EIO);
1087                 return;
1088         }
1089
1090         if (use_plug && !blk_queue_nomerges(q) &&
1091             blk_attempt_plug_merge(q, bio, &request_count))
1092                 return;
1093
1094         if (blk_mq_queue_enter(q)) {
1095                 bio_endio(bio, -EIO);
1096                 return;
1097         }
1098
1099         ctx = blk_mq_get_ctx(q);
1100         hctx = q->mq_ops->map_queue(q, ctx->cpu);
1101
1102         if (is_sync)
1103                 rw |= REQ_SYNC;
1104         trace_block_getrq(q, bio, rw);
1105         rq = __blk_mq_alloc_request(hctx, ctx, GFP_ATOMIC, false);
1106         if (likely(rq))
1107                 blk_mq_rq_ctx_init(q, ctx, rq, rw);
1108         else {
1109                 blk_mq_put_ctx(ctx);
1110                 trace_block_sleeprq(q, bio, rw);
1111                 rq = blk_mq_alloc_request_pinned(q, rw, __GFP_WAIT|GFP_ATOMIC,
1112                                                         false);
1113                 ctx = rq->mq_ctx;
1114                 hctx = q->mq_ops->map_queue(q, ctx->cpu);
1115         }
1116
1117         hctx->queued++;
1118
1119         if (unlikely(is_flush_fua)) {
1120                 blk_mq_bio_to_request(rq, bio);
1121                 blk_insert_flush(rq);
1122                 goto run_queue;
1123         }
1124
1125         /*
1126          * A task plug currently exists. Since this is completely lockless,
1127          * utilize that to temporarily store requests until the task is
1128          * either done or scheduled away.
1129          */
1130         if (use_plug) {
1131                 struct blk_plug *plug = current->plug;
1132
1133                 if (plug) {
1134                         blk_mq_bio_to_request(rq, bio);
1135                         if (list_empty(&plug->mq_list))
1136                                 trace_block_plug(q);
1137                         else if (request_count >= BLK_MAX_REQUEST_COUNT) {
1138                                 blk_flush_plug_list(plug, false);
1139                                 trace_block_plug(q);
1140                         }
1141                         list_add_tail(&rq->queuelist, &plug->mq_list);
1142                         blk_mq_put_ctx(ctx);
1143                         return;
1144                 }
1145         }
1146
1147         if (!(hctx->flags & BLK_MQ_F_SHOULD_MERGE)) {
1148                 blk_mq_bio_to_request(rq, bio);
1149                 spin_lock(&ctx->lock);
1150 insert_rq:
1151                 __blk_mq_insert_request(hctx, rq, false);
1152                 spin_unlock(&ctx->lock);
1153         } else {
1154                 spin_lock(&ctx->lock);
1155                 if (!blk_mq_attempt_merge(q, ctx, bio)) {
1156                         blk_mq_bio_to_request(rq, bio);
1157                         goto insert_rq;
1158                 }
1159
1160                 spin_unlock(&ctx->lock);
1161                 __blk_mq_free_request(hctx, ctx, rq);
1162         }
1163
1164
1165         /*
1166          * For a SYNC request, send it to the hardware immediately. For an
1167          * ASYNC request, just ensure that we run it later on. The latter
1168          * allows for merging opportunities and more efficient dispatching.
1169          */
1170 run_queue:
1171         blk_mq_run_hw_queue(hctx, !is_sync || is_flush_fua);
1172         blk_mq_put_ctx(ctx);
1173 }
1174
1175 /*
1176  * Default mapping to a software queue, since we use one per CPU.
1177  */
1178 struct blk_mq_hw_ctx *blk_mq_map_queue(struct request_queue *q, const int cpu)
1179 {
1180         return q->queue_hw_ctx[q->mq_map[cpu]];
1181 }
1182 EXPORT_SYMBOL(blk_mq_map_queue);
1183
1184 struct blk_mq_hw_ctx *blk_mq_alloc_single_hw_queue(struct blk_mq_tag_set *set,
1185                                                    unsigned int hctx_index)
1186 {
1187         return kzalloc_node(sizeof(struct blk_mq_hw_ctx), GFP_KERNEL,
1188                                 set->numa_node);
1189 }
1190 EXPORT_SYMBOL(blk_mq_alloc_single_hw_queue);
1191
1192 void blk_mq_free_single_hw_queue(struct blk_mq_hw_ctx *hctx,
1193                                  unsigned int hctx_index)
1194 {
1195         kfree(hctx);
1196 }
1197 EXPORT_SYMBOL(blk_mq_free_single_hw_queue);
1198
1199 static void blk_mq_hctx_notify(void *data, unsigned long action,
1200                                unsigned int cpu)
1201 {
1202         struct blk_mq_hw_ctx *hctx = data;
1203         struct request_queue *q = hctx->queue;
1204         struct blk_mq_ctx *ctx;
1205         LIST_HEAD(tmp);
1206
1207         if (action != CPU_DEAD && action != CPU_DEAD_FROZEN)
1208                 return;
1209
1210         /*
1211          * Move ctx entries to new CPU, if this one is going away.
1212          */
1213         ctx = __blk_mq_get_ctx(q, cpu);
1214
1215         spin_lock(&ctx->lock);
1216         if (!list_empty(&ctx->rq_list)) {
1217                 list_splice_init(&ctx->rq_list, &tmp);
1218                 blk_mq_hctx_clear_pending(hctx, ctx);
1219         }
1220         spin_unlock(&ctx->lock);
1221
1222         if (list_empty(&tmp))
1223                 return;
1224
1225         ctx = blk_mq_get_ctx(q);
1226         spin_lock(&ctx->lock);
1227
1228         while (!list_empty(&tmp)) {
1229                 struct request *rq;
1230
1231                 rq = list_first_entry(&tmp, struct request, queuelist);
1232                 rq->mq_ctx = ctx;
1233                 list_move_tail(&rq->queuelist, &ctx->rq_list);
1234         }
1235
1236         hctx = q->mq_ops->map_queue(q, ctx->cpu);
1237         blk_mq_hctx_mark_pending(hctx, ctx);
1238
1239         spin_unlock(&ctx->lock);
1240
1241         blk_mq_run_hw_queue(hctx, true);
1242         blk_mq_put_ctx(ctx);
1243 }
1244
1245 static void blk_mq_free_rq_map(struct blk_mq_tag_set *set,
1246                 struct blk_mq_tags *tags, unsigned int hctx_idx)
1247 {
1248         struct page *page;
1249
1250         if (tags->rqs && set->ops->exit_request) {
1251                 int i;
1252
1253                 for (i = 0; i < tags->nr_tags; i++) {
1254                         if (!tags->rqs[i])
1255                                 continue;
1256                         set->ops->exit_request(set->driver_data, tags->rqs[i],
1257                                                 hctx_idx, i);
1258                 }
1259         }
1260
1261         while (!list_empty(&tags->page_list)) {
1262                 page = list_first_entry(&tags->page_list, struct page, lru);
1263                 list_del_init(&page->lru);
1264                 __free_pages(page, page->private);
1265         }
1266
1267         kfree(tags->rqs);
1268
1269         blk_mq_free_tags(tags);
1270 }
1271
1272 static size_t order_to_size(unsigned int order)
1273 {
1274         return (size_t)PAGE_SIZE << order;
1275 }
1276
1277 static struct blk_mq_tags *blk_mq_init_rq_map(struct blk_mq_tag_set *set,
1278                 unsigned int hctx_idx)
1279 {
1280         struct blk_mq_tags *tags;
1281         unsigned int i, j, entries_per_page, max_order = 4;
1282         size_t rq_size, left;
1283
1284         tags = blk_mq_init_tags(set->queue_depth, set->reserved_tags,
1285                                 set->numa_node);
1286         if (!tags)
1287                 return NULL;
1288
1289         INIT_LIST_HEAD(&tags->page_list);
1290
1291         tags->rqs = kmalloc_node(set->queue_depth * sizeof(struct request *),
1292                                         GFP_KERNEL, set->numa_node);
1293         if (!tags->rqs) {
1294                 blk_mq_free_tags(tags);
1295                 return NULL;
1296         }
1297
1298         /*
1299          * rq_size is the size of the request plus driver payload, rounded
1300          * to the cacheline size
1301          */
1302         rq_size = round_up(sizeof(struct request) + set->cmd_size,
1303                                 cache_line_size());
1304         left = rq_size * set->queue_depth;
1305
1306         for (i = 0; i < set->queue_depth; ) {
1307                 int this_order = max_order;
1308                 struct page *page;
1309                 int to_do;
1310                 void *p;
1311
1312                 while (left < order_to_size(this_order - 1) && this_order)
1313                         this_order--;
1314
1315                 do {
1316                         page = alloc_pages_node(set->numa_node, GFP_KERNEL,
1317                                                 this_order);
1318                         if (page)
1319                                 break;
1320                         if (!this_order--)
1321                                 break;
1322                         if (order_to_size(this_order) < rq_size)
1323                                 break;
1324                 } while (1);
1325
1326                 if (!page)
1327                         goto fail;
1328
1329                 page->private = this_order;
1330                 list_add_tail(&page->lru, &tags->page_list);
1331
1332                 p = page_address(page);
1333                 entries_per_page = order_to_size(this_order) / rq_size;
1334                 to_do = min(entries_per_page, set->queue_depth - i);
1335                 left -= to_do * rq_size;
1336                 for (j = 0; j < to_do; j++) {
1337                         tags->rqs[i] = p;
1338                         if (set->ops->init_request) {
1339                                 if (set->ops->init_request(set->driver_data,
1340                                                 tags->rqs[i], hctx_idx, i,
1341                                                 set->numa_node))
1342                                         goto fail;
1343                         }
1344
1345                         p += rq_size;
1346                         i++;
1347                 }
1348         }
1349
1350         return tags;
1351
1352 fail:
1353         pr_warn("%s: failed to allocate requests\n", __func__);
1354         blk_mq_free_rq_map(set, tags, hctx_idx);
1355         return NULL;
1356 }
1357
1358 static void blk_mq_free_bitmap(struct blk_mq_ctxmap *bitmap)
1359 {
1360         kfree(bitmap->map);
1361 }
1362
1363 static int blk_mq_alloc_bitmap(struct blk_mq_ctxmap *bitmap, int node)
1364 {
1365         unsigned int bpw = 8, total, num_maps, i;
1366
1367         bitmap->bits_per_word = bpw;
1368
1369         num_maps = ALIGN(nr_cpu_ids, bpw) / bpw;
1370         bitmap->map = kzalloc_node(num_maps * sizeof(struct blk_align_bitmap),
1371                                         GFP_KERNEL, node);
1372         if (!bitmap->map)
1373                 return -ENOMEM;
1374
1375         bitmap->map_size = num_maps;
1376
1377         total = nr_cpu_ids;
1378         for (i = 0; i < num_maps; i++) {
1379                 bitmap->map[i].depth = min(total, bitmap->bits_per_word);
1380                 total -= bitmap->map[i].depth;
1381         }
1382
1383         return 0;
1384 }
1385
1386 static int blk_mq_init_hw_queues(struct request_queue *q,
1387                 struct blk_mq_tag_set *set)
1388 {
1389         struct blk_mq_hw_ctx *hctx;
1390         unsigned int i, j;
1391
1392         /*
1393          * Initialize hardware queues
1394          */
1395         queue_for_each_hw_ctx(q, hctx, i) {
1396                 int node;
1397
1398                 node = hctx->numa_node;
1399                 if (node == NUMA_NO_NODE)
1400                         node = hctx->numa_node = set->numa_node;
1401
1402                 INIT_DELAYED_WORK(&hctx->run_work, blk_mq_run_work_fn);
1403                 INIT_DELAYED_WORK(&hctx->delay_work, blk_mq_delay_work_fn);
1404                 spin_lock_init(&hctx->lock);
1405                 INIT_LIST_HEAD(&hctx->dispatch);
1406                 hctx->queue = q;
1407                 hctx->queue_num = i;
1408                 hctx->flags = set->flags;
1409                 hctx->cmd_size = set->cmd_size;
1410
1411                 blk_mq_init_cpu_notifier(&hctx->cpu_notifier,
1412                                                 blk_mq_hctx_notify, hctx);
1413                 blk_mq_register_cpu_notifier(&hctx->cpu_notifier);
1414
1415                 hctx->tags = set->tags[i];
1416
1417                 /*
1418                  * Allocate space for all possible cpus to avoid allocation in
1419                  * runtime
1420                  */
1421                 hctx->ctxs = kmalloc_node(nr_cpu_ids * sizeof(void *),
1422                                                 GFP_KERNEL, node);
1423                 if (!hctx->ctxs)
1424                         break;
1425
1426                 if (blk_mq_alloc_bitmap(&hctx->ctx_map, node))
1427                         break;
1428
1429                 hctx->nr_ctx = 0;
1430
1431                 if (set->ops->init_hctx &&
1432                     set->ops->init_hctx(hctx, set->driver_data, i))
1433                         break;
1434         }
1435
1436         if (i == q->nr_hw_queues)
1437                 return 0;
1438
1439         /*
1440          * Init failed
1441          */
1442         queue_for_each_hw_ctx(q, hctx, j) {
1443                 if (i == j)
1444                         break;
1445
1446                 if (set->ops->exit_hctx)
1447                         set->ops->exit_hctx(hctx, j);
1448
1449                 blk_mq_unregister_cpu_notifier(&hctx->cpu_notifier);
1450                 kfree(hctx->ctxs);
1451                 blk_mq_free_bitmap(&hctx->ctx_map);
1452         }
1453
1454         return 1;
1455 }
1456
1457 static void blk_mq_init_cpu_queues(struct request_queue *q,
1458                                    unsigned int nr_hw_queues)
1459 {
1460         unsigned int i;
1461
1462         for_each_possible_cpu(i) {
1463                 struct blk_mq_ctx *__ctx = per_cpu_ptr(q->queue_ctx, i);
1464                 struct blk_mq_hw_ctx *hctx;
1465
1466                 memset(__ctx, 0, sizeof(*__ctx));
1467                 __ctx->cpu = i;
1468                 spin_lock_init(&__ctx->lock);
1469                 INIT_LIST_HEAD(&__ctx->rq_list);
1470                 __ctx->queue = q;
1471
1472                 /* If the cpu isn't online, the cpu is mapped to first hctx */
1473                 if (!cpu_online(i))
1474                         continue;
1475
1476                 hctx = q->mq_ops->map_queue(q, i);
1477                 cpumask_set_cpu(i, hctx->cpumask);
1478                 hctx->nr_ctx++;
1479
1480                 /*
1481                  * Set local node, IFF we have more than one hw queue. If
1482                  * not, we remain on the home node of the device
1483                  */
1484                 if (nr_hw_queues > 1 && hctx->numa_node == NUMA_NO_NODE)
1485                         hctx->numa_node = cpu_to_node(i);
1486         }
1487 }
1488
1489 static void blk_mq_map_swqueue(struct request_queue *q)
1490 {
1491         unsigned int i;
1492         struct blk_mq_hw_ctx *hctx;
1493         struct blk_mq_ctx *ctx;
1494
1495         queue_for_each_hw_ctx(q, hctx, i) {
1496                 cpumask_clear(hctx->cpumask);
1497                 hctx->nr_ctx = 0;
1498         }
1499
1500         /*
1501          * Map software to hardware queues
1502          */
1503         queue_for_each_ctx(q, ctx, i) {
1504                 /* If the cpu isn't online, the cpu is mapped to first hctx */
1505                 if (!cpu_online(i))
1506                         continue;
1507
1508                 hctx = q->mq_ops->map_queue(q, i);
1509                 cpumask_set_cpu(i, hctx->cpumask);
1510                 ctx->index_hw = hctx->nr_ctx;
1511                 hctx->ctxs[hctx->nr_ctx++] = ctx;
1512         }
1513
1514         queue_for_each_hw_ctx(q, hctx, i) {
1515                 hctx->next_cpu = cpumask_first(hctx->cpumask);
1516                 hctx->next_cpu_batch = BLK_MQ_CPU_WORK_BATCH;
1517         }
1518 }
1519
1520 static void blk_mq_update_tag_set_depth(struct blk_mq_tag_set *set)
1521 {
1522         struct blk_mq_hw_ctx *hctx;
1523         struct request_queue *q;
1524         bool shared;
1525         int i;
1526
1527         if (set->tag_list.next == set->tag_list.prev)
1528                 shared = false;
1529         else
1530                 shared = true;
1531
1532         list_for_each_entry(q, &set->tag_list, tag_set_list) {
1533                 blk_mq_freeze_queue(q);
1534
1535                 queue_for_each_hw_ctx(q, hctx, i) {
1536                         if (shared)
1537                                 hctx->flags |= BLK_MQ_F_TAG_SHARED;
1538                         else
1539                                 hctx->flags &= ~BLK_MQ_F_TAG_SHARED;
1540                 }
1541                 blk_mq_unfreeze_queue(q);
1542         }
1543 }
1544
1545 static void blk_mq_del_queue_tag_set(struct request_queue *q)
1546 {
1547         struct blk_mq_tag_set *set = q->tag_set;
1548
1549         blk_mq_freeze_queue(q);
1550
1551         mutex_lock(&set->tag_list_lock);
1552         list_del_init(&q->tag_set_list);
1553         blk_mq_update_tag_set_depth(set);
1554         mutex_unlock(&set->tag_list_lock);
1555
1556         blk_mq_unfreeze_queue(q);
1557 }
1558
1559 static void blk_mq_add_queue_tag_set(struct blk_mq_tag_set *set,
1560                                      struct request_queue *q)
1561 {
1562         q->tag_set = set;
1563
1564         mutex_lock(&set->tag_list_lock);
1565         list_add_tail(&q->tag_set_list, &set->tag_list);
1566         blk_mq_update_tag_set_depth(set);
1567         mutex_unlock(&set->tag_list_lock);
1568 }
1569
1570 struct request_queue *blk_mq_init_queue(struct blk_mq_tag_set *set)
1571 {
1572         struct blk_mq_hw_ctx **hctxs;
1573         struct blk_mq_ctx *ctx;
1574         struct request_queue *q;
1575         int i;
1576
1577         ctx = alloc_percpu(struct blk_mq_ctx);
1578         if (!ctx)
1579                 return ERR_PTR(-ENOMEM);
1580
1581         hctxs = kmalloc_node(set->nr_hw_queues * sizeof(*hctxs), GFP_KERNEL,
1582                         set->numa_node);
1583
1584         if (!hctxs)
1585                 goto err_percpu;
1586
1587         for (i = 0; i < set->nr_hw_queues; i++) {
1588                 hctxs[i] = set->ops->alloc_hctx(set, i);
1589                 if (!hctxs[i])
1590                         goto err_hctxs;
1591
1592                 if (!zalloc_cpumask_var(&hctxs[i]->cpumask, GFP_KERNEL))
1593                         goto err_hctxs;
1594
1595                 atomic_set(&hctxs[i]->nr_active, 0);
1596                 hctxs[i]->numa_node = NUMA_NO_NODE;
1597                 hctxs[i]->queue_num = i;
1598         }
1599
1600         q = blk_alloc_queue_node(GFP_KERNEL, set->numa_node);
1601         if (!q)
1602                 goto err_hctxs;
1603
1604         q->mq_map = blk_mq_make_queue_map(set);
1605         if (!q->mq_map)
1606                 goto err_map;
1607
1608         setup_timer(&q->timeout, blk_mq_rq_timer, (unsigned long) q);
1609         blk_queue_rq_timeout(q, 30000);
1610
1611         q->nr_queues = nr_cpu_ids;
1612         q->nr_hw_queues = set->nr_hw_queues;
1613
1614         q->queue_ctx = ctx;
1615         q->queue_hw_ctx = hctxs;
1616
1617         q->mq_ops = set->ops;
1618         q->queue_flags |= QUEUE_FLAG_MQ_DEFAULT;
1619
1620         q->sg_reserved_size = INT_MAX;
1621
1622         blk_queue_make_request(q, blk_mq_make_request);
1623         blk_queue_rq_timed_out(q, blk_mq_rq_timed_out);
1624         if (set->timeout)
1625                 blk_queue_rq_timeout(q, set->timeout);
1626
1627         /*
1628          * Do this after blk_queue_make_request() overrides it...
1629          */
1630         q->nr_requests = set->queue_depth;
1631
1632         if (set->ops->complete)
1633                 blk_queue_softirq_done(q, set->ops->complete);
1634
1635         blk_mq_init_flush(q);
1636         blk_mq_init_cpu_queues(q, set->nr_hw_queues);
1637
1638         q->flush_rq = kzalloc(round_up(sizeof(struct request) +
1639                                 set->cmd_size, cache_line_size()),
1640                                 GFP_KERNEL);
1641         if (!q->flush_rq)
1642                 goto err_hw;
1643
1644         if (blk_mq_init_hw_queues(q, set))
1645                 goto err_flush_rq;
1646
1647         blk_mq_map_swqueue(q);
1648
1649         mutex_lock(&all_q_mutex);
1650         list_add_tail(&q->all_q_node, &all_q_list);
1651         mutex_unlock(&all_q_mutex);
1652
1653         blk_mq_add_queue_tag_set(set, q);
1654
1655         return q;
1656
1657 err_flush_rq:
1658         kfree(q->flush_rq);
1659 err_hw:
1660         kfree(q->mq_map);
1661 err_map:
1662         blk_cleanup_queue(q);
1663 err_hctxs:
1664         for (i = 0; i < set->nr_hw_queues; i++) {
1665                 if (!hctxs[i])
1666                         break;
1667                 free_cpumask_var(hctxs[i]->cpumask);
1668                 set->ops->free_hctx(hctxs[i], i);
1669         }
1670         kfree(hctxs);
1671 err_percpu:
1672         free_percpu(ctx);
1673         return ERR_PTR(-ENOMEM);
1674 }
1675 EXPORT_SYMBOL(blk_mq_init_queue);
1676
1677 void blk_mq_free_queue(struct request_queue *q)
1678 {
1679         struct blk_mq_hw_ctx *hctx;
1680         int i;
1681
1682         blk_mq_del_queue_tag_set(q);
1683
1684         queue_for_each_hw_ctx(q, hctx, i) {
1685                 kfree(hctx->ctxs);
1686                 blk_mq_unregister_cpu_notifier(&hctx->cpu_notifier);
1687                 if (q->mq_ops->exit_hctx)
1688                         q->mq_ops->exit_hctx(hctx, i);
1689                 free_cpumask_var(hctx->cpumask);
1690                 q->mq_ops->free_hctx(hctx, i);
1691         }
1692
1693         free_percpu(q->queue_ctx);
1694         kfree(q->queue_hw_ctx);
1695         kfree(q->mq_map);
1696
1697         q->queue_ctx = NULL;
1698         q->queue_hw_ctx = NULL;
1699         q->mq_map = NULL;
1700
1701         mutex_lock(&all_q_mutex);
1702         list_del_init(&q->all_q_node);
1703         mutex_unlock(&all_q_mutex);
1704 }
1705
1706 /* Basically redo blk_mq_init_queue with queue frozen */
1707 static void blk_mq_queue_reinit(struct request_queue *q)
1708 {
1709         blk_mq_freeze_queue(q);
1710
1711         blk_mq_update_queue_map(q->mq_map, q->nr_hw_queues);
1712
1713         /*
1714          * redo blk_mq_init_cpu_queues and blk_mq_init_hw_queues. FIXME: maybe
1715          * we should change hctx numa_node according to new topology (this
1716          * involves free and re-allocate memory, worthy doing?)
1717          */
1718
1719         blk_mq_map_swqueue(q);
1720
1721         blk_mq_unfreeze_queue(q);
1722 }
1723
1724 static int blk_mq_queue_reinit_notify(struct notifier_block *nb,
1725                                       unsigned long action, void *hcpu)
1726 {
1727         struct request_queue *q;
1728
1729         /*
1730          * Before new mappings are established, hotadded cpu might already
1731          * start handling requests. This doesn't break anything as we map
1732          * offline CPUs to first hardware queue. We will re-init the queue
1733          * below to get optimal settings.
1734          */
1735         if (action != CPU_DEAD && action != CPU_DEAD_FROZEN &&
1736             action != CPU_ONLINE && action != CPU_ONLINE_FROZEN)
1737                 return NOTIFY_OK;
1738
1739         mutex_lock(&all_q_mutex);
1740         list_for_each_entry(q, &all_q_list, all_q_node)
1741                 blk_mq_queue_reinit(q);
1742         mutex_unlock(&all_q_mutex);
1743         return NOTIFY_OK;
1744 }
1745
1746 int blk_mq_alloc_tag_set(struct blk_mq_tag_set *set)
1747 {
1748         int i;
1749
1750         if (!set->nr_hw_queues)
1751                 return -EINVAL;
1752         if (!set->queue_depth || set->queue_depth > BLK_MQ_MAX_DEPTH)
1753                 return -EINVAL;
1754         if (set->queue_depth < set->reserved_tags + BLK_MQ_TAG_MIN)
1755                 return -EINVAL;
1756
1757         if (!set->nr_hw_queues ||
1758             !set->ops->queue_rq || !set->ops->map_queue ||
1759             !set->ops->alloc_hctx || !set->ops->free_hctx)
1760                 return -EINVAL;
1761
1762
1763         set->tags = kmalloc_node(set->nr_hw_queues *
1764                                  sizeof(struct blk_mq_tags *),
1765                                  GFP_KERNEL, set->numa_node);
1766         if (!set->tags)
1767                 goto out;
1768
1769         for (i = 0; i < set->nr_hw_queues; i++) {
1770                 set->tags[i] = blk_mq_init_rq_map(set, i);
1771                 if (!set->tags[i])
1772                         goto out_unwind;
1773         }
1774
1775         mutex_init(&set->tag_list_lock);
1776         INIT_LIST_HEAD(&set->tag_list);
1777
1778         return 0;
1779
1780 out_unwind:
1781         while (--i >= 0)
1782                 blk_mq_free_rq_map(set, set->tags[i], i);
1783 out:
1784         return -ENOMEM;
1785 }
1786 EXPORT_SYMBOL(blk_mq_alloc_tag_set);
1787
1788 void blk_mq_free_tag_set(struct blk_mq_tag_set *set)
1789 {
1790         int i;
1791
1792         for (i = 0; i < set->nr_hw_queues; i++)
1793                 blk_mq_free_rq_map(set, set->tags[i], i);
1794         kfree(set->tags);
1795 }
1796 EXPORT_SYMBOL(blk_mq_free_tag_set);
1797
1798 int blk_mq_update_nr_requests(struct request_queue *q, unsigned int nr)
1799 {
1800         struct blk_mq_tag_set *set = q->tag_set;
1801         struct blk_mq_hw_ctx *hctx;
1802         int i, ret;
1803
1804         if (!set || nr > set->queue_depth)
1805                 return -EINVAL;
1806
1807         ret = 0;
1808         queue_for_each_hw_ctx(q, hctx, i) {
1809                 ret = blk_mq_tag_update_depth(hctx->tags, nr);
1810                 if (ret)
1811                         break;
1812         }
1813
1814         if (!ret)
1815                 q->nr_requests = nr;
1816
1817         return ret;
1818 }
1819
1820 void blk_mq_disable_hotplug(void)
1821 {
1822         mutex_lock(&all_q_mutex);
1823 }
1824
1825 void blk_mq_enable_hotplug(void)
1826 {
1827         mutex_unlock(&all_q_mutex);
1828 }
1829
1830 static int __init blk_mq_init(void)
1831 {
1832         blk_mq_cpu_init();
1833
1834         /* Must be called after percpu_counter_hotcpu_callback() */
1835         hotcpu_notifier(blk_mq_queue_reinit_notify, -10);
1836
1837         return 0;
1838 }
1839 subsys_initcall(blk_mq_init);