trusty-irq.c
15.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
// SPDX-License-Identifier: GPL-2.0-only
/*
* Copyright (C) 2013 Google, Inc.
*/
#include <linux/cpu.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/irqdomain.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_irq.h>
#include <linux/platform_device.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/trusty/smcall.h>
#include <linux/trusty/sm_err.h>
#include <linux/trusty/trusty.h>
struct trusty_irq {
struct trusty_irq_state *is;
struct hlist_node node;
unsigned int irq;
bool percpu;
bool enable;
bool doorbell;
struct trusty_irq __percpu *percpu_ptr;
};
struct trusty_irq_irqset {
struct hlist_head pending;
struct hlist_head inactive;
};
struct trusty_irq_state {
struct device *dev;
struct device *trusty_dev;
struct trusty_irq_irqset normal_irqs;
spinlock_t normal_irqs_lock;
struct trusty_irq_irqset __percpu *percpu_irqs;
struct notifier_block trusty_call_notifier;
struct hlist_node cpuhp_node;
bool gicv3_workaround;
};
static int trusty_irq_cpuhp_slot = -1;
static void trusty_irq_enable_pending_irqs(struct trusty_irq_state *is,
struct trusty_irq_irqset *irqset,
bool percpu)
{
struct hlist_node *n;
struct trusty_irq *trusty_irq;
hlist_for_each_entry_safe(trusty_irq, n, &irqset->pending, node) {
dev_dbg(is->dev,
"%s: enable pending irq %d, percpu %d, cpu %d\n",
__func__, trusty_irq->irq, percpu, smp_processor_id());
if (percpu)
enable_percpu_irq(trusty_irq->irq, 0);
else
enable_irq(trusty_irq->irq);
hlist_del(&trusty_irq->node);
hlist_add_head(&trusty_irq->node, &irqset->inactive);
}
}
static void trusty_irq_enable_irqset(struct trusty_irq_state *is,
struct trusty_irq_irqset *irqset)
{
struct trusty_irq *trusty_irq;
hlist_for_each_entry(trusty_irq, &irqset->inactive, node) {
if (trusty_irq->enable) {
dev_warn(is->dev,
"%s: percpu irq %d already enabled, cpu %d\n",
__func__, trusty_irq->irq, smp_processor_id());
continue;
}
dev_dbg(is->dev, "%s: enable percpu irq %d, cpu %d\n",
__func__, trusty_irq->irq, smp_processor_id());
enable_percpu_irq(trusty_irq->irq, 0);
trusty_irq->enable = true;
}
}
static void trusty_irq_disable_irqset(struct trusty_irq_state *is,
struct trusty_irq_irqset *irqset)
{
struct hlist_node *n;
struct trusty_irq *trusty_irq;
hlist_for_each_entry(trusty_irq, &irqset->inactive, node) {
if (!trusty_irq->enable) {
dev_warn(is->dev,
"irq %d already disabled, percpu %d, cpu %d\n",
trusty_irq->irq, trusty_irq->percpu,
smp_processor_id());
continue;
}
dev_dbg(is->dev, "%s: disable irq %d, percpu %d, cpu %d\n",
__func__, trusty_irq->irq, trusty_irq->percpu,
smp_processor_id());
trusty_irq->enable = false;
if (trusty_irq->percpu)
disable_percpu_irq(trusty_irq->irq);
else
disable_irq_nosync(trusty_irq->irq);
}
hlist_for_each_entry_safe(trusty_irq, n, &irqset->pending, node) {
if (!trusty_irq->enable) {
dev_warn(is->dev,
"pending irq %d already disabled, percpu %d, cpu %d\n",
trusty_irq->irq, trusty_irq->percpu,
smp_processor_id());
}
dev_dbg(is->dev,
"%s: disable pending irq %d, percpu %d, cpu %d\n",
__func__, trusty_irq->irq, trusty_irq->percpu,
smp_processor_id());
trusty_irq->enable = false;
hlist_del(&trusty_irq->node);
hlist_add_head(&trusty_irq->node, &irqset->inactive);
}
}
static int trusty_irq_call_notify(struct notifier_block *nb,
unsigned long action, void *data)
{
struct trusty_irq_state *is;
is = container_of(nb, struct trusty_irq_state, trusty_call_notifier);
if (!(is->gicv3_workaround) && WARN_ON(!irqs_disabled()))
return NOTIFY_DONE;
if (action != TRUSTY_CALL_PREPARE)
return NOTIFY_DONE;
/*
* If use GICv3, we trigger the nop to Trusty OS
* directly instead of yield another pending
* IRQ.
*/
if (is->gicv3_workaround) {
return NOTIFY_OK;
}
spin_lock(&is->normal_irqs_lock);
trusty_irq_enable_pending_irqs(is, &is->normal_irqs, false);
spin_unlock(&is->normal_irqs_lock);
trusty_irq_enable_pending_irqs(is, this_cpu_ptr(is->percpu_irqs), true);
return NOTIFY_OK;
}
static irqreturn_t trusty_irq_handler(int irq, void *data)
{
struct trusty_irq *trusty_irq = data;
struct trusty_irq_state *is = trusty_irq->is;
struct trusty_irq_irqset *irqset;
/*
* If use GICv3, we trigger the nop to Trusty OS
* directly instead of yield another pending
* IRQ.
*/
if (is->gicv3_workaround) {
trusty_enqueue_nop(is->trusty_dev, NULL);
return IRQ_HANDLED;
}
dev_dbg(is->dev, "%s: irq %d, percpu %d, cpu %d, enable %d\n",
__func__, irq, trusty_irq->irq, smp_processor_id(),
trusty_irq->enable);
if (!trusty_irq->doorbell) {
if (trusty_irq->percpu) {
disable_percpu_irq(irq);
irqset = this_cpu_ptr(is->percpu_irqs);
} else {
disable_irq_nosync(irq);
irqset = &is->normal_irqs;
}
spin_lock(&is->normal_irqs_lock);
if (trusty_irq->enable) {
hlist_del(&trusty_irq->node);
hlist_add_head(&trusty_irq->node, &irqset->pending);
}
spin_unlock(&is->normal_irqs_lock);
}
trusty_enqueue_nop(is->trusty_dev, NULL);
dev_dbg(is->dev, "%s: irq %d done\n", __func__, irq);
return IRQ_HANDLED;
}
static int trusty_irq_cpu_up(unsigned int cpu, struct hlist_node *node)
{
unsigned long irq_flags;
struct trusty_irq_state *is;
is = container_of(node, struct trusty_irq_state, cpuhp_node);
dev_dbg(is->dev, "%s: cpu %d\n", __func__, cpu);
local_irq_save(irq_flags);
trusty_irq_enable_irqset(is, this_cpu_ptr(is->percpu_irqs));
local_irq_restore(irq_flags);
return 0;
}
static int trusty_irq_cpu_down(unsigned int cpu, struct hlist_node *node)
{
unsigned long irq_flags;
struct trusty_irq_state *is;
is = container_of(node, struct trusty_irq_state, cpuhp_node);
dev_dbg(is->dev, "%s: cpu %d\n", __func__, cpu);
local_irq_save(irq_flags);
trusty_irq_disable_irqset(is, this_cpu_ptr(is->percpu_irqs));
local_irq_restore(irq_flags);
return 0;
}
static int trusty_irq_create_irq_mapping(struct trusty_irq_state *is, int irq)
{
int ret;
int index;
u32 irq_pos;
u32 templ_idx;
u32 range_base;
u32 range_end;
struct of_phandle_args oirq;
/* check if "interrupt-ranges" property is present */
if (!of_find_property(is->dev->of_node, "interrupt-ranges", NULL)) {
/* fallback to old behavior to be backward compatible with
* systems that do not need IRQ domains.
*/
return irq;
}
/* find irq range */
for (index = 0;; index += 3) {
ret = of_property_read_u32_index(is->dev->of_node,
"interrupt-ranges",
index, &range_base);
if (ret)
return ret;
ret = of_property_read_u32_index(is->dev->of_node,
"interrupt-ranges",
index + 1, &range_end);
if (ret)
return ret;
if (irq >= range_base && irq <= range_end)
break;
}
/* read the rest of range entry: template index and irq_pos */
ret = of_property_read_u32_index(is->dev->of_node,
"interrupt-ranges",
index + 2, &templ_idx);
if (ret)
return ret;
/* read irq template */
ret = of_parse_phandle_with_args(is->dev->of_node,
"interrupt-templates",
"#interrupt-cells",
templ_idx, &oirq);
if (ret)
return ret;
WARN_ON(!oirq.np);
WARN_ON(!oirq.args_count);
/*
* An IRQ template is a non empty array of u32 values describing group
* of interrupts having common properties. The u32 entry with index
* zero contains the position of irq_id in interrupt specifier array
* followed by data representing interrupt specifier array with irq id
* field omitted, so to convert irq template to interrupt specifier
* array we have to move down one slot the first irq_pos entries and
* replace the resulting gap with real irq id.
*/
irq_pos = oirq.args[0];
if (irq_pos >= oirq.args_count) {
dev_err(is->dev, "irq pos is out of range: %d\n", irq_pos);
return -EINVAL;
}
for (index = 1; index <= irq_pos; index++)
oirq.args[index - 1] = oirq.args[index];
oirq.args[irq_pos] = irq - range_base;
ret = irq_create_of_mapping(&oirq);
return (!ret) ? -EINVAL : ret;
}
static int trusty_irq_init_normal_irq(struct trusty_irq_state *is, int tirq)
{
int ret;
int irq;
unsigned long irq_flags;
struct trusty_irq *trusty_irq;
dev_dbg(is->dev, "%s: irq %d\n", __func__, tirq);
irq = trusty_irq_create_irq_mapping(is, tirq);
if (irq < 0) {
dev_err(is->dev,
"trusty_irq_create_irq_mapping failed (%d)\n", irq);
return irq;
}
trusty_irq = kzalloc(sizeof(*trusty_irq), GFP_KERNEL);
if (!trusty_irq)
return -ENOMEM;
trusty_irq->is = is;
trusty_irq->irq = irq;
trusty_irq->enable = true;
spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
hlist_add_head(&trusty_irq->node, &is->normal_irqs.inactive);
spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
ret = request_irq(irq, trusty_irq_handler, IRQF_NO_THREAD,
"trusty", trusty_irq);
if (ret) {
dev_err(is->dev, "request_irq failed %d\n", ret);
goto err_request_irq;
}
return 0;
err_request_irq:
spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
hlist_del(&trusty_irq->node);
spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
kfree(trusty_irq);
return ret;
}
static int trusty_irq_init_per_cpu_irq(struct trusty_irq_state *is, int tirq,
unsigned int type)
{
int ret;
int irq;
unsigned int cpu;
struct trusty_irq __percpu *trusty_irq_handler_data;
dev_dbg(is->dev, "%s: irq %d\n", __func__, tirq);
irq = trusty_irq_create_irq_mapping(is, tirq);
if (irq <= 0) {
dev_err(is->dev,
"trusty_irq_create_irq_mapping failed (%d)\n", irq);
return irq;
}
trusty_irq_handler_data = alloc_percpu(struct trusty_irq);
if (!trusty_irq_handler_data)
return -ENOMEM;
for_each_possible_cpu(cpu) {
struct trusty_irq *trusty_irq;
struct trusty_irq_irqset *irqset;
trusty_irq = per_cpu_ptr(trusty_irq_handler_data, cpu);
irqset = per_cpu_ptr(is->percpu_irqs, cpu);
trusty_irq->is = is;
hlist_add_head(&trusty_irq->node, &irqset->inactive);
trusty_irq->irq = irq;
trusty_irq->percpu = true;
trusty_irq->doorbell = type == TRUSTY_IRQ_TYPE_DOORBELL;
trusty_irq->percpu_ptr = trusty_irq_handler_data;
}
ret = request_percpu_irq(irq, trusty_irq_handler, "trusty",
trusty_irq_handler_data);
if (ret) {
dev_err(is->dev, "request_percpu_irq failed %d\n", ret);
goto err_request_percpu_irq;
}
return 0;
err_request_percpu_irq:
for_each_possible_cpu(cpu) {
struct trusty_irq *trusty_irq;
trusty_irq = per_cpu_ptr(trusty_irq_handler_data, cpu);
hlist_del(&trusty_irq->node);
}
free_percpu(trusty_irq_handler_data);
return ret;
}
static int trusty_smc_get_next_irq(struct trusty_irq_state *is,
unsigned long min_irq, unsigned int type)
{
return trusty_fast_call32(is->trusty_dev, SMC_FC_GET_NEXT_IRQ,
min_irq, type, 0);
}
static int trusty_irq_init_one(struct trusty_irq_state *is,
int irq, unsigned int type)
{
int ret;
irq = trusty_smc_get_next_irq(is, irq, type);
if (irq < 0)
return irq;
if (type != TRUSTY_IRQ_TYPE_NORMAL)
ret = trusty_irq_init_per_cpu_irq(is, irq, type);
else
ret = trusty_irq_init_normal_irq(is, irq);
if (ret) {
dev_warn(is->dev,
"failed to initialize irq %d, irq will be ignored\n",
irq);
}
return irq + 1;
}
static void trusty_irq_free_irqs(struct trusty_irq_state *is)
{
struct trusty_irq *irq;
struct hlist_node *n;
unsigned int cpu;
hlist_for_each_entry_safe(irq, n, &is->normal_irqs.inactive, node) {
dev_dbg(is->dev, "%s: irq %d\n", __func__, irq->irq);
free_irq(irq->irq, irq);
hlist_del(&irq->node);
kfree(irq);
}
hlist_for_each_entry_safe(irq, n,
&this_cpu_ptr(is->percpu_irqs)->inactive,
node) {
struct trusty_irq __percpu *trusty_irq_handler_data;
dev_dbg(is->dev, "%s: percpu irq %d\n", __func__, irq->irq);
trusty_irq_handler_data = irq->percpu_ptr;
free_percpu_irq(irq->irq, trusty_irq_handler_data);
for_each_possible_cpu(cpu) {
struct trusty_irq *irq_tmp;
irq_tmp = per_cpu_ptr(trusty_irq_handler_data, cpu);
hlist_del(&irq_tmp->node);
}
free_percpu(trusty_irq_handler_data);
}
}
static int trusty_irq_probe(struct platform_device *pdev)
{
int ret;
int irq;
unsigned long irq_flags;
struct trusty_irq_state *is;
is = kzalloc(sizeof(*is), GFP_KERNEL);
if (!is) {
ret = -ENOMEM;
goto err_alloc_is;
}
is->dev = &pdev->dev;
is->trusty_dev = is->dev->parent;
spin_lock_init(&is->normal_irqs_lock);
is->percpu_irqs = alloc_percpu(struct trusty_irq_irqset);
if (!is->percpu_irqs) {
ret = -ENOMEM;
goto err_alloc_pending_percpu_irqs;
}
platform_set_drvdata(pdev, is);
if (of_find_property(is->dev->of_node, "use-gicv3-workaround", NULL)) {
is->gicv3_workaround = true;
} else {
is->gicv3_workaround = false;
}
is->trusty_call_notifier.notifier_call = trusty_irq_call_notify;
ret = trusty_call_notifier_register(is->trusty_dev,
&is->trusty_call_notifier);
if (ret) {
dev_err(&pdev->dev,
"failed to register trusty call notifier\n");
goto err_trusty_call_notifier_register;
}
for (irq = 0; irq >= 0;)
irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_PER_CPU);
for (irq = 0; irq >= 0;)
irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_NORMAL);
for (irq = 0; irq >= 0;)
irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_DOORBELL);
ret = cpuhp_state_add_instance(trusty_irq_cpuhp_slot, &is->cpuhp_node);
if (ret < 0) {
dev_err(&pdev->dev, "cpuhp_state_add_instance failed %d\n",
ret);
goto err_add_cpuhp_instance;
}
return 0;
err_add_cpuhp_instance:
spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
trusty_irq_disable_irqset(is, &is->normal_irqs);
spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
trusty_irq_free_irqs(is);
trusty_call_notifier_unregister(is->trusty_dev,
&is->trusty_call_notifier);
err_trusty_call_notifier_register:
free_percpu(is->percpu_irqs);
err_alloc_pending_percpu_irqs:
kfree(is);
err_alloc_is:
return ret;
}
static int trusty_irq_remove(struct platform_device *pdev)
{
int ret;
unsigned long irq_flags;
struct trusty_irq_state *is = platform_get_drvdata(pdev);
ret = cpuhp_state_remove_instance(trusty_irq_cpuhp_slot,
&is->cpuhp_node);
if (WARN_ON(ret))
return ret;
spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
trusty_irq_disable_irqset(is, &is->normal_irqs);
spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
trusty_irq_free_irqs(is);
trusty_call_notifier_unregister(is->trusty_dev,
&is->trusty_call_notifier);
free_percpu(is->percpu_irqs);
kfree(is);
return 0;
}
static const struct of_device_id trusty_test_of_match[] = {
{ .compatible = "android,trusty-irq-v1", },
{},
};
MODULE_DEVICE_TABLE(trusty, trusty_test_of_match);
static struct platform_driver trusty_irq_driver = {
.probe = trusty_irq_probe,
.remove = trusty_irq_remove,
.driver = {
.name = "trusty-irq",
.of_match_table = trusty_test_of_match,
},
};
static int __init trusty_irq_driver_init(void)
{
int ret;
/* allocate dynamic cpuhp state slot */
ret = cpuhp_setup_state_multi(CPUHP_AP_ONLINE_DYN,
"trusty-irq:cpu:online",
trusty_irq_cpu_up,
trusty_irq_cpu_down);
if (ret < 0)
return ret;
trusty_irq_cpuhp_slot = ret;
/* Register platform driver */
ret = platform_driver_register(&trusty_irq_driver);
if (ret < 0)
goto err_driver_register;
return ret;
err_driver_register:
/* undo cpuhp slot allocation */
cpuhp_remove_multi_state(trusty_irq_cpuhp_slot);
trusty_irq_cpuhp_slot = -1;
return ret;
}
static void __exit trusty_irq_driver_exit(void)
{
platform_driver_unregister(&trusty_irq_driver);
cpuhp_remove_multi_state(trusty_irq_cpuhp_slot);
trusty_irq_cpuhp_slot = -1;
}
module_init(trusty_irq_driver_init);
module_exit(trusty_irq_driver_exit);
MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("Trusty IRQ driver");