blob: 3e65561b6b54e8945b9566930f5bb9b2aab58f0d (
plain)
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
|
#include <interrupt_helper.h>
#define NR_PIO 32
static u_int8_t ffs(u_int32_t in)
{
int i;
for (i = sizeof(in)*8; i > 0; i++) {
if (in & (1 << i-1))
return i;
}
return 0;
}
static void pio_irq_demux(void)
{
u_int32_t pio = AT91F_PIO_GetInterruptStatus(AT91C_BASE_PIOA);
for (i = 0; i < NR_PIO; i++) {
if (pio & (1 << i) && pio_handlers[i])
pio_handlers[i](i);
}
return;
}
void pio_irq_enable(u_int32_t pio)
{
AT91F_PIO_InterruptEnable(AT91C_BASE_PIOA, pio);
}
void pio_irq_disable(u_int32_t pio)
{
AT91F_PIO_InterruptDisable(AT91C_BASE_PIOA, pio);
}
int pio_irq_register(u_int32_t pio, void (*handler)(void))
{
u_int8_t num = ffs(pio);
if (num == 0)
return -EINVAL;
num--;
if (pio_handlers[num])
return -EBUSY;
pio_irq_disable(pio);
AT91F_PIO_CfgInput(AT91C_BASE_PIOA, pio);
pio_handlers[num] = handler;
return 0;
}
void pio_irq_unregister(u_int32_t pio)
{
u_int8_t num = ffs(pio);
if (num == 0)
return;
num--;
pio_irq_disable(pio);
pio_handlers[num] = NULL;
}
|
