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#include <linux/module.h>
33#include <linux/init.h>
34#include <linux/serio.h>
35#include <linux/interrupt.h>
36#include <linux/err.h>
37#include <linux/bitops.h>
38#include <linux/platform_device.h>
39
40#include <asm/io.h>
41#include <asm/uaccess.h>
42#include <asm/q40_master.h>
43#include <asm/irq.h>
44#include <asm/q40ints.h>
45
46MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
47MODULE_DESCRIPTION("Q40 PS/2 keyboard controller driver");
48MODULE_LICENSE("GPL");
49
50static DEFINE_SPINLOCK(q40kbd_lock);
51static struct serio *q40kbd_port;
52static struct platform_device *q40kbd_device;
53
54static irqreturn_t q40kbd_interrupt(int irq, void *dev_id)
55{
56 unsigned long flags;
57
58 spin_lock_irqsave(&q40kbd_lock, flags);
59
60 if (Q40_IRQ_KEYB_MASK & master_inb(INTERRUPT_REG))
61 serio_interrupt(q40kbd_port, master_inb(KEYCODE_REG), 0);
62
63 master_outb(-1, KEYBOARD_UNLOCK_REG);
64
65 spin_unlock_irqrestore(&q40kbd_lock, flags);
66
67 return IRQ_HANDLED;
68}
69
70
71
72
73
74static void q40kbd_flush(void)
75{
76 int maxread = 100;
77 unsigned long flags;
78
79 spin_lock_irqsave(&q40kbd_lock, flags);
80
81 while (maxread-- && (Q40_IRQ_KEYB_MASK & master_inb(INTERRUPT_REG)))
82 master_inb(KEYCODE_REG);
83
84 spin_unlock_irqrestore(&q40kbd_lock, flags);
85}
86
87
88
89
90
91
92static int q40kbd_open(struct serio *port)
93{
94 q40kbd_flush();
95
96 if (request_irq(Q40_IRQ_KEYBOARD, q40kbd_interrupt, 0, "q40kbd", NULL)) {
97 printk(KERN_ERR "q40kbd.c: Can't get irq %d.\n", Q40_IRQ_KEYBOARD);
98 return -EBUSY;
99 }
100
101
102 master_outb(-1, KEYBOARD_UNLOCK_REG);
103 master_outb(1, KEY_IRQ_ENABLE_REG);
104
105 return 0;
106}
107
108static void q40kbd_close(struct serio *port)
109{
110 master_outb(0, KEY_IRQ_ENABLE_REG);
111 master_outb(-1, KEYBOARD_UNLOCK_REG);
112 free_irq(Q40_IRQ_KEYBOARD, NULL);
113
114 q40kbd_flush();
115}
116
117static int __devinit q40kbd_probe(struct platform_device *dev)
118{
119 q40kbd_port = kzalloc(sizeof(struct serio), GFP_KERNEL);
120 if (!q40kbd_port)
121 return -ENOMEM;
122
123 q40kbd_port->id.type = SERIO_8042;
124 q40kbd_port->open = q40kbd_open;
125 q40kbd_port->close = q40kbd_close;
126 q40kbd_port->dev.parent = &dev->dev;
127 strlcpy(q40kbd_port->name, "Q40 Kbd Port", sizeof(q40kbd_port->name));
128 strlcpy(q40kbd_port->phys, "Q40", sizeof(q40kbd_port->phys));
129
130 serio_register_port(q40kbd_port);
131 printk(KERN_INFO "serio: Q40 kbd registered\n");
132
133 return 0;
134}
135
136static int __devexit q40kbd_remove(struct platform_device *dev)
137{
138 serio_unregister_port(q40kbd_port);
139
140 return 0;
141}
142
143static struct platform_driver q40kbd_driver = {
144 .driver = {
145 .name = "q40kbd",
146 .owner = THIS_MODULE,
147 },
148 .probe = q40kbd_probe,
149 .remove = __devexit_p(q40kbd_remove),
150};
151
152static int __init q40kbd_init(void)
153{
154 int error;
155
156 if (!MACH_IS_Q40)
157 return -ENODEV;
158
159 error = platform_driver_register(&q40kbd_driver);
160 if (error)
161 return error;
162
163 q40kbd_device = platform_device_alloc("q40kbd", -1);
164 if (!q40kbd_device)
165 goto err_unregister_driver;
166
167 error = platform_device_add(q40kbd_device);
168 if (error)
169 goto err_free_device;
170
171 return 0;
172
173 err_free_device:
174 platform_device_put(q40kbd_device);
175 err_unregister_driver:
176 platform_driver_unregister(&q40kbd_driver);
177 return error;
178}
179
180static void __exit q40kbd_exit(void)
181{
182 platform_device_unregister(q40kbd_device);
183 platform_driver_unregister(&q40kbd_driver);
184}
185
186module_init(q40kbd_init);
187module_exit(q40kbd_exit);
188