drivers/soc/ti/knav_qmss_queue.c

   1 /*
   2  * Keystone Queue Manager subsystem driver
   3  *
   4  * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
   5  * Authors:     Sandeep Nair <sandeep_n@ti.com>
   6  *              Cyril Chemparathy <cyril@ti.com>
   7  *              Santosh Shilimkar <santosh.shilimkar@ti.com>
   8  *
   9  * This program is free software; you can redistribute it and/or
  10  * modify it under the terms of the GNU General Public License
  11  * version 2 as published by the Free Software Foundation.
  12  *
  13  * This program is distributed in the hope that it will be useful, but
  14  * WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * General Public License for more details.
  17  */
  18
  19 #include <linux/kernel.h>
  20 #include <linux/module.h>
  21 #include <linux/device.h>
  22 #include <linux/clk.h>
  23 #include <linux/io.h>
  24 #include <linux/interrupt.h>
  25 #include <linux/bitops.h>
  26 #include <linux/slab.h>
  27 #include <linux/spinlock.h>
  28 #include <linux/platform_device.h>
  29 #include <linux/dma-mapping.h>
  30 #include <linux/of.h>
  31 #include <linux/of_irq.h>
  32 #include <linux/of_device.h>
  33 #include <linux/of_address.h>
  34 #include <linux/pm_runtime.h>
  35 #include <linux/firmware.h>
  36 #include <linux/debugfs.h>
  37 #include <linux/seq_file.h>
  38 #include <linux/string.h>
  39 #include <linux/soc/ti/knav_qmss.h>
  40
  41 #include "knav_qmss.h"
  42
  43 static struct knav_device *kdev;
  44 static DEFINE_MUTEX(knav_dev_lock);
  45
  46 /* Queue manager register indices in DTS */
  47 #define KNAV_QUEUE_PEEK_REG_INDEX       0
  48 #define KNAV_QUEUE_STATUS_REG_INDEX     1
  49 #define KNAV_QUEUE_CONFIG_REG_INDEX     2
  50 #define KNAV_QUEUE_REGION_REG_INDEX     3
  51 #define KNAV_QUEUE_PUSH_REG_INDEX       4
  52 #define KNAV_QUEUE_POP_REG_INDEX        5
  53
  54 /* PDSP register indices in DTS */
  55 #define KNAV_QUEUE_PDSP_IRAM_REG_INDEX  0
  56 #define KNAV_QUEUE_PDSP_REGS_REG_INDEX  1
  57 #define KNAV_QUEUE_PDSP_INTD_REG_INDEX  2
  58 #define KNAV_QUEUE_PDSP_CMD_REG_INDEX   3
  59
  60 #define knav_queue_idx_to_inst(kdev, idx)                       \
  61         (kdev->instances + (idx << kdev->inst_shift))
  62
  63 #define for_each_handle_rcu(qh, inst)                   \
  64         list_for_each_entry_rcu(qh, &inst->handles, list)
  65
  66 #define for_each_instance(idx, inst, kdev)              \
  67         for (idx = 0, inst = kdev->instances;           \
  68              idx < (kdev)->num_queues_in_use;                   \
  69              idx++, inst = knav_queue_idx_to_inst(kdev, idx))
  70
  71 /**
  72  * knav_queue_notify: qmss queue notfier call
  73  *
  74  * @inst:               qmss queue instance like accumulator
  75  */
  76 void knav_queue_notify(struct knav_queue_inst *inst)
  77 {
  78         struct knav_queue *qh;
  79
  80         if (!inst)
  81                 return;
  82
  83         rcu_read_lock();
  84         for_each_handle_rcu(qh, inst) {
  85                 if (atomic_read(&qh->notifier_enabled) <= 0)
  86                         continue;
  87                 if (WARN_ON(!qh->notifier_fn))
  88                         continue;
  89                 atomic_inc(&qh->stats.notifies);
  90                 qh->notifier_fn(qh->notifier_fn_arg);
  91         }
  92         rcu_read_unlock();
  93 }
  94 EXPORT_SYMBOL_GPL(knav_queue_notify);
  95
  96 static irqreturn_t knav_queue_int_handler(int irq, void *_instdata)
  97 {
  98         struct knav_queue_inst *inst = _instdata;
  99
 100         knav_queue_notify(inst);
 101         return IRQ_HANDLED;
 102 }
 103
 104 static int knav_queue_setup_irq(struct knav_range_info *range,
 105                           struct knav_queue_inst *inst)
 106 {
 107         unsigned queue = inst->id - range->queue_base;
 108         unsigned long cpu_map;
 109         int ret = 0, irq;
 110
 111         if (range->flags & RANGE_HAS_IRQ) {
 112                 irq = range->irqs[queue].irq;
 113                 cpu_map = range->irqs[queue].cpu_map;
 114                 ret = request_irq(irq, knav_queue_int_handler, 0,
 115                                         inst->irq_name, inst);
 116                 if (ret)
 117                         return ret;
 118                 disable_irq(irq);
 119                 if (cpu_map) {
 120                         ret = irq_set_affinity_hint(irq, to_cpumask(&cpu_map));
 121                         if (ret) {
 122                                 dev_warn(range->kdev->dev,
 123                                          "Failed to set IRQ affinity\n");
 124                                 return ret;
 125                         }
 126                 }
 127         }
 128         return ret;
 129 }
 130
 131 static void knav_queue_free_irq(struct knav_queue_inst *inst)
 132 {
 133         struct knav_range_info *range = inst->range;
 134         unsigned queue = inst->id - inst->range->queue_base;
 135         int irq;
 136
 137         if (range->flags & RANGE_HAS_IRQ) {
 138                 irq = range->irqs[queue].irq;
 139                 irq_set_affinity_hint(irq, NULL);
 140                 free_irq(irq, inst);
 141         }
 142 }
 143
 144 static inline bool knav_queue_is_busy(struct knav_queue_inst *inst)
 145 {
 146         return !list_empty(&inst->handles);
 147 }
 148
 149 static inline bool knav_queue_is_reserved(struct knav_queue_inst *inst)
 150 {
 151         return inst->range->flags & RANGE_RESERVED;
 152 }
 153
 154 static inline bool knav_queue_is_shared(struct knav_queue_inst *inst)
 155 {
 156         struct knav_queue *tmp;
 157
 158         rcu_read_lock();
 159         for_each_handle_rcu(tmp, inst) {
 160                 if (tmp->flags & KNAV_QUEUE_SHARED) {
 161                         rcu_read_unlock();
 162                         return true;
 163                 }
 164         }
 165         rcu_read_unlock();
 166         return false;
 167 }
 168
 169 static inline bool knav_queue_match_type(struct knav_queue_inst *inst,
 170                                                 unsigned type)
 171 {
 172         if ((type == KNAV_QUEUE_QPEND) &&
 173             (inst->range->flags & RANGE_HAS_IRQ)) {
 174                 return true;
 175         } else if ((type == KNAV_QUEUE_ACC) &&
 176                 (inst->range->flags & RANGE_HAS_ACCUMULATOR)) {
 177                 return true;
 178         } else if ((type == KNAV_QUEUE_GP) &&
 179                 !(inst->range->flags &
 180                         (RANGE_HAS_ACCUMULATOR | RANGE_HAS_IRQ))) {
 181                 return true;
 182         }
 183         return false;
 184 }
 185
 186 static inline struct knav_queue_inst *
 187 knav_queue_match_id_to_inst(struct knav_device *kdev, unsigned id)
 188 {
 189         struct knav_queue_inst *inst;
 190         int idx;
 191
 192         for_each_instance(idx, inst, kdev) {
 193                 if (inst->id == id)
 194                         return inst;
 195         }
 196         return NULL;
 197 }
 198
 199 static inline struct knav_queue_inst *knav_queue_find_by_id(int id)
 200 {
 201         if (kdev->base_id <= id &&
 202             kdev->base_id + kdev->num_queues > id) {
 203                 id -= kdev->base_id;
 204                 return knav_queue_match_id_to_inst(kdev, id);
 205         }
 206         return NULL;
 207 }
 208
 209 static struct knav_queue *__knav_queue_open(struct knav_queue_inst *inst,
 210                                       const char *name, unsigned flags)
 211 {
 212         struct knav_queue *qh;
 213         unsigned id;
 214         int ret = 0;
 215
 216         qh = devm_kzalloc(inst->kdev->dev, sizeof(*qh), GFP_KERNEL);
 217         if (!qh)
 218                 return ERR_PTR(-ENOMEM);
 219
 220         qh->flags = flags;
 221         qh->inst = inst;
 222         id = inst->id - inst->qmgr->start_queue;
 223         qh->reg_push = &inst->qmgr->reg_push[id];
 224         qh->reg_pop = &inst->qmgr->reg_pop[id];
 225         qh->reg_peek = &inst->qmgr->reg_peek[id];
 226
 227         /* first opener? */
 228         if (!knav_queue_is_busy(inst)) {
 229                 struct knav_range_info *range = inst->range;
 230
 231                 inst->name = kstrndup(name, KNAV_NAME_SIZE, GFP_KERNEL);
 232                 if (range->ops && range->ops->open_queue)
 233                         ret = range->ops->open_queue(range, inst, flags);
 234
 235                 if (ret) {
 236                         devm_kfree(inst->kdev->dev, qh);
 237                         return ERR_PTR(ret);
 238                 }
 239         }
 240         list_add_tail_rcu(&qh->list, &inst->handles);
 241         return qh;
 242 }
 243
 244 static struct knav_queue *
 245 knav_queue_open_by_id(const char *name, unsigned id, unsigned flags)
 246 {
 247         struct knav_queue_inst *inst;
 248         struct knav_queue *qh;
 249
 250         mutex_lock(&knav_dev_lock);
 251
 252         qh = ERR_PTR(-ENODEV);
 253         inst = knav_queue_find_by_id(id);
 254         if (!inst)
 255                 goto unlock_ret;
 256
 257         qh = ERR_PTR(-EEXIST);
 258         if (!(flags & KNAV_QUEUE_SHARED) && knav_queue_is_busy(inst))
 259                 goto unlock_ret;
 260
 261         qh = ERR_PTR(-EBUSY);
 262         if ((flags & KNAV_QUEUE_SHARED) &&
 263             (knav_queue_is_busy(inst) && !knav_queue_is_shared(inst)))
 264                 goto unlock_ret;
 265
 266         qh = __knav_queue_open(inst, name, flags);
 267
 268 unlock_ret:
 269         mutex_unlock(&knav_dev_lock);
 270
 271         return qh;
 272 }
 273
 274 static struct knav_queue *knav_queue_open_by_type(const char *name,
 275                                                 unsigned type, unsigned flags)
 276 {
 277         struct knav_queue_inst *inst;
 278         struct knav_queue *qh = ERR_PTR(-EINVAL);
 279         int idx;
 280
 281         mutex_lock(&knav_dev_lock);
 282
 283         for_each_instance(idx, inst, kdev) {
 284                 if (knav_queue_is_reserved(inst))
 285                         continue;
 286                 if (!knav_queue_match_type(inst, type))
 287                         continue;
 288                 if (knav_queue_is_busy(inst))
 289                         continue;
 290                 qh = __knav_queue_open(inst, name, flags);
 291                 goto unlock_ret;
 292         }
 293
 294 unlock_ret:
 295         mutex_unlock(&knav_dev_lock);
 296         return qh;
 297 }
 298
 299 static void knav_queue_set_notify(struct knav_queue_inst *inst, bool enabled)
 300 {
 301         struct knav_range_info *range = inst->range;
 302
 303         if (range->ops && range->ops->set_notify)
 304                 range->ops->set_notify(range, inst, enabled);
 305 }
 306
 307 static int knav_queue_enable_notifier(struct knav_queue *qh)
 308 {
 309         struct knav_queue_inst *inst = qh->inst;
 310         bool first;
 311
 312         if (WARN_ON(!qh->notifier_fn))
 313                 return -EINVAL;
 314
 315         /* Adjust the per handle notifier count */
 316         first = (atomic_inc_return(&qh->notifier_enabled) == 1);
 317         if (!first)
 318                 return 0; /* nothing to do */
 319
 320         /* Now adjust the per instance notifier count */
 321         first = (atomic_inc_return(&inst->num_notifiers) == 1);
 322         if (first)
 323                 knav_queue_set_notify(inst, true);
 324
 325         return 0;
 326 }
 327
 328 static int knav_queue_disable_notifier(struct knav_queue *qh)
 329 {
 330         struct knav_queue_inst *inst = qh->inst;
 331         bool last;
 332
 333         last = (atomic_dec_return(&qh->notifier_enabled) == 0);
 334         if (!last)
 335                 return 0; /* nothing to do */
 336
 337         last = (atomic_dec_return(&inst->num_notifiers) == 0);
 338         if (last)
 339                 knav_queue_set_notify(inst, false);
 340
 341         return 0;
 342 }
 343
 344 static int knav_queue_set_notifier(struct knav_queue *qh,
 345                                 struct knav_queue_notify_config *cfg)
 346 {
 347         knav_queue_notify_fn old_fn = qh->notifier_fn;
 348
 349         if (!cfg)
 350                 return -EINVAL;
 351
 352         if (!(qh->inst->range->flags & (RANGE_HAS_ACCUMULATOR | RANGE_HAS_IRQ)))
 353                 return -ENOTSUPP;
 354
 355         if (!cfg->fn && old_fn)
 356                 knav_queue_disable_notifier(qh);
 357
 358         qh->notifier_fn = cfg->fn;
 359         qh->notifier_fn_arg = cfg->fn_arg;
 360
 361         if (cfg->fn && !old_fn)
 362                 knav_queue_enable_notifier(qh);
 363
 364         return 0;
 365 }
 366
 367 static int knav_gp_set_notify(struct knav_range_info *range,
 368                                struct knav_queue_inst *inst,
 369                                bool enabled)
 370 {
 371         unsigned queue;
 372
 373         if (range->flags & RANGE_HAS_IRQ) {
 374                 queue = inst->id - range->queue_base;
 375                 if (enabled)
 376                         enable_irq(range->irqs[queue].irq);
 377                 else
 378                         disable_irq_nosync(range->irqs[queue].irq);
 379         }
 380         return 0;
 381 }
 382
 383 static int knav_gp_open_queue(struct knav_range_info *range,
 384                                 struct knav_queue_inst *inst, unsigned flags)
 385 {
 386         return knav_queue_setup_irq(range, inst);
 387 }
 388
 389 static int knav_gp_close_queue(struct knav_range_info *range,
 390                                 struct knav_queue_inst *inst)
 391 {
 392         knav_queue_free_irq(inst);
 393         return 0;
 394 }
 395
 396 struct knav_range_ops knav_gp_range_ops = {
 397         .set_notify     = knav_gp_set_notify,
 398         .open_queue     = knav_gp_open_queue,
 399         .close_queue    = knav_gp_close_queue,
 400 };
 401
 402
 403 static int knav_queue_get_count(void *qhandle)
 404 {
 405         struct knav_queue *qh = qhandle;
 406         struct knav_queue_inst *inst = qh->inst;
 407
 408         return readl_relaxed(&qh->reg_peek[0].entry_count) +
 409                 atomic_read(&inst->desc_count);
 410 }
 411
 412 static void knav_queue_debug_show_instance(struct seq_file *s,
 413                                         struct knav_queue_inst *inst)
 414 {
 415         struct knav_device *kdev = inst->kdev;
 416         struct knav_queue *qh;
 417
 418         if (!knav_queue_is_busy(inst))
 419                 return;
 420
 421         seq_printf(s, "\tqueue id %d (%s)\n",
 422                    kdev->base_id + inst->id, inst->name);
 423         for_each_handle_rcu(qh, inst) {
 424                 seq_printf(s, "\t\thandle %p: ", qh);
 425                 seq_printf(s, "pushes %8d, ",
 426                            atomic_read(&qh->stats.pushes));
 427                 seq_printf(s, "pops %8d, ",
 428                            atomic_read(&qh->stats.pops));
 429                 seq_printf(s, "count %8d, ",
 430                            knav_queue_get_count(qh));
 431                 seq_printf(s, "notifies %8d, ",
 432                            atomic_read(&qh->stats.notifies));
 433                 seq_printf(s, "push errors %8d, ",
 434                            atomic_read(&qh->stats.push_errors));
 435                 seq_printf(s, "pop errors %8d\n",
 436                            atomic_read(&qh->stats.pop_errors));
 437         }
 438 }
 439
 440 static int knav_queue_debug_show(struct seq_file *s, void *v)
 441 {
 442         struct knav_queue_inst *inst;
 443         int idx;
 444
 445         mutex_lock(&knav_dev_lock);
 446         seq_printf(s, "%s: %u-%u\n",
 447                    dev_name(kdev->dev), kdev->base_id,
 448                    kdev->base_id + kdev->num_queues - 1);
 449         for_each_instance(idx, inst, kdev)
 450                 knav_queue_debug_show_instance(s, inst);
 451         mutex_unlock(&knav_dev_lock);
 452
 453         return 0;
 454 }
 455
 456 static int knav_queue_debug_open(struct inode *inode, struct file *file)
 457 {
 458         return single_open(file, knav_queue_debug_show, NULL);
 459 }
 460
 461 static const struct file_operations knav_queue_debug_ops = {
 462         .open           = knav_queue_debug_open,
 463         .read           = seq_read,
 464         .llseek         = seq_lseek,
 465         .release        = single_release,
 466 };
 467
 468 static inline int knav_queue_pdsp_wait(u32 * __iomem addr, unsigned timeout,
 469                                         u32 flags)
 470 {
 471         unsigned long end;
 472         u32 val = 0;
 473
 474         end = jiffies + msecs_to_jiffies(timeout);
 475         while (time_after(end, jiffies)) {
 476                 val = readl_relaxed(addr);
 477                 if (flags)
 478                         val &= flags;
 479                 if (!val)
 480                         break;
 481                 cpu_relax();
 482         }
 483         return val ? -ETIMEDOUT : 0;
 484 }
 485
 486
 487 static int knav_queue_flush(struct knav_queue *qh)
 488 {
 489         struct knav_queue_inst *inst = qh->inst;
 490         unsigned id = inst->id - inst->qmgr->start_queue;
 491
 492         atomic_set(&inst->desc_count, 0);
 493         writel_relaxed(0, &inst->qmgr->reg_push[id].ptr_size_thresh);
 494         return 0;
 495 }
 496
 497 /**
 498  * knav_queue_open()    - open a hardware queue
 499  * @name                - name to give the queue handle
 500  * @id                  - desired queue number if any or specifes the type
 501  *                        of queue
 502  * @flags               - the following flags are applicable to queues:
 503  *      KNAV_QUEUE_SHARED - allow the queue to be shared. Queues are
 504  *                           exclusive by default.
 505  *                           Subsequent attempts to open a shared queue should
 506  *                           also have this flag.
 507  *
 508  * Returns a handle to the open hardware queue if successful. Use IS_ERR()
 509  * to check the returned value for error codes.
 510  */
 511 void *knav_queue_open(const char *name, unsigned id,
 512                                         unsigned flags)
 513 {
 514         struct knav_queue *qh = ERR_PTR(-EINVAL);
 515
 516         switch (id) {
 517         case KNAV_QUEUE_QPEND:
 518         case KNAV_QUEUE_ACC:
 519         case KNAV_QUEUE_GP:
 520                 qh = knav_queue_open_by_type(name, id, flags);
 521                 break;
 522
 523         default:
 524                 qh = knav_queue_open_by_id(name, id, flags);
 525                 break;
 526         }
 527         return qh;
 528 }
 529 EXPORT_SYMBOL_GPL(knav_queue_open);
 530
 531 /**
 532  * knav_queue_close()   - close a hardware queue handle
 533  * @qh                  - handle to close
 534  */
 535 void knav_queue_close(void *qhandle)
 536 {
 537         struct knav_queue *qh = qhandle;
 538         struct knav_queue_inst *inst = qh->inst;
 539
 540         while (atomic_read(&qh->notifier_enabled) > 0)
 541                 knav_queue_disable_notifier(qh);
 542
 543         mutex_lock(&knav_dev_lock);
 544         list_del_rcu(&qh->list);
 545         mutex_unlock(&knav_dev_lock);
 546         synchronize_rcu();
 547         if (!knav_queue_is_busy(inst)) {
 548                 struct knav_range_info *range = inst->range;
 549
 550                 if (range->ops && range->ops->close_queue)
 551                         range->ops->close_queue(range, inst);
 552         }
 553         devm_kfree(inst->kdev->dev, qh);
 554 }
 555 EXPORT_SYMBOL_GPL(knav_queue_close);
 556
 557 /**
 558  * knav_queue_device_control()  - Perform control operations on a queue
 559  * @qh                          - queue handle
 560  * @cmd                         - control commands
 561  * @arg                         - command argument
 562  *
 563  * Returns 0 on success, errno otherwise.
 564  */
 565 int knav_queue_device_control(void *qhandle, enum knav_queue_ctrl_cmd cmd,
 566                                 unsigned long arg)
 567 {
 568         struct knav_queue *qh = qhandle;
 569         struct knav_queue_notify_config *cfg;
 570         int ret;
 571
 572         switch ((int)cmd) {
 573         case KNAV_QUEUE_GET_ID:
 574                 ret = qh->inst->kdev->base_id + qh->inst->id;
 575                 break;
 576
 577         case KNAV_QUEUE_FLUSH:
 578                 ret = knav_queue_flush(qh);
 579                 break;
 580
 581         case KNAV_QUEUE_SET_NOTIFIER:
 582                 cfg = (void *)arg;
 583                 ret = knav_queue_set_notifier(qh, cfg);
 584                 break;
 585
 586         case KNAV_QUEUE_ENABLE_NOTIFY:
 587                 ret = knav_queue_enable_notifier(qh);
 588                 break;
 589
 590         case KNAV_QUEUE_DISABLE_NOTIFY:
 591                 ret = knav_queue_disable_notifier(qh);
 592                 break;
 593
 594         case KNAV_QUEUE_GET_COUNT:
 595                 ret = knav_queue_get_count(qh);
 596                 break;
 597
 598         default:
 599                 ret = -ENOTSUPP;
 600                 break;
 601         }
 602         return ret;
 603 }
 604 EXPORT_SYMBOL_GPL(knav_queue_device_control);
 605
 606
 607
 608 /**
 609  * knav_queue_push()    - push data (or descriptor) to the tail of a queue
 610  * @qh                  - hardware queue handle
 611  * @data                - data to push
 612  * @size                - size of data to push
 613  * @flags               - can be used to pass additional information
 614  *
 615  * Returns 0 on success, errno otherwise.
 616  */
 617 int knav_queue_push(void *qhandle, dma_addr_t dma,
 618                                         unsigned size, unsigned flags)
 619 {
 620         struct knav_queue *qh = qhandle;
 621         u32 val;
 622
 623         val = (u32)dma | ((size / 16) - 1);
 624         writel_relaxed(val, &qh->reg_push[0].ptr_size_thresh);
 625
 626         atomic_inc(&qh->stats.pushes);
 627         return 0;
 628 }
 629
 630 /**
 631  * knav_queue_pop()     - pop data (or descriptor) from the head of a queue
 632  * @qh                  - hardware queue handle
 633  * @size                - (optional) size of the data pop'ed.
 634  *
 635  * Returns a DMA address on success, 0 on failure.
 636  */
 637 dma_addr_t knav_queue_pop(void *qhandle, unsigned *size)
 638 {
 639         struct knav_queue *qh = qhandle;
 640         struct knav_queue_inst *inst = qh->inst;
 641         dma_addr_t dma;
 642         u32 val, idx;
 643
 644         /* are we accumulated? */
 645         if (inst->descs) {
 646                 if (unlikely(atomic_dec_return(&inst->desc_count) < 0)) {
 647                         atomic_inc(&inst->desc_count);
 648                         return 0;
 649                 }
 650                 idx  = atomic_inc_return(&inst->desc_head);
 651                 idx &= ACC_DESCS_MASK;
 652                 val = inst->descs[idx];
 653         } else {
 654                 val = readl_relaxed(&qh->reg_pop[0].ptr_size_thresh);
 655                 if (unlikely(!val))
 656                         return 0;
 657         }
 658
 659         dma = val & DESC_PTR_MASK;
 660         if (size)
 661                 *size = ((val & DESC_SIZE_MASK) + 1) * 16;
 662
 663         atomic_inc(&qh->stats.pops);
 664         return dma;
 665 }
 666
 667 /* carve out descriptors and push into queue */
 668 static void kdesc_fill_pool(struct knav_pool *pool)
 669 {
 670         struct knav_region *region;
 671         int i;
 672
 673         region = pool->region;
 674         pool->desc_size = region->desc_size;
 675         for (i = 0; i < pool->num_desc; i++) {
 676                 int index = pool->region_offset + i;
 677                 dma_addr_t dma_addr;
 678                 unsigned dma_size;
 679                 dma_addr = region->dma_start + (region->desc_size * index);
 680                 dma_size = ALIGN(pool->desc_size, SMP_CACHE_BYTES);
 681                 dma_sync_single_for_device(pool->dev, dma_addr, dma_size,
 682                                            DMA_TO_DEVICE);
 683                 knav_queue_push(pool->queue, dma_addr, dma_size, 0);
 684         }
 685 }
 686
 687 /* pop out descriptors and close the queue */
 688 static void kdesc_empty_pool(struct knav_pool *pool)
 689 {
 690         dma_addr_t dma;
 691         unsigned size;
 692         void *desc;
 693         int i;
 694
 695         if (!pool->queue)
 696                 return;
 697
 698         for (i = 0;; i++) {
 699                 dma = knav_queue_pop(pool->queue, &size);
 700                 if (!dma)
 701                         break;
 702                 desc = knav_pool_desc_dma_to_virt(pool, dma);
 703                 if (!desc) {
 704                         dev_dbg(pool->kdev->dev,
 705                                 "couldn't unmap desc, continuing\n");
 706                         continue;
 707                 }
 708         }
 709         WARN_ON(i != pool->num_desc);
 710         knav_queue_close(pool->queue);
 711 }
 712
 713
 714 /* Get the DMA address of a descriptor */
 715 dma_addr_t knav_pool_desc_virt_to_dma(void *ph, void *virt)
 716 {
 717         struct knav_pool *pool = ph;
 718         return pool->region->dma_start + (virt - pool->region->virt_start);
 719 }
 720
 721 void *knav_pool_desc_dma_to_virt(void *ph, dma_addr_t dma)
 722 {
 723         struct knav_pool *pool = ph;
 724         return pool->region->virt_start + (dma - pool->region->dma_start);
 725 }
 726
 727 /**
 728  * knav_pool_create()   - Create a pool of descriptors
 729  * @name                - name to give the pool handle
 730  * @num_desc            - numbers of descriptors in the pool
 731  * @region_id           - QMSS region id from which the descriptors are to be
 732  *                        allocated.
 733  *
 734  * Returns a pool handle on success.
 735  * Use IS_ERR_OR_NULL() to identify error values on return.
 736  */
 737 void *knav_pool_create(const char *name,
 738                                         int num_desc, int region_id)
 739 {
 740         struct knav_region *reg_itr, *region = NULL;
 741         struct knav_pool *pool, *pi;
 742         struct list_head *node;
 743         unsigned last_offset;
 744         bool slot_found;
 745         int ret;
 746
 747         if (!kdev->dev)
 748                 return ERR_PTR(-ENODEV);
 749
 750         pool = devm_kzalloc(kdev->dev, sizeof(*pool), GFP_KERNEL);
 751         if (!pool) {
 752                 dev_err(kdev->dev, "out of memory allocating pool\n");
 753                 return ERR_PTR(-ENOMEM);
 754         }
 755
 756         for_each_region(kdev, reg_itr) {
 757                 if (reg_itr->id != region_id)
 758                         continue;
 759                 region = reg_itr;
 760                 break;
 761         }
 762
 763         if (!region) {
 764                 dev_err(kdev->dev, "region-id(%d) not found\n", region_id);
 765                 ret = -EINVAL;
 766                 goto err;
 767         }
 768
 769         pool->queue = knav_queue_open(name, KNAV_QUEUE_GP, 0);
 770         if (IS_ERR_OR_NULL(pool->queue)) {
 771                 dev_err(kdev->dev,
 772                         "failed to open queue for pool(%s), error %ld\n",
 773                         name, PTR_ERR(pool->queue));
 774                 ret = PTR_ERR(pool->queue);
 775                 goto err;
 776         }
 777
 778         pool->name = kstrndup(name, KNAV_NAME_SIZE, GFP_KERNEL);
 779         pool->kdev = kdev;
 780         pool->dev = kdev->dev;
 781
 782         mutex_lock(&knav_dev_lock);
 783
 784         if (num_desc > (region->num_desc - region->used_desc)) {
 785                 dev_err(kdev->dev, "out of descs in region(%d) for pool(%s)\n",
 786                         region_id, name);
 787                 ret = -ENOMEM;
 788                 goto err_unlock;
 789         }
 790
 791         /* Region maintains a sorted (by region offset) list of pools
 792          * use the first free slot which is large enough to accomodate
 793          * the request
 794          */
 795         last_offset = 0;
 796         slot_found = false;
 797         node = &region->pools;
 798         list_for_each_entry(pi, &region->pools, region_inst) {
 799                 if ((pi->region_offset - last_offset) >= num_desc) {
 800                         slot_found = true;
 801                         break;
 802                 }
 803                 last_offset = pi->region_offset + pi->num_desc;
 804         }
 805         node = &pi->region_inst;
 806
 807         if (slot_found) {
 808                 pool->region = region;
 809                 pool->num_desc = num_desc;
 810                 pool->region_offset = last_offset;
 811                 region->used_desc += num_desc;
 812                 list_add_tail(&pool->list, &kdev->pools);
 813                 list_add_tail(&pool->region_inst, node);
 814         } else {
 815                 dev_err(kdev->dev, "pool(%s) create failed: fragmented desc pool in region(%d)\n",
 816                         name, region_id);
 817                 ret = -ENOMEM;
 818                 goto err_unlock;
 819         }
 820
 821         mutex_unlock(&knav_dev_lock);
 822         kdesc_fill_pool(pool);
 823         return pool;
 824
 825 err_unlock:
 826         mutex_unlock(&knav_dev_lock);
 827 err:
 828         kfree(pool->name);
 829         devm_kfree(kdev->dev, pool);
 830         return ERR_PTR(ret);
 831 }
 832 EXPORT_SYMBOL_GPL(knav_pool_create);
 833
 834 /**
 835  * knav_pool_destroy()  - Free a pool of descriptors
 836  * @pool                - pool handle
 837  */
 838 void knav_pool_destroy(void *ph)
 839 {
 840         struct knav_pool *pool = ph;
 841
 842         if (!pool)
 843                 return;
 844
 845         if (!pool->region)
 846                 return;
 847
 848         kdesc_empty_pool(pool);
 849         mutex_lock(&knav_dev_lock);
 850
 851         pool->region->used_desc -= pool->num_desc;
 852         list_del(&pool->region_inst);
 853         list_del(&pool->list);
 854
 855         mutex_unlock(&knav_dev_lock);
 856         kfree(pool->name);
 857         devm_kfree(kdev->dev, pool);
 858 }
 859 EXPORT_SYMBOL_GPL(knav_pool_destroy);
 860
 861
 862 /**
 863  * knav_pool_desc_get() - Get a descriptor from the pool
 864  * @pool                        - pool handle
 865  *
 866  * Returns descriptor from the pool.
 867  */
 868 void *knav_pool_desc_get(void *ph)
 869 {
 870         struct knav_pool *pool = ph;
 871         dma_addr_t dma;
 872         unsigned size;
 873         void *data;
 874
 875         dma = knav_queue_pop(pool->queue, &size);
 876         if (unlikely(!dma))
 877                 return ERR_PTR(-ENOMEM);
 878         data = knav_pool_desc_dma_to_virt(pool, dma);
 879         return data;
 880 }
 881
 882 /**
 883  * knav_pool_desc_put() - return a descriptor to the pool
 884  * @pool                        - pool handle
 885  */
 886 void knav_pool_desc_put(void *ph, void *desc)
 887 {
 888         struct knav_pool *pool = ph;
 889         dma_addr_t dma;
 890         dma = knav_pool_desc_virt_to_dma(pool, desc);
 891         knav_queue_push(pool->queue, dma, pool->region->desc_size, 0);
 892 }
 893
 894 /**
 895  * knav_pool_desc_map() - Map descriptor for DMA transfer
 896  * @pool                        - pool handle
 897  * @desc                        - address of descriptor to map
 898  * @size                        - size of descriptor to map
 899  * @dma                         - DMA address return pointer
 900  * @dma_sz                      - adjusted return pointer
 901  *
 902  * Returns 0 on success, errno otherwise.
 903  */
 904 int knav_pool_desc_map(void *ph, void *desc, unsigned size,
 905                                         dma_addr_t *dma, unsigned *dma_sz)
 906 {
 907         struct knav_pool *pool = ph;
 908         *dma = knav_pool_desc_virt_to_dma(pool, desc);
 909         size = min(size, pool->region->desc_size);
 910         size = ALIGN(size, SMP_CACHE_BYTES);
 911         *dma_sz = size;
 912         dma_sync_single_for_device(pool->dev, *dma, size, DMA_TO_DEVICE);
 913
 914         /* Ensure the descriptor reaches to the memory */
 915         __iowmb();
 916
 917         return 0;
 918 }
 919
 920 /**
 921  * knav_pool_desc_unmap()       - Unmap descriptor after DMA transfer
 922  * @pool                        - pool handle
 923  * @dma                         - DMA address of descriptor to unmap
 924  * @dma_sz                      - size of descriptor to unmap
 925  *
 926  * Returns descriptor address on success, Use IS_ERR_OR_NULL() to identify
 927  * error values on return.
 928  */
 929 void *knav_pool_desc_unmap(void *ph, dma_addr_t dma, unsigned dma_sz)
 930 {
 931         struct knav_pool *pool = ph;
 932         unsigned desc_sz;
 933         void *desc;
 934
 935         desc_sz = min(dma_sz, pool->region->desc_size);
 936         desc = knav_pool_desc_dma_to_virt(pool, dma);
 937         dma_sync_single_for_cpu(pool->dev, dma, desc_sz, DMA_FROM_DEVICE);
 938         prefetch(desc);
 939         return desc;
 940 }
 941
 942 /**
 943  * knav_pool_count()    - Get the number of descriptors in pool.
 944  * @pool                - pool handle
 945  * Returns number of elements in the pool.
 946  */
 947 int knav_pool_count(void *ph)
 948 {
 949         struct knav_pool *pool = ph;
 950         return knav_queue_get_count(pool->queue);
 951 }
 952
 953 static void knav_queue_setup_region(struct knav_device *kdev,
 954                                         struct knav_region *region)
 955 {
 956         unsigned hw_num_desc, hw_desc_size, size;
 957         struct knav_reg_region __iomem  *regs;
 958         struct knav_qmgr_info *qmgr;
 959         struct knav_pool *pool;
 960         int id = region->id;
 961         struct page *page;
 962
 963         /* unused region? */
 964         if (!region->num_desc) {
 965                 dev_warn(kdev->dev, "unused region %s\n", region->name);
 966                 return;
 967         }
 968
 969         /* get hardware descriptor value */
 970         hw_num_desc = ilog2(region->num_desc - 1) + 1;
 971
 972         /* did we force fit ourselves into nothingness? */
 973         if (region->num_desc < 32) {
 974                 region->num_desc = 0;
 975                 dev_warn(kdev->dev, "too few descriptors in region %s\n",
 976                          region->name);
 977                 return;
 978         }
 979
 980         size = region->num_desc * region->desc_size;
 981         region->virt_start = alloc_pages_exact(size, GFP_KERNEL | GFP_DMA |
 982                                                 GFP_DMA32);
 983         if (!region->virt_start) {
 984                 region->num_desc = 0;
 985                 dev_err(kdev->dev, "memory alloc failed for region %s\n",
 986                         region->name);
 987                 return;
 988         }
 989         region->virt_end = region->virt_start + size;
 990         page = virt_to_page(region->virt_start);
 991
 992         region->dma_start = dma_map_page(kdev->dev, page, 0, size,
 993                                          DMA_BIDIRECTIONAL);
 994         if (dma_mapping_error(kdev->dev, region->dma_start)) {
 995                 dev_err(kdev->dev, "dma map failed for region %s\n",
 996                         region->name);
 997                 goto fail;
 998         }
 999         region->dma_end = region->dma_start + size;
1000
1001         pool = devm_kzalloc(kdev->dev, sizeof(*pool), GFP_KERNEL);
1002         if (!pool) {
1003                 dev_err(kdev->dev, "out of memory allocating dummy pool\n");
1004                 goto fail;
1005         }
1006         pool->num_desc = 0;
1007         pool->region_offset = region->num_desc;
1008         list_add(&pool->region_inst, &region->pools);
1009
1010         dev_dbg(kdev->dev,
1011                 "region %s (%d): size:%d, link:%d@%d, phys:%08x-%08x, virt:%p-%p\n",
1012                 region->name, id, region->desc_size, region->num_desc,
1013                 region->link_index, region->dma_start, region->dma_end,
1014                 region->virt_start, region->virt_end);
1015
1016         hw_desc_size = (region->desc_size / 16) - 1;
1017         hw_num_desc -= 5;
1018
1019         for_each_qmgr(kdev, qmgr) {
1020                 regs = qmgr->reg_region + id;
1021                 writel_relaxed(region->dma_start, &regs->base);
1022                 writel_relaxed(region->link_index, &regs->start_index);
1023                 writel_relaxed(hw_desc_size << 16 | hw_num_desc,
1024                                &regs->size_count);
1025         }
1026         return;
1027
1028 fail:
1029         if (region->dma_start)
1030                 dma_unmap_page(kdev->dev, region->dma_start, size,
1031                                 DMA_BIDIRECTIONAL);
1032         if (region->virt_start)
1033                 free_pages_exact(region->virt_start, size);
1034         region->num_desc = 0;
1035         return;
1036 }
1037
1038 static const char *knav_queue_find_name(struct device_node *node)
1039 {
1040         const char *name;
1041
1042         if (of_property_read_string(node, "label", &name) < 0)
1043                 name = node->name;
1044         if (!name)
1045                 name = "unknown";
1046         return name;
1047 }
1048
1049 static int knav_queue_setup_regions(struct knav_device *kdev,
1050                                         struct device_node *regions)
1051 {
1052         struct device *dev = kdev->dev;
1053         struct knav_region *region;
1054         struct device_node *child;
1055         u32 temp[2];
1056         int ret;
1057
1058         for_each_child_of_node(regions, child) {
1059                 region = devm_kzalloc(dev, sizeof(*region), GFP_KERNEL);
1060                 if (!region) {
1061                         dev_err(dev, "out of memory allocating region\n");
1062                         return -ENOMEM;
1063                 }
1064
1065                 region->name = knav_queue_find_name(child);
1066                 of_property_read_u32(child, "id", &region->id);
1067                 ret = of_property_read_u32_array(child, "region-spec", temp, 2);
1068                 if (!ret) {
1069                         region->num_desc  = temp[0];
1070                         region->desc_size = temp[1];
1071                 } else {
1072                         dev_err(dev, "invalid region info %s\n", region->name);
1073                         devm_kfree(dev, region);
1074                         continue;
1075                 }
1076
1077                 if (!of_get_property(child, "link-index", NULL)) {
1078                         dev_err(dev, "No link info for %s\n", region->name);
1079                         devm_kfree(dev, region);
1080                         continue;
1081                 }
1082                 ret = of_property_read_u32(child, "link-index",
1083                                            &region->link_index);
1084                 if (ret) {
1085                         dev_err(dev, "link index not found for %s\n",
1086                                 region->name);
1087                         devm_kfree(dev, region);
1088                         continue;
1089                 }
1090
1091                 INIT_LIST_HEAD(&region->pools);
1092                 list_add_tail(&region->list, &kdev->regions);
1093         }
1094         if (list_empty(&kdev->regions)) {
1095                 dev_err(dev, "no valid region information found\n");
1096                 return -ENODEV;
1097         }
1098
1099         /* Next, we run through the regions and set things up */
1100         for_each_region(kdev, region)
1101                 knav_queue_setup_region(kdev, region);
1102
1103         return 0;
1104 }
1105
1106 static int knav_get_link_ram(struct knav_device *kdev,
1107                                        const char *name,
1108                                        struct knav_link_ram_block *block)
1109 {
1110         struct platform_device *pdev = to_platform_device(kdev->dev);
1111         struct device_node *node = pdev->dev.of_node;
1112         u32 temp[2];
1113
1114         /*
1115          * Note: link ram resources are specified in "entry" sized units. In
1116          * reality, although entries are ~40bits in hardware, we treat them as
1117          * 64-bit entities here.
1118          *
1119          * For example, to specify the internal link ram for Keystone-I class
1120          * devices, we would set the linkram0 resource to 0x80000-0x83fff.
1121          *
1122          * This gets a bit weird when other link rams are used.  For example,
1123          * if the range specified is 0x0c000000-0x0c003fff (i.e., 16K entries
1124          * in MSMC SRAM), the actual memory used is 0x0c000000-0x0c020000,
1125          * which accounts for 64-bits per entry, for 16K entries.
1126          */
1127         if (!of_property_read_u32_array(node, name , temp, 2)) {
1128                 if (temp[0]) {
1129                         /*
1130                          * queue_base specified => using internal or onchip
1131                          * link ram WARNING - we do not "reserve" this block
1132                          */
1133                         block->phys = (dma_addr_t)temp[0];
1134                         block->virt = NULL;
1135                         block->size = temp[1];
1136                 } else {
1137                         block->size = temp[1];
1138                         /* queue_base not specific => allocate requested size */
1139                         block->virt = dmam_alloc_coherent(kdev->dev,
1140                                                   8 * block->size, &block->phys,
1141                                                   GFP_KERNEL);
1142                         if (!block->virt) {
1143                                 dev_err(kdev->dev, "failed to alloc linkram\n");
1144                                 return -ENOMEM;
1145                         }
1146                 }
1147         } else {
1148                 return -ENODEV;
1149         }
1150         return 0;
1151 }
1152
1153 static int knav_queue_setup_link_ram(struct knav_device *kdev)
1154 {
1155         struct knav_link_ram_block *block;
1156         struct knav_qmgr_info *qmgr;
1157
1158         for_each_qmgr(kdev, qmgr) {
1159                 block = &kdev->link_rams[0];
1160                 dev_dbg(kdev->dev, "linkram0: phys:%x, virt:%p, size:%x\n",
1161                         block->phys, block->virt, block->size);
1162                 writel_relaxed(block->phys, &qmgr->reg_config->link_ram_base0);
1163                 writel_relaxed(block->size, &qmgr->reg_config->link_ram_size0);
1164
1165                 block++;
1166                 if (!block->size)
1167                         return 0;
1168
1169                 dev_dbg(kdev->dev, "linkram1: phys:%x, virt:%p, size:%x\n",
1170                         block->phys, block->virt, block->size);
1171                 writel_relaxed(block->phys, &qmgr->reg_config->link_ram_base1);
1172         }
1173
1174         return 0;
1175 }
1176
1177 static int knav_setup_queue_range(struct knav_device *kdev,
1178                                         struct device_node *node)
1179 {
1180         struct device *dev = kdev->dev;
1181         struct knav_range_info *range;
1182         struct knav_qmgr_info *qmgr;
1183         u32 temp[2], start, end, id, index;
1184         int ret, i;
1185
1186         range = devm_kzalloc(dev, sizeof(*range), GFP_KERNEL);
1187         if (!range) {
1188                 dev_err(dev, "out of memory allocating range\n");
1189                 return -ENOMEM;
1190         }
1191
1192         range->kdev = kdev;
1193         range->name = knav_queue_find_name(node);
1194         ret = of_property_read_u32_array(node, "qrange", temp, 2);
1195         if (!ret) {
1196                 range->queue_base = temp[0] - kdev->base_id;
1197                 range->num_queues = temp[1];
1198         } else {
1199                 dev_err(dev, "invalid queue range %s\n", range->name);
1200                 devm_kfree(dev, range);
1201                 return -EINVAL;
1202         }
1203
1204         for (i = 0; i < RANGE_MAX_IRQS; i++) {
1205                 struct of_phandle_args oirq;
1206
1207                 if (of_irq_parse_one(node, i, &oirq))
1208                         break;
1209
1210                 range->irqs[i].irq = irq_create_of_mapping(&oirq);
1211                 if (range->irqs[i].irq == IRQ_NONE)
1212                         break;
1213
1214                 range->num_irqs++;
1215
1216                 if (oirq.args_count == 3)
1217                         range->irqs[i].cpu_map =
1218                                 (oirq.args[2] & 0x0000ff00) >> 8;
1219         }
1220
1221         range->num_irqs = min(range->num_irqs, range->num_queues);
1222         if (range->num_irqs)
1223                 range->flags |= RANGE_HAS_IRQ;
1224
1225         if (of_get_property(node, "qalloc-by-id", NULL))
1226                 range->flags |= RANGE_RESERVED;
1227
1228         if (of_get_property(node, "accumulator", NULL)) {
1229                 ret = knav_init_acc_range(kdev, node, range);
1230                 if (ret < 0) {
1231                         devm_kfree(dev, range);
1232                         return ret;
1233                 }
1234         } else {
1235                 range->ops = &knav_gp_range_ops;
1236         }
1237
1238         /* set threshold to 1, and flush out the queues */
1239         for_each_qmgr(kdev, qmgr) {
1240                 start = max(qmgr->start_queue, range->queue_base);
1241                 end   = min(qmgr->start_queue + qmgr->num_queues,
1242                             range->queue_base + range->num_queues);
1243                 for (id = start; id < end; id++) {
1244                         index = id - qmgr->start_queue;
1245                         writel_relaxed(THRESH_GTE | 1,
1246                                        &qmgr->reg_peek[index].ptr_size_thresh);
1247                         writel_relaxed(0,
1248                                        &qmgr->reg_push[index].ptr_size_thresh);
1249                 }
1250         }
1251
1252         list_add_tail(&range->list, &kdev->queue_ranges);
1253         dev_dbg(dev, "added range %s: %d-%d, %d irqs%s%s%s\n",
1254                 range->name, range->queue_base,
1255                 range->queue_base + range->num_queues - 1,
1256                 range->num_irqs,
1257                 (range->flags & RANGE_HAS_IRQ) ? ", has irq" : "",
1258                 (range->flags & RANGE_RESERVED) ? ", reserved" : "",
1259                 (range->flags & RANGE_HAS_ACCUMULATOR) ? ", acc" : "");
1260         kdev->num_queues_in_use += range->num_queues;
1261         return 0;
1262 }
1263
1264 static int knav_setup_queue_pools(struct knav_device *kdev,
1265                                    struct device_node *queue_pools)
1266 {
1267         struct device_node *type, *range;
1268         int ret;
1269
1270         for_each_child_of_node(queue_pools, type) {
1271                 for_each_child_of_node(type, range) {
1272                         ret = knav_setup_queue_range(kdev, range);
1273                         /* return value ignored, we init the rest... */
1274                 }
1275         }
1276
1277         /* ... and barf if they all failed! */
1278         if (list_empty(&kdev->queue_ranges)) {
1279                 dev_err(kdev->dev, "no valid queue range found\n");
1280                 return -ENODEV;
1281         }
1282         return 0;
1283 }
1284
1285 static void knav_free_queue_range(struct knav_device *kdev,
1286                                   struct knav_range_info *range)
1287 {
1288         if (range->ops && range->ops->free_range)
1289                 range->ops->free_range(range);
1290         list_del(&range->list);
1291         devm_kfree(kdev->dev, range);
1292 }
1293
1294 static void knav_free_queue_ranges(struct knav_device *kdev)
1295 {
1296         struct knav_range_info *range;
1297
1298         for (;;) {
1299                 range = first_queue_range(kdev);
1300                 if (!range)
1301                         break;
1302                 knav_free_queue_range(kdev, range);
1303         }
1304 }
1305
1306 static void knav_queue_free_regions(struct knav_device *kdev)
1307 {
1308         struct knav_region *region;
1309         struct knav_pool *pool, *tmp;
1310         unsigned size;
1311
1312         for (;;) {
1313                 region = first_region(kdev);
1314                 if (!region)
1315                         break;
1316                 list_for_each_entry_safe(pool, tmp, &region->pools, region_inst)
1317                         knav_pool_destroy(pool);
1318
1319                 size = region->virt_end - region->virt_start;
1320                 if (size)
1321                         free_pages_exact(region->virt_start, size);
1322                 list_del(&region->list);
1323                 devm_kfree(kdev->dev, region);
1324         }
1325 }
1326
1327 static void __iomem *knav_queue_map_reg(struct knav_device *kdev,
1328                                         struct device_node *node, int index)
1329 {
1330         struct resource res;
1331         void __iomem *regs;
1332         int ret;
1333
1334         ret = of_address_to_resource(node, index, &res);
1335         if (ret) {
1336                 dev_err(kdev->dev, "Can't translate of node(%s) address for index(%d)\n",
1337                         node->name, index);
1338                 return ERR_PTR(ret);
1339         }
1340
1341         regs = devm_ioremap_resource(kdev->dev, &res);
1342         if (IS_ERR(regs))
1343                 dev_err(kdev->dev, "Failed to map register base for index(%d) node(%s)\n",
1344                         index, node->name);
1345         return regs;
1346 }
1347
1348 static int knav_queue_init_qmgrs(struct knav_device *kdev,
1349                                         struct device_node *qmgrs)
1350 {
1351         struct device *dev = kdev->dev;
1352         struct knav_qmgr_info *qmgr;
1353         struct device_node *child;
1354         u32 temp[2];
1355         int ret;
1356
1357         for_each_child_of_node(qmgrs, child) {
1358                 qmgr = devm_kzalloc(dev, sizeof(*qmgr), GFP_KERNEL);
1359                 if (!qmgr) {
1360                         dev_err(dev, "out of memory allocating qmgr\n");
1361                         return -ENOMEM;
1362                 }
1363
1364                 ret = of_property_read_u32_array(child, "managed-queues",
1365                                                  temp, 2);
1366                 if (!ret) {
1367                         qmgr->start_queue = temp[0];
1368                         qmgr->num_queues = temp[1];
1369                 } else {
1370                         dev_err(dev, "invalid qmgr queue range\n");
1371                         devm_kfree(dev, qmgr);
1372                         continue;
1373                 }
1374
1375                 dev_info(dev, "qmgr start queue %d, number of queues %d\n",
1376                          qmgr->start_queue, qmgr->num_queues);
1377
1378                 qmgr->reg_peek =
1379                         knav_queue_map_reg(kdev, child,
1380                                            KNAV_QUEUE_PEEK_REG_INDEX);
1381                 qmgr->reg_status =
1382                         knav_queue_map_reg(kdev, child,
1383                                            KNAV_QUEUE_STATUS_REG_INDEX);
1384                 qmgr->reg_config =
1385                         knav_queue_map_reg(kdev, child,
1386                                            KNAV_QUEUE_CONFIG_REG_INDEX);
1387                 qmgr->reg_region =
1388                         knav_queue_map_reg(kdev, child,
1389                                            KNAV_QUEUE_REGION_REG_INDEX);
1390                 qmgr->reg_push =
1391                         knav_queue_map_reg(kdev, child,
1392                                            KNAV_QUEUE_PUSH_REG_INDEX);
1393                 qmgr->reg_pop =
1394                         knav_queue_map_reg(kdev, child,
1395                                            KNAV_QUEUE_POP_REG_INDEX);
1396
1397                 if (IS_ERR(qmgr->reg_peek) || IS_ERR(qmgr->reg_status) ||
1398                     IS_ERR(qmgr->reg_config) || IS_ERR(qmgr->reg_region) ||
1399                     IS_ERR(qmgr->reg_push) || IS_ERR(qmgr->reg_pop)) {
1400                         dev_err(dev, "failed to map qmgr regs\n");
1401                         if (!IS_ERR(qmgr->reg_peek))
1402                                 devm_iounmap(dev, qmgr->reg_peek);
1403                         if (!IS_ERR(qmgr->reg_status))
1404                                 devm_iounmap(dev, qmgr->reg_status);
1405                         if (!IS_ERR(qmgr->reg_config))
1406                                 devm_iounmap(dev, qmgr->reg_config);
1407                         if (!IS_ERR(qmgr->reg_region))
1408                                 devm_iounmap(dev, qmgr->reg_region);
1409                         if (!IS_ERR(qmgr->reg_push))
1410                                 devm_iounmap(dev, qmgr->reg_push);
1411                         if (!IS_ERR(qmgr->reg_pop))
1412                                 devm_iounmap(dev, qmgr->reg_pop);
1413                         devm_kfree(dev, qmgr);
1414                         continue;
1415                 }
1416
1417                 list_add_tail(&qmgr->list, &kdev->qmgrs);
1418                 dev_info(dev, "added qmgr start queue %d, num of queues %d, reg_peek %p, reg_status %p, reg_config %p, reg_region %p, reg_push %p, reg_pop %p\n",
1419                          qmgr->start_queue, qmgr->num_queues,
1420                          qmgr->reg_peek, qmgr->reg_status,
1421                          qmgr->reg_config, qmgr->reg_region,
1422                          qmgr->reg_push, qmgr->reg_pop);
1423         }
1424         return 0;
1425 }
1426
1427 static int knav_queue_init_pdsps(struct knav_device *kdev,
1428                                         struct device_node *pdsps)
1429 {
1430         struct device *dev = kdev->dev;
1431         struct knav_pdsp_info *pdsp;
1432         struct device_node *child;
1433         int ret;
1434
1435         for_each_child_of_node(pdsps, child) {
1436                 pdsp = devm_kzalloc(dev, sizeof(*pdsp), GFP_KERNEL);
1437                 if (!pdsp) {
1438                         dev_err(dev, "out of memory allocating pdsp\n");
1439                         return -ENOMEM;
1440                 }
1441                 pdsp->name = knav_queue_find_name(child);
1442                 ret = of_property_read_string(child, "firmware",
1443                                               &pdsp->firmware);
1444                 if (ret < 0 || !pdsp->firmware) {
1445                         dev_err(dev, "unknown firmware for pdsp %s\n",
1446                                 pdsp->name);
1447                         devm_kfree(dev, pdsp);
1448                         continue;
1449                 }
1450                 dev_dbg(dev, "pdsp name %s fw name :%s\n", pdsp->name,
1451                         pdsp->firmware);
1452
1453                 pdsp->iram =
1454                         knav_queue_map_reg(kdev, child,
1455                                            KNAV_QUEUE_PDSP_IRAM_REG_INDEX);
1456                 pdsp->regs =
1457                         knav_queue_map_reg(kdev, child,
1458                                            KNAV_QUEUE_PDSP_REGS_REG_INDEX);
1459                 pdsp->intd =
1460                         knav_queue_map_reg(kdev, child,
1461                                            KNAV_QUEUE_PDSP_INTD_REG_INDEX);
1462                 pdsp->command =
1463                         knav_queue_map_reg(kdev, child,
1464                                            KNAV_QUEUE_PDSP_CMD_REG_INDEX);
1465
1466                 if (IS_ERR(pdsp->command) || IS_ERR(pdsp->iram) ||
1467                     IS_ERR(pdsp->regs) || IS_ERR(pdsp->intd)) {
1468                         dev_err(dev, "failed to map pdsp %s regs\n",
1469                                 pdsp->name);
1470                         if (!IS_ERR(pdsp->command))
1471                                 devm_iounmap(dev, pdsp->command);
1472                         if (!IS_ERR(pdsp->iram))
1473                                 devm_iounmap(dev, pdsp->iram);
1474                         if (!IS_ERR(pdsp->regs))
1475                                 devm_iounmap(dev, pdsp->regs);
1476                         if (!IS_ERR(pdsp->intd))
1477                                 devm_iounmap(dev, pdsp->intd);
1478                         devm_kfree(dev, pdsp);
1479                         continue;
1480                 }
1481                 of_property_read_u32(child, "id", &pdsp->id);
1482                 list_add_tail(&pdsp->list, &kdev->pdsps);
1483                 dev_dbg(dev, "added pdsp %s: command %p, iram %p, regs %p, intd %p, firmware %s\n",
1484                         pdsp->name, pdsp->command, pdsp->iram, pdsp->regs,
1485                         pdsp->intd, pdsp->firmware);
1486         }
1487         return 0;
1488 }
1489
1490 static int knav_queue_stop_pdsp(struct knav_device *kdev,
1491                           struct knav_pdsp_info *pdsp)
1492 {
1493         u32 val, timeout = 1000;
1494         int ret;
1495
1496         val = readl_relaxed(&pdsp->regs->control) & ~PDSP_CTRL_ENABLE;
1497         writel_relaxed(val, &pdsp->regs->control);
1498         ret = knav_queue_pdsp_wait(&pdsp->regs->control, timeout,
1499                                         PDSP_CTRL_RUNNING);
1500         if (ret < 0) {
1501                 dev_err(kdev->dev, "timed out on pdsp %s stop\n", pdsp->name);
1502                 return ret;
1503         }
1504         return 0;
1505 }
1506
1507 static int knav_queue_load_pdsp(struct knav_device *kdev,
1508                           struct knav_pdsp_info *pdsp)
1509 {
1510         int i, ret, fwlen;
1511         const struct firmware *fw;
1512         u32 *fwdata;
1513
1514         ret = request_firmware(&fw, pdsp->firmware, kdev->dev);
1515         if (ret) {
1516                 dev_err(kdev->dev, "failed to get firmware %s for pdsp %s\n",
1517                         pdsp->firmware, pdsp->name);
1518                 return ret;
1519         }
1520         writel_relaxed(pdsp->id + 1, pdsp->command + 0x18);
1521         /* download the firmware */
1522         fwdata = (u32 *)fw->data;
1523         fwlen = (fw->size + sizeof(u32) - 1) / sizeof(u32);
1524         for (i = 0; i < fwlen; i++)
1525                 writel_relaxed(be32_to_cpu(fwdata[i]), pdsp->iram + i);
1526
1527         release_firmware(fw);
1528         return 0;
1529 }
1530
1531 static int knav_queue_start_pdsp(struct knav_device *kdev,
1532                            struct knav_pdsp_info *pdsp)
1533 {
1534         u32 val, timeout = 1000;
1535         int ret;
1536
1537         /* write a command for sync */
1538         writel_relaxed(0xffffffff, pdsp->command);
1539         while (readl_relaxed(pdsp->command) != 0xffffffff)
1540                 cpu_relax();
1541
1542         /* soft reset the PDSP */
1543         val  = readl_relaxed(&pdsp->regs->control);
1544         val &= ~(PDSP_CTRL_PC_MASK | PDSP_CTRL_SOFT_RESET);
1545         writel_relaxed(val, &pdsp->regs->control);
1546
1547         /* enable pdsp */
1548         val = readl_relaxed(&pdsp->regs->control) | PDSP_CTRL_ENABLE;
1549         writel_relaxed(val, &pdsp->regs->control);
1550
1551         /* wait for command register to clear */
1552         ret = knav_queue_pdsp_wait(pdsp->command, timeout, 0);
1553         if (ret < 0) {
1554                 dev_err(kdev->dev,
1555                         "timed out on pdsp %s command register wait\n",
1556                         pdsp->name);
1557                 return ret;
1558         }
1559         return 0;
1560 }
1561
1562 static void knav_queue_stop_pdsps(struct knav_device *kdev)
1563 {
1564         struct knav_pdsp_info *pdsp;
1565
1566         /* disable all pdsps */
1567         for_each_pdsp(kdev, pdsp)
1568                 knav_queue_stop_pdsp(kdev, pdsp);
1569 }
1570
1571 static int knav_queue_start_pdsps(struct knav_device *kdev)
1572 {
1573         struct knav_pdsp_info *pdsp;
1574         int ret;
1575
1576         knav_queue_stop_pdsps(kdev);
1577         /* now load them all */
1578         for_each_pdsp(kdev, pdsp) {
1579                 ret = knav_queue_load_pdsp(kdev, pdsp);
1580                 if (ret < 0)
1581                         return ret;
1582         }
1583
1584         for_each_pdsp(kdev, pdsp) {
1585                 ret = knav_queue_start_pdsp(kdev, pdsp);
1586                 WARN_ON(ret);
1587         }
1588         return 0;
1589 }
1590
1591 static inline struct knav_qmgr_info *knav_find_qmgr(unsigned id)
1592 {
1593         struct knav_qmgr_info *qmgr;
1594
1595         for_each_qmgr(kdev, qmgr) {
1596                 if ((id >= qmgr->start_queue) &&
1597                     (id < qmgr->start_queue + qmgr->num_queues))
1598                         return qmgr;
1599         }
1600         return NULL;
1601 }
1602
1603 static int knav_queue_init_queue(struct knav_device *kdev,
1604                                         struct knav_range_info *range,
1605                                         struct knav_queue_inst *inst,
1606                                         unsigned id)
1607 {
1608         char irq_name[KNAV_NAME_SIZE];
1609         inst->qmgr = knav_find_qmgr(id);
1610         if (!inst->qmgr)
1611                 return -1;
1612
1613         INIT_LIST_HEAD(&inst->handles);
1614         inst->kdev = kdev;
1615         inst->range = range;
1616         inst->irq_num = -1;
1617         inst->id = id;
1618         scnprintf(irq_name, sizeof(irq_name), "hwqueue-%d", id);
1619         inst->irq_name = kstrndup(irq_name, sizeof(irq_name), GFP_KERNEL);
1620
1621         if (range->ops && range->ops->init_queue)
1622                 return range->ops->init_queue(range, inst);
1623         else
1624                 return 0;
1625 }
1626
1627 static int knav_queue_init_queues(struct knav_device *kdev)
1628 {
1629         struct knav_range_info *range;
1630         int size, id, base_idx;
1631         int idx = 0, ret = 0;
1632
1633         /* how much do we need for instance data? */
1634         size = sizeof(struct knav_queue_inst);
1635
1636         /* round this up to a power of 2, keep the index to instance
1637          * arithmetic fast.
1638          * */
1639         kdev->inst_shift = order_base_2(size);
1640         size = (1 << kdev->inst_shift) * kdev->num_queues_in_use;
1641         kdev->instances = devm_kzalloc(kdev->dev, size, GFP_KERNEL);
1642         if (!kdev->instances)
1643                 return -ENOMEM;
1644
1645         for_each_queue_range(kdev, range) {
1646                 if (range->ops && range->ops->init_range)
1647                         range->ops->init_range(range);
1648                 base_idx = idx;
1649                 for (id = range->queue_base;
1650                      id < range->queue_base + range->num_queues; id++, idx++) {
1651                         ret = knav_queue_init_queue(kdev, range,
1652                                         knav_queue_idx_to_inst(kdev, idx), id);
1653                         if (ret < 0)
1654                                 return ret;
1655                 }
1656                 range->queue_base_inst =
1657                         knav_queue_idx_to_inst(kdev, base_idx);
1658         }
1659         return 0;
1660 }
1661
1662 static int knav_queue_probe(struct platform_device *pdev)
1663 {
1664         struct device_node *node = pdev->dev.of_node;
1665         struct device_node *qmgrs, *queue_pools, *regions, *pdsps;
1666         struct device *dev = &pdev->dev;
1667         u32 temp[2];
1668         int ret;
1669
1670         if (!node) {
1671                 dev_err(dev, "device tree info unavailable\n");
1672                 return -ENODEV;
1673         }
1674
1675         kdev = devm_kzalloc(dev, sizeof(struct knav_device), GFP_KERNEL);
1676         if (!kdev) {
1677                 dev_err(dev, "memory allocation failed\n");
1678                 return -ENOMEM;
1679         }
1680
1681         platform_set_drvdata(pdev, kdev);
1682         kdev->dev = dev;
1683         INIT_LIST_HEAD(&kdev->queue_ranges);
1684         INIT_LIST_HEAD(&kdev->qmgrs);
1685         INIT_LIST_HEAD(&kdev->pools);
1686         INIT_LIST_HEAD(&kdev->regions);
1687         INIT_LIST_HEAD(&kdev->pdsps);
1688
1689         pm_runtime_enable(&pdev->dev);
1690         ret = pm_runtime_get_sync(&pdev->dev);
1691         if (ret < 0) {
1692                 dev_err(dev, "Failed to enable QMSS\n");
1693                 return ret;
1694         }
1695
1696         if (of_property_read_u32_array(node, "queue-range", temp, 2)) {
1697                 dev_err(dev, "queue-range not specified\n");
1698                 ret = -ENODEV;
1699                 goto err;
1700         }
1701         kdev->base_id    = temp[0];
1702         kdev->num_queues = temp[1];
1703
1704         /* Initialize queue managers using device tree configuration */
1705         qmgrs =  of_get_child_by_name(node, "qmgrs");
1706         if (!qmgrs) {
1707                 dev_err(dev, "queue manager info not specified\n");
1708                 ret = -ENODEV;
1709                 goto err;
1710         }
1711         ret = knav_queue_init_qmgrs(kdev, qmgrs);
1712         of_node_put(qmgrs);
1713         if (ret)
1714                 goto err;
1715
1716         /* get pdsp configuration values from device tree */
1717         pdsps =  of_get_child_by_name(node, "pdsps");
1718         if (pdsps) {
1719                 ret = knav_queue_init_pdsps(kdev, pdsps);
1720                 if (ret)
1721                         goto err;
1722
1723                 ret = knav_queue_start_pdsps(kdev);
1724                 if (ret)
1725                         goto err;
1726         }
1727         of_node_put(pdsps);
1728
1729         /* get usable queue range values from device tree */
1730         queue_pools = of_get_child_by_name(node, "queue-pools");
1731         if (!queue_pools) {
1732                 dev_err(dev, "queue-pools not specified\n");
1733                 ret = -ENODEV;
1734                 goto err;
1735         }
1736         ret = knav_setup_queue_pools(kdev, queue_pools);
1737         of_node_put(queue_pools);
1738         if (ret)
1739                 goto err;
1740
1741         ret = knav_get_link_ram(kdev, "linkram0", &kdev->link_rams[0]);
1742         if (ret) {
1743                 dev_err(kdev->dev, "could not setup linking ram\n");
1744                 goto err;
1745         }
1746
1747         ret = knav_get_link_ram(kdev, "linkram1", &kdev->link_rams[1]);
1748         if (ret) {
1749                 /*
1750                  * nothing really, we have one linking ram already, so we just
1751                  * live within our means
1752                  */
1753         }
1754
1755         ret = knav_queue_setup_link_ram(kdev);
1756         if (ret)
1757                 goto err;
1758
1759         regions =  of_get_child_by_name(node, "descriptor-regions");
1760         if (!regions) {
1761                 dev_err(dev, "descriptor-regions not specified\n");
1762                 goto err;
1763         }
1764         ret = knav_queue_setup_regions(kdev, regions);
1765         of_node_put(regions);
1766         if (ret)
1767                 goto err;
1768
1769         ret = knav_queue_init_queues(kdev);
1770         if (ret < 0) {
1771                 dev_err(dev, "hwqueue initialization failed\n");
1772                 goto err;
1773         }
1774
1775         debugfs_create_file("qmss", S_IFREG | S_IRUGO, NULL, NULL,
1776                             &knav_queue_debug_ops);
1777         return 0;
1778
1779 err:
1780         knav_queue_stop_pdsps(kdev);
1781         knav_queue_free_regions(kdev);
1782         knav_free_queue_ranges(kdev);
1783         pm_runtime_put_sync(&pdev->dev);
1784         pm_runtime_disable(&pdev->dev);
1785         return ret;
1786 }
1787
1788 static int knav_queue_remove(struct platform_device *pdev)
1789 {
1790         /* TODO: Free resources */
1791         pm_runtime_put_sync(&pdev->dev);
1792         pm_runtime_disable(&pdev->dev);
1793         return 0;
1794 }
1795
1796 /* Match table for of_platform binding */
1797 static struct of_device_id keystone_qmss_of_match[] = {
1798         { .compatible = "ti,keystone-navigator-qmss", },
1799         {},
1800 };
1801 MODULE_DEVICE_TABLE(of, keystone_qmss_of_match);
1802
1803 static struct platform_driver keystone_qmss_driver = {
1804         .probe          = knav_queue_probe,
1805         .remove         = knav_queue_remove,
1806         .driver         = {
1807                 .name   = "keystone-navigator-qmss",
1808                 .owner  = THIS_MODULE,
1809                 .of_match_table = keystone_qmss_of_match,
1810         },
1811 };
1812 module_platform_driver(keystone_qmss_driver);
1813
1814 MODULE_LICENSE("GPL v2");
1815 MODULE_DESCRIPTION("TI QMSS driver for Keystone SOCs");
1816 MODULE_AUTHOR("Sandeep Nair <sandeep_n@ti.com>");
1817 MODULE_AUTHOR("Santosh Shilimkar <santosh.shilimkar@ti.com>");