1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Driver for the Gravis Grip Multiport, a gamepad "hub" that |
4 | * connects up to four 9-pin digital gamepads/joysticks. |
5 | * Driver tested on SMP and UP kernel versions 2.4.18-4 and 2.4.18-5. |
6 | * |
7 | * Thanks to Chris Gassib for helpful advice. |
8 | * |
9 | * Copyright (c) 2002 Brian Bonnlander, Bill Soudan |
10 | * Copyright (c) 1998-2000 Vojtech Pavlik |
11 | */ |
12 | |
13 | #include <linux/kernel.h> |
14 | #include <linux/module.h> |
15 | #include <linux/slab.h> |
16 | #include <linux/gameport.h> |
17 | #include <linux/input.h> |
18 | #include <linux/delay.h> |
19 | #include <linux/proc_fs.h> |
20 | #include <linux/jiffies.h> |
21 | |
22 | #define DRIVER_DESC "Gravis Grip Multiport driver" |
23 | |
24 | MODULE_AUTHOR("Brian Bonnlander" ); |
25 | MODULE_DESCRIPTION(DRIVER_DESC); |
26 | MODULE_LICENSE("GPL" ); |
27 | |
28 | #ifdef GRIP_DEBUG |
29 | #define dbg(format, arg...) printk(KERN_ERR __FILE__ ": " format "\n" , ## arg) |
30 | #else |
31 | #define dbg(format, arg...) do {} while (0) |
32 | #endif |
33 | |
34 | #define GRIP_MAX_PORTS 4 |
35 | /* |
36 | * Grip multiport state |
37 | */ |
38 | |
39 | struct grip_port { |
40 | struct input_dev *dev; |
41 | int mode; |
42 | int registered; |
43 | |
44 | /* individual gamepad states */ |
45 | int buttons; |
46 | int xaxes; |
47 | int yaxes; |
48 | int dirty; /* has the state been updated? */ |
49 | }; |
50 | |
51 | struct grip_mp { |
52 | struct gameport *gameport; |
53 | struct grip_port *port[GRIP_MAX_PORTS]; |
54 | int reads; |
55 | int bads; |
56 | }; |
57 | |
58 | /* |
59 | * Multiport packet interpretation |
60 | */ |
61 | |
62 | #define PACKET_FULL 0x80000000 /* packet is full */ |
63 | #define PACKET_IO_FAST 0x40000000 /* 3 bits per gameport read */ |
64 | #define PACKET_IO_SLOW 0x20000000 /* 1 bit per gameport read */ |
65 | #define PACKET_MP_MORE 0x04000000 /* multiport wants to send more */ |
66 | #define PACKET_MP_DONE 0x02000000 /* multiport done sending */ |
67 | |
68 | /* |
69 | * Packet status code interpretation |
70 | */ |
71 | |
72 | #define IO_GOT_PACKET 0x0100 /* Got a packet */ |
73 | #define IO_MODE_FAST 0x0200 /* Used 3 data bits per gameport read */ |
74 | #define IO_SLOT_CHANGE 0x0800 /* Multiport physical slot status changed */ |
75 | #define IO_DONE 0x1000 /* Multiport is done sending packets */ |
76 | #define IO_RETRY 0x4000 /* Try again later to get packet */ |
77 | #define IO_RESET 0x8000 /* Force multiport to resend all packets */ |
78 | |
79 | /* |
80 | * Gamepad configuration data. Other 9-pin digital joystick devices |
81 | * may work with the multiport, so this may not be an exhaustive list! |
82 | * Commodore 64 joystick remains untested. |
83 | */ |
84 | |
85 | #define GRIP_INIT_DELAY 2000 /* 2 ms */ |
86 | |
87 | #define GRIP_MODE_NONE 0 |
88 | #define GRIP_MODE_RESET 1 |
89 | #define GRIP_MODE_GP 2 |
90 | #define GRIP_MODE_C64 3 |
91 | |
92 | static const int grip_btn_gp[] = { BTN_TR, BTN_TL, BTN_A, BTN_B, BTN_C, BTN_X, BTN_Y, BTN_Z, -1 }; |
93 | static const int grip_btn_c64[] = { BTN_JOYSTICK, -1 }; |
94 | |
95 | static const int grip_abs_gp[] = { ABS_X, ABS_Y, -1 }; |
96 | static const int grip_abs_c64[] = { ABS_X, ABS_Y, -1 }; |
97 | |
98 | static const int *grip_abs[] = { NULL, NULL, grip_abs_gp, grip_abs_c64 }; |
99 | static const int *grip_btn[] = { NULL, NULL, grip_btn_gp, grip_btn_c64 }; |
100 | |
101 | static const char *grip_name[] = { NULL, NULL, "Gravis Grip Pad" , "Commodore 64 Joystick" }; |
102 | |
103 | static const int init_seq[] = { |
104 | 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, |
105 | 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, |
106 | 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, |
107 | 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 1, 1 }; |
108 | |
109 | /* Maps multiport directional values to X,Y axis values (each axis encoded in 3 bits) */ |
110 | |
111 | static const int axis_map[] = { 5, 9, 1, 5, 6, 10, 2, 6, 4, 8, 0, 4, 5, 9, 1, 5 }; |
112 | |
113 | static int register_slot(int i, struct grip_mp *grip); |
114 | |
115 | /* |
116 | * Returns whether an odd or even number of bits are on in pkt. |
117 | */ |
118 | |
119 | static int bit_parity(u32 pkt) |
120 | { |
121 | int x = pkt ^ (pkt >> 16); |
122 | x ^= x >> 8; |
123 | x ^= x >> 4; |
124 | x ^= x >> 2; |
125 | x ^= x >> 1; |
126 | return x & 1; |
127 | } |
128 | |
129 | /* |
130 | * Poll gameport; return true if all bits set in 'onbits' are on and |
131 | * all bits set in 'offbits' are off. |
132 | */ |
133 | |
134 | static inline int poll_until(u8 onbits, u8 offbits, int u_sec, struct gameport* gp, u8 *data) |
135 | { |
136 | int i, nloops; |
137 | |
138 | nloops = gameport_time(gameport: gp, time: u_sec); |
139 | for (i = 0; i < nloops; i++) { |
140 | *data = gameport_read(gameport: gp); |
141 | if ((*data & onbits) == onbits && |
142 | (~(*data) & offbits) == offbits) |
143 | return 1; |
144 | } |
145 | dbg("gameport timed out after %d microseconds.\n" , u_sec); |
146 | return 0; |
147 | } |
148 | |
149 | /* |
150 | * Gets a 28-bit packet from the multiport. |
151 | * |
152 | * After getting a packet successfully, commands encoded by sendcode may |
153 | * be sent to the multiport. |
154 | * |
155 | * The multiport clock value is reflected in gameport bit B4. |
156 | * |
157 | * Returns a packet status code indicating whether packet is valid, the transfer |
158 | * mode, and any error conditions. |
159 | * |
160 | * sendflags: current I/O status |
161 | * sendcode: data to send to the multiport if sendflags is nonzero |
162 | */ |
163 | |
164 | static int mp_io(struct gameport* gameport, int sendflags, int sendcode, u32 *packet) |
165 | { |
166 | u8 raw_data; /* raw data from gameport */ |
167 | u8 data_mask; /* packet data bits from raw_data */ |
168 | u32 pkt; /* packet temporary storage */ |
169 | int bits_per_read; /* num packet bits per gameport read */ |
170 | int portvals = 0; /* used for port value sanity check */ |
171 | int i; |
172 | |
173 | /* Gameport bits B0, B4, B5 should first be off, then B4 should come on. */ |
174 | |
175 | *packet = 0; |
176 | raw_data = gameport_read(gameport); |
177 | if (raw_data & 1) |
178 | return IO_RETRY; |
179 | |
180 | for (i = 0; i < 64; i++) { |
181 | raw_data = gameport_read(gameport); |
182 | portvals |= 1 << ((raw_data >> 4) & 3); /* Demux B4, B5 */ |
183 | } |
184 | |
185 | if (portvals == 1) { /* B4, B5 off */ |
186 | raw_data = gameport_read(gameport); |
187 | portvals = raw_data & 0xf0; |
188 | |
189 | if (raw_data & 0x31) |
190 | return IO_RESET; |
191 | gameport_trigger(gameport); |
192 | |
193 | if (!poll_until(onbits: 0x10, offbits: 0, u_sec: 308, gp: gameport, data: &raw_data)) |
194 | return IO_RESET; |
195 | } else |
196 | return IO_RETRY; |
197 | |
198 | /* Determine packet transfer mode and prepare for packet construction. */ |
199 | |
200 | if (raw_data & 0x20) { /* 3 data bits/read */ |
201 | portvals |= raw_data >> 4; /* Compare B4-B7 before & after trigger */ |
202 | |
203 | if (portvals != 0xb) |
204 | return 0; |
205 | data_mask = 7; |
206 | bits_per_read = 3; |
207 | pkt = (PACKET_FULL | PACKET_IO_FAST) >> 28; |
208 | } else { /* 1 data bit/read */ |
209 | data_mask = 1; |
210 | bits_per_read = 1; |
211 | pkt = (PACKET_FULL | PACKET_IO_SLOW) >> 28; |
212 | } |
213 | |
214 | /* Construct a packet. Final data bits must be zero. */ |
215 | |
216 | while (1) { |
217 | if (!poll_until(onbits: 0, offbits: 0x10, u_sec: 77, gp: gameport, data: &raw_data)) |
218 | return IO_RESET; |
219 | raw_data = (raw_data >> 5) & data_mask; |
220 | |
221 | if (pkt & PACKET_FULL) |
222 | break; |
223 | pkt = (pkt << bits_per_read) | raw_data; |
224 | |
225 | if (!poll_until(onbits: 0x10, offbits: 0, u_sec: 77, gp: gameport, data: &raw_data)) |
226 | return IO_RESET; |
227 | } |
228 | |
229 | if (raw_data) |
230 | return IO_RESET; |
231 | |
232 | /* If 3 bits/read used, drop from 30 bits to 28. */ |
233 | |
234 | if (bits_per_read == 3) { |
235 | pkt = (pkt & 0xffff0000) | ((pkt << 1) & 0xffff); |
236 | pkt = (pkt >> 2) | 0xf0000000; |
237 | } |
238 | |
239 | if (bit_parity(pkt) == 1) |
240 | return IO_RESET; |
241 | |
242 | /* Acknowledge packet receipt */ |
243 | |
244 | if (!poll_until(onbits: 0x30, offbits: 0, u_sec: 77, gp: gameport, data: &raw_data)) |
245 | return IO_RESET; |
246 | |
247 | raw_data = gameport_read(gameport); |
248 | |
249 | if (raw_data & 1) |
250 | return IO_RESET; |
251 | |
252 | gameport_trigger(gameport); |
253 | |
254 | if (!poll_until(onbits: 0, offbits: 0x20, u_sec: 77, gp: gameport, data: &raw_data)) |
255 | return IO_RESET; |
256 | |
257 | /* Return if we just wanted the packet or multiport wants to send more */ |
258 | |
259 | *packet = pkt; |
260 | if ((sendflags == 0) || ((sendflags & IO_RETRY) && !(pkt & PACKET_MP_DONE))) |
261 | return IO_GOT_PACKET; |
262 | |
263 | if (pkt & PACKET_MP_MORE) |
264 | return IO_GOT_PACKET | IO_RETRY; |
265 | |
266 | /* Multiport is done sending packets and is ready to receive data */ |
267 | |
268 | if (!poll_until(onbits: 0x20, offbits: 0, u_sec: 77, gp: gameport, data: &raw_data)) |
269 | return IO_GOT_PACKET | IO_RESET; |
270 | |
271 | raw_data = gameport_read(gameport); |
272 | if (raw_data & 1) |
273 | return IO_GOT_PACKET | IO_RESET; |
274 | |
275 | /* Trigger gameport based on bits in sendcode */ |
276 | |
277 | gameport_trigger(gameport); |
278 | do { |
279 | if (!poll_until(onbits: 0x20, offbits: 0x10, u_sec: 116, gp: gameport, data: &raw_data)) |
280 | return IO_GOT_PACKET | IO_RESET; |
281 | |
282 | if (!poll_until(onbits: 0x30, offbits: 0, u_sec: 193, gp: gameport, data: &raw_data)) |
283 | return IO_GOT_PACKET | IO_RESET; |
284 | |
285 | if (raw_data & 1) |
286 | return IO_GOT_PACKET | IO_RESET; |
287 | |
288 | if (sendcode & 1) |
289 | gameport_trigger(gameport); |
290 | |
291 | sendcode >>= 1; |
292 | } while (sendcode); |
293 | |
294 | return IO_GOT_PACKET | IO_MODE_FAST; |
295 | } |
296 | |
297 | /* |
298 | * Disables and restores interrupts for mp_io(), which does the actual I/O. |
299 | */ |
300 | |
301 | static int multiport_io(struct gameport* gameport, int sendflags, int sendcode, u32 *packet) |
302 | { |
303 | int status; |
304 | unsigned long flags; |
305 | |
306 | local_irq_save(flags); |
307 | status = mp_io(gameport, sendflags, sendcode, packet); |
308 | local_irq_restore(flags); |
309 | |
310 | return status; |
311 | } |
312 | |
313 | /* |
314 | * Puts multiport into digital mode. Multiport LED turns green. |
315 | * |
316 | * Returns true if a valid digital packet was received, false otherwise. |
317 | */ |
318 | |
319 | static int dig_mode_start(struct gameport *gameport, u32 *packet) |
320 | { |
321 | int i; |
322 | int flags, tries = 0, bads = 0; |
323 | |
324 | for (i = 0; i < ARRAY_SIZE(init_seq); i++) { /* Send magic sequence */ |
325 | if (init_seq[i]) |
326 | gameport_trigger(gameport); |
327 | udelay(GRIP_INIT_DELAY); |
328 | } |
329 | |
330 | for (i = 0; i < 16; i++) /* Wait for multiport to settle */ |
331 | udelay(GRIP_INIT_DELAY); |
332 | |
333 | while (tries < 64 && bads < 8) { /* Reset multiport and try getting a packet */ |
334 | |
335 | flags = multiport_io(gameport, IO_RESET, sendcode: 0x27, packet); |
336 | |
337 | if (flags & IO_MODE_FAST) |
338 | return 1; |
339 | |
340 | if (flags & IO_RETRY) |
341 | tries++; |
342 | else |
343 | bads++; |
344 | } |
345 | return 0; |
346 | } |
347 | |
348 | /* |
349 | * Packet structure: B0-B15 => gamepad state |
350 | * B16-B20 => gamepad device type |
351 | * B21-B24 => multiport slot index (1-4) |
352 | * |
353 | * Known device types: 0x1f (grip pad), 0x0 (no device). Others may exist. |
354 | * |
355 | * Returns the packet status. |
356 | */ |
357 | |
358 | static int get_and_decode_packet(struct grip_mp *grip, int flags) |
359 | { |
360 | struct grip_port *port; |
361 | u32 packet; |
362 | int joytype = 0; |
363 | int slot; |
364 | |
365 | /* Get a packet and check for validity */ |
366 | |
367 | flags &= IO_RESET | IO_RETRY; |
368 | flags = multiport_io(gameport: grip->gameport, sendflags: flags, sendcode: 0, packet: &packet); |
369 | grip->reads++; |
370 | |
371 | if (packet & PACKET_MP_DONE) |
372 | flags |= IO_DONE; |
373 | |
374 | if (flags && !(flags & IO_GOT_PACKET)) { |
375 | grip->bads++; |
376 | return flags; |
377 | } |
378 | |
379 | /* Ignore non-gamepad packets, e.g. multiport hardware version */ |
380 | |
381 | slot = ((packet >> 21) & 0xf) - 1; |
382 | if ((slot < 0) || (slot > 3)) |
383 | return flags; |
384 | |
385 | port = grip->port[slot]; |
386 | |
387 | /* |
388 | * Handle "reset" packets, which occur at startup, and when gamepads |
389 | * are removed or plugged in. May contain configuration of a new gamepad. |
390 | */ |
391 | |
392 | joytype = (packet >> 16) & 0x1f; |
393 | if (!joytype) { |
394 | |
395 | if (port->registered) { |
396 | printk(KERN_INFO "grip_mp: removing %s, slot %d\n" , |
397 | grip_name[port->mode], slot); |
398 | input_unregister_device(port->dev); |
399 | port->registered = 0; |
400 | } |
401 | dbg("Reset: grip multiport slot %d\n" , slot); |
402 | port->mode = GRIP_MODE_RESET; |
403 | flags |= IO_SLOT_CHANGE; |
404 | return flags; |
405 | } |
406 | |
407 | /* Interpret a grip pad packet */ |
408 | |
409 | if (joytype == 0x1f) { |
410 | |
411 | int dir = (packet >> 8) & 0xf; /* eight way directional value */ |
412 | port->buttons = (~packet) & 0xff; |
413 | port->yaxes = ((axis_map[dir] >> 2) & 3) - 1; |
414 | port->xaxes = (axis_map[dir] & 3) - 1; |
415 | port->dirty = 1; |
416 | |
417 | if (port->mode == GRIP_MODE_RESET) |
418 | flags |= IO_SLOT_CHANGE; |
419 | |
420 | port->mode = GRIP_MODE_GP; |
421 | |
422 | if (!port->registered) { |
423 | dbg("New Grip pad in multiport slot %d.\n" , slot); |
424 | if (register_slot(i: slot, grip)) { |
425 | port->mode = GRIP_MODE_RESET; |
426 | port->dirty = 0; |
427 | } |
428 | } |
429 | return flags; |
430 | } |
431 | |
432 | /* Handle non-grip device codes. For now, just print diagnostics. */ |
433 | |
434 | { |
435 | static int strange_code = 0; |
436 | if (strange_code != joytype) { |
437 | printk(KERN_INFO "Possible non-grip pad/joystick detected.\n" ); |
438 | printk(KERN_INFO "Got joy type 0x%x and packet 0x%x.\n" , joytype, packet); |
439 | strange_code = joytype; |
440 | } |
441 | } |
442 | return flags; |
443 | } |
444 | |
445 | /* |
446 | * Returns true if all multiport slot states appear valid. |
447 | */ |
448 | |
449 | static int slots_valid(struct grip_mp *grip) |
450 | { |
451 | int flags, slot, invalid = 0, active = 0; |
452 | |
453 | flags = get_and_decode_packet(grip, flags: 0); |
454 | if (!(flags & IO_GOT_PACKET)) |
455 | return 0; |
456 | |
457 | for (slot = 0; slot < 4; slot++) { |
458 | if (grip->port[slot]->mode == GRIP_MODE_RESET) |
459 | invalid = 1; |
460 | if (grip->port[slot]->mode != GRIP_MODE_NONE) |
461 | active = 1; |
462 | } |
463 | |
464 | /* Return true if no active slot but multiport sent all its data */ |
465 | if (!active) |
466 | return (flags & IO_DONE) ? 1 : 0; |
467 | |
468 | /* Return false if invalid device code received */ |
469 | return invalid ? 0 : 1; |
470 | } |
471 | |
472 | /* |
473 | * Returns whether the multiport was placed into digital mode and |
474 | * able to communicate its state successfully. |
475 | */ |
476 | |
477 | static int multiport_init(struct grip_mp *grip) |
478 | { |
479 | int dig_mode, initialized = 0, tries = 0; |
480 | u32 packet; |
481 | |
482 | dig_mode = dig_mode_start(gameport: grip->gameport, packet: &packet); |
483 | while (!dig_mode && tries < 4) { |
484 | dig_mode = dig_mode_start(gameport: grip->gameport, packet: &packet); |
485 | tries++; |
486 | } |
487 | |
488 | if (dig_mode) |
489 | dbg("multiport_init(): digital mode activated.\n" ); |
490 | else { |
491 | dbg("multiport_init(): unable to activate digital mode.\n" ); |
492 | return 0; |
493 | } |
494 | |
495 | /* Get packets, store multiport state, and check state's validity */ |
496 | for (tries = 0; tries < 4096; tries++) { |
497 | if (slots_valid(grip)) { |
498 | initialized = 1; |
499 | break; |
500 | } |
501 | } |
502 | dbg("multiport_init(): initialized == %d\n" , initialized); |
503 | return initialized; |
504 | } |
505 | |
506 | /* |
507 | * Reports joystick state to the linux input layer. |
508 | */ |
509 | |
510 | static void report_slot(struct grip_mp *grip, int slot) |
511 | { |
512 | struct grip_port *port = grip->port[slot]; |
513 | int i; |
514 | |
515 | /* Store button states with linux input driver */ |
516 | |
517 | for (i = 0; i < 8; i++) |
518 | input_report_key(dev: port->dev, code: grip_btn_gp[i], value: (port->buttons >> i) & 1); |
519 | |
520 | /* Store axis states with linux driver */ |
521 | |
522 | input_report_abs(dev: port->dev, ABS_X, value: port->xaxes); |
523 | input_report_abs(dev: port->dev, ABS_Y, value: port->yaxes); |
524 | |
525 | /* Tell the receiver of the events to process them */ |
526 | |
527 | input_sync(dev: port->dev); |
528 | |
529 | port->dirty = 0; |
530 | } |
531 | |
532 | /* |
533 | * Get the multiport state. |
534 | */ |
535 | |
536 | static void grip_poll(struct gameport *gameport) |
537 | { |
538 | struct grip_mp *grip = gameport_get_drvdata(gameport); |
539 | int i, npkts, flags; |
540 | |
541 | for (npkts = 0; npkts < 4; npkts++) { |
542 | flags = IO_RETRY; |
543 | for (i = 0; i < 32; i++) { |
544 | flags = get_and_decode_packet(grip, flags); |
545 | if ((flags & IO_GOT_PACKET) || !(flags & IO_RETRY)) |
546 | break; |
547 | } |
548 | if (flags & IO_DONE) |
549 | break; |
550 | } |
551 | |
552 | for (i = 0; i < 4; i++) |
553 | if (grip->port[i]->dirty) |
554 | report_slot(grip, slot: i); |
555 | } |
556 | |
557 | /* |
558 | * Called when a joystick device file is opened |
559 | */ |
560 | |
561 | static int grip_open(struct input_dev *dev) |
562 | { |
563 | struct grip_mp *grip = input_get_drvdata(dev); |
564 | |
565 | gameport_start_polling(gameport: grip->gameport); |
566 | return 0; |
567 | } |
568 | |
569 | /* |
570 | * Called when a joystick device file is closed |
571 | */ |
572 | |
573 | static void grip_close(struct input_dev *dev) |
574 | { |
575 | struct grip_mp *grip = input_get_drvdata(dev); |
576 | |
577 | gameport_stop_polling(gameport: grip->gameport); |
578 | } |
579 | |
580 | /* |
581 | * Tell the linux input layer about a newly plugged-in gamepad. |
582 | */ |
583 | |
584 | static int register_slot(int slot, struct grip_mp *grip) |
585 | { |
586 | struct grip_port *port = grip->port[slot]; |
587 | struct input_dev *input_dev; |
588 | int j, t; |
589 | int err; |
590 | |
591 | port->dev = input_dev = input_allocate_device(); |
592 | if (!input_dev) |
593 | return -ENOMEM; |
594 | |
595 | input_dev->name = grip_name[port->mode]; |
596 | input_dev->id.bustype = BUS_GAMEPORT; |
597 | input_dev->id.vendor = GAMEPORT_ID_VENDOR_GRAVIS; |
598 | input_dev->id.product = 0x0100 + port->mode; |
599 | input_dev->id.version = 0x0100; |
600 | input_dev->dev.parent = &grip->gameport->dev; |
601 | |
602 | input_set_drvdata(dev: input_dev, data: grip); |
603 | |
604 | input_dev->open = grip_open; |
605 | input_dev->close = grip_close; |
606 | |
607 | input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS); |
608 | |
609 | for (j = 0; (t = grip_abs[port->mode][j]) >= 0; j++) |
610 | input_set_abs_params(dev: input_dev, axis: t, min: -1, max: 1, fuzz: 0, flat: 0); |
611 | |
612 | for (j = 0; (t = grip_btn[port->mode][j]) >= 0; j++) |
613 | if (t > 0) |
614 | set_bit(nr: t, addr: input_dev->keybit); |
615 | |
616 | err = input_register_device(port->dev); |
617 | if (err) { |
618 | input_free_device(dev: port->dev); |
619 | return err; |
620 | } |
621 | |
622 | port->registered = 1; |
623 | |
624 | if (port->dirty) /* report initial state, if any */ |
625 | report_slot(grip, slot); |
626 | |
627 | return 0; |
628 | } |
629 | |
630 | static int grip_connect(struct gameport *gameport, struct gameport_driver *drv) |
631 | { |
632 | struct grip_mp *grip; |
633 | int err; |
634 | |
635 | if (!(grip = kzalloc(size: sizeof(struct grip_mp), GFP_KERNEL))) |
636 | return -ENOMEM; |
637 | |
638 | grip->gameport = gameport; |
639 | |
640 | gameport_set_drvdata(gameport, data: grip); |
641 | |
642 | err = gameport_open(gameport, drv, GAMEPORT_MODE_RAW); |
643 | if (err) |
644 | goto fail1; |
645 | |
646 | gameport_set_poll_handler(gameport, handler: grip_poll); |
647 | gameport_set_poll_interval(gameport, msecs: 20); |
648 | |
649 | if (!multiport_init(grip)) { |
650 | err = -ENODEV; |
651 | goto fail2; |
652 | } |
653 | |
654 | if (!grip->port[0]->mode && !grip->port[1]->mode && !grip->port[2]->mode && !grip->port[3]->mode) { |
655 | /* nothing plugged in */ |
656 | err = -ENODEV; |
657 | goto fail2; |
658 | } |
659 | |
660 | return 0; |
661 | |
662 | fail2: gameport_close(gameport); |
663 | fail1: gameport_set_drvdata(gameport, NULL); |
664 | kfree(objp: grip); |
665 | return err; |
666 | } |
667 | |
668 | static void grip_disconnect(struct gameport *gameport) |
669 | { |
670 | struct grip_mp *grip = gameport_get_drvdata(gameport); |
671 | int i; |
672 | |
673 | for (i = 0; i < 4; i++) |
674 | if (grip->port[i]->registered) |
675 | input_unregister_device(grip->port[i]->dev); |
676 | gameport_close(gameport); |
677 | gameport_set_drvdata(gameport, NULL); |
678 | kfree(objp: grip); |
679 | } |
680 | |
681 | static struct gameport_driver grip_drv = { |
682 | .driver = { |
683 | .name = "grip_mp" , |
684 | }, |
685 | .description = DRIVER_DESC, |
686 | .connect = grip_connect, |
687 | .disconnect = grip_disconnect, |
688 | }; |
689 | |
690 | module_gameport_driver(grip_drv); |
691 | |