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