projects / GitHub / LineageOS / android_kernel_motorola_exynos9610.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
soc: ti: knav_qmss_queue: Return proper error if devm_kzalloc fails
[GitHub/LineageOS/android_kernel_motorola_exynos9610.git] / drivers / soc / ti / knav_qmss_queue.c
CommitLineData
41f93af9
SN		 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
43static struct knav_device *kdev;
44static 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 */
76void 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}
94EXPORT_SYMBOL_GPL(knav_queue_notify);
95
96static 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
104static 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
131static 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
144static inline bool knav_queue_is_busy(struct knav_queue_inst *inst)
145{
146 return !list_empty(&inst->handles);
147}
148
149static inline bool knav_queue_is_reserved(struct knav_queue_inst *inst)
150{
151 return inst->range->flags & RANGE_RESERVED;
152}
153
154static 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
169static 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
186static inline struct knav_queue_inst *
187knav_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
199static 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
209static 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
244static struct knav_queue *
245knav_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
268unlock_ret:
269 mutex_unlock(&knav_dev_lock);
270
271 return qh;
272}
273
274static 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
294unlock_ret:
295 mutex_unlock(&knav_dev_lock);
296 return qh;
297}
298
299static 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
307static 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
328static 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
344static 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
367static 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
383static 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
389static 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
396struct 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
403static 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
412static 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
440static 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
456static int knav_queue_debug_open(struct inode *inode, struct file *file)
457{
458 return single_open(file, knav_queue_debug_show, NULL);
459}
460
461static 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
468static 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
487static 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 */
511void *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}
529EXPORT_SYMBOL_GPL(knav_queue_open);
530
531/**
532 * knav_queue_close() - close a hardware queue handle
533 * @qh - handle to close
534 */
535void 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}
555EXPORT_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 */
565int 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}
604EXPORT_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 */
617int 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 */
637dma_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 */
668static 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 */
688static 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 */
715dma_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
721void *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 */
737void *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;
ea6d4c07 788 goto err_unlock;
41f93af9
SN		 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;
ea6d4c07 818 goto err_unlock;
41f93af9
SN		 819 }
820
821 mutex_unlock(&knav_dev_lock);
822 kdesc_fill_pool(pool);
823 return pool;
824
ea6d4c07 825err_unlock:
41f93af9 826 mutex_unlock(&knav_dev_lock);
ea6d4c07 827err:
41f93af9
SN		 828 kfree(pool->name);
829 devm_kfree(kdev->dev, pool);
830 return ERR_PTR(ret);
831}
832EXPORT_SYMBOL_GPL(knav_pool_create);
833
834/**
835 * knav_pool_destroy() - Free a pool of descriptors
836 * @pool - pool handle
837 */
838void 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}
859EXPORT_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 */
868void *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 */
886void 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 */
904int 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 */
929void *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 */
947int knav_pool_count(void *ph)
948{
949 struct knav_pool *pool = ph;
950 return knav_queue_get_count(pool->queue);
951}
952
953static 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
1028fail:
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
1038static 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
1049static 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
1106static 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
1153static 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
1177static 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
1264static 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
1285static 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
1294static 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
1306static void knav_queue_free_regions(struct knav_device *kdev)
1307{
1308 struct knav_region *region;
1309 struct knav_pool *pool;
1310 unsigned size;
1311
1312 for (;;) {
1313 region = first_region(kdev);
1314 if (!region)
1315 break;
1316 list_for_each_entry(pool, &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
1327static 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
1348static 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
1427static 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
1490static 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
1507static 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
1531static 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
1562static 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
1571static 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
1591static 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
1603static 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
1627static 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)
39179cb5 1643 return -ENOMEM;
41f93af9
SN		 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
1662static 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
1779err:
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
1788static 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 */
1797static struct of_device_id keystone_qmss_of_match[] = {
1798 { .compatible = "ti,keystone-navigator-qmss", },
1799 {},
1800};
1801MODULE_DEVICE_TABLE(of, keystone_qmss_of_match);
1802
1803static 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};
1812module_platform_driver(keystone_qmss_driver);
1813
1814MODULE_LICENSE("GPL v2");
1815MODULE_DESCRIPTION("TI QMSS driver for Keystone SOCs");
1816MODULE_AUTHOR("Sandeep Nair <sandeep_n@ti.com>");
1817MODULE_AUTHOR("Santosh Shilimkar <santosh.shilimkar@ti.com>");