Orange Pi5 kernel

Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards

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