1
2
3
4
5
6
7
8
9#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
10
11#include <linux/init.h>
12#include <linux/kernel.h>
13#include <linux/module.h>
14#include <linux/bitmap.h>
15#include <linux/io.h>
16#include <mach/ilsel.h>
17
18
19
20
21
22
23
24
25
26
27
28#define ILSEL_BASE 0xb8100004
29#define ILSEL_LEVELS 15
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44static unsigned long ilsel_level_map;
45
46static inline unsigned int ilsel_offset(unsigned int bit)
47{
48 return ILSEL_LEVELS - bit - 1;
49}
50
51static inline unsigned long mk_ilsel_addr(unsigned int bit)
52{
53 return ILSEL_BASE + ((ilsel_offset(bit) >> 1) & ~0x1);
54}
55
56static inline unsigned int mk_ilsel_shift(unsigned int bit)
57{
58 return (ilsel_offset(bit) & 0x3) << 2;
59}
60
61static void __ilsel_enable(ilsel_source_t set, unsigned int bit)
62{
63 unsigned int tmp, shift;
64 unsigned long addr;
65
66 pr_notice("enabling ILSEL set %d\n", set);
67
68 addr = mk_ilsel_addr(bit);
69 shift = mk_ilsel_shift(bit);
70
71 pr_debug("%s: bit#%d: addr - 0x%08lx (shift %d, set %d)\n",
72 __func__, bit, addr, shift, set);
73
74 tmp = __raw_readw(addr);
75 tmp &= ~(0xf << shift);
76 tmp |= set << shift;
77 __raw_writew(tmp, addr);
78}
79
80
81
82
83
84
85
86
87
88
89
90
91
92int ilsel_enable(ilsel_source_t set)
93{
94 unsigned int bit;
95
96 if (unlikely(set > ILSEL_KEY)) {
97 pr_err("Aliased sources must use ilsel_enable_fixed()\n");
98 return -EINVAL;
99 }
100
101 do {
102 bit = find_first_zero_bit(&ilsel_level_map, ILSEL_LEVELS);
103 } while (test_and_set_bit(bit, &ilsel_level_map));
104
105 __ilsel_enable(set, bit);
106
107 return bit;
108}
109EXPORT_SYMBOL_GPL(ilsel_enable);
110
111
112
113
114
115
116
117
118
119
120
121
122int ilsel_enable_fixed(ilsel_source_t set, unsigned int level)
123{
124 unsigned int bit = ilsel_offset(level - 1);
125
126 if (test_and_set_bit(bit, &ilsel_level_map))
127 return -EBUSY;
128
129 __ilsel_enable(set, bit);
130
131 return bit;
132}
133EXPORT_SYMBOL_GPL(ilsel_enable_fixed);
134
135
136
137
138
139
140
141void ilsel_disable(unsigned int irq)
142{
143 unsigned long addr;
144 unsigned int tmp;
145
146 pr_notice("disabling ILSEL set %d\n", irq);
147
148 addr = mk_ilsel_addr(irq);
149
150 tmp = __raw_readw(addr);
151 tmp &= ~(0xf << mk_ilsel_shift(irq));
152 __raw_writew(tmp, addr);
153
154 clear_bit(irq, &ilsel_level_map);
155}
156EXPORT_SYMBOL_GPL(ilsel_disable);
157