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