grip_mp.c

来自「底层驱动开发」· C语言 代码 · 共 678 行 · 第 1/2 页

	}
	return 0;
}

/*
 * Packet structure: B0-B15   => gamepad state
 *                   B16-B20  => gamepad device type
 *                   B21-B24  => multiport slot index (1-4)
 *
 * Known device types: 0x1f (grip pad), 0x0 (no device).  Others may exist.
 *
 * Returns the packet status.
 */

static int get_and_decode_packet(struct grip_mp *grip, int flags)
{
	u32 packet;
	int joytype = 0;
	int slot = 0;

	/* Get a packet and check for validity */

	flags &= IO_RESET | IO_RETRY;
	flags = multiport_io(grip->gameport, flags, 0, &packet);
	grip->reads++;

	if (packet & PACKET_MP_DONE)
		flags |= IO_DONE;

	if (flags && !(flags & IO_GOT_PACKET)) {
		grip->bads++;
		return flags;
	}

	/* Ignore non-gamepad packets, e.g. multiport hardware version */

	slot = ((packet >> 21) & 0xf) - 1;
	if ((slot < 0) || (slot > 3))
		return flags;

	/*
	 * Handle "reset" packets, which occur at startup, and when gamepads
	 * are removed or plugged in.  May contain configuration of a new gamepad.
	 */

	joytype = (packet >> 16) & 0x1f;
	if (!joytype) {

		if (grip->registered[slot]) {
			printk(KERN_INFO "grip_mp: removing %s, slot %d\n",
			       grip_name[grip->mode[slot]], slot);
			input_unregister_device(grip->dev + slot);
			grip->registered[slot] = 0;
		}
		dbg("Reset: grip multiport slot %d\n", slot);
		grip->mode[slot] = GRIP_MODE_RESET;
		flags |= IO_SLOT_CHANGE;
		return flags;
	}

	/* Interpret a grip pad packet */

	if (joytype == 0x1f) {

		int dir = (packet >> 8) & 0xf;          /* eight way directional value */
		grip->buttons[slot] = (~packet) & 0xff;
		grip->yaxes[slot] = ((axis_map[dir] >> 2) & 3) - 1;
		grip->xaxes[slot] = (axis_map[dir] & 3) - 1;
		grip->dirty[slot] = 1;

		if (grip->mode[slot] == GRIP_MODE_RESET)
			flags |= IO_SLOT_CHANGE;

		grip->mode[slot] = GRIP_MODE_GP;

		if (!grip->registered[slot]) {
			dbg("New Grip pad in multiport slot %d.\n", slot);
			register_slot(slot, grip);
		}
		return flags;
	}

	/* Handle non-grip device codes.  For now, just print diagnostics. */

	{
		static int strange_code = 0;
		if (strange_code != joytype) {
			printk(KERN_INFO "Possible non-grip pad/joystick detected.\n");
			printk(KERN_INFO "Got joy type 0x%x and packet 0x%x.\n", joytype, packet);
			strange_code = joytype;
		}
	}
	return flags;
}

/*
 * Returns true if all multiport slot states appear valid.
 */

static int slots_valid(struct grip_mp *grip)
{
	int flags, slot, invalid = 0, active = 0;

	flags = get_and_decode_packet(grip, 0);
	if (!(flags & IO_GOT_PACKET))
		return 0;

	for (slot = 0; slot < 4; slot++) {
		if (grip->mode[slot] == GRIP_MODE_RESET)
			invalid = 1;
		if (grip->mode[slot] != GRIP_MODE_NONE)
			active = 1;
	}

	/* Return true if no active slot but multiport sent all its data */
	if (!active)
		return (flags & IO_DONE) ? 1 : 0;

	/* Return false if invalid device code received */
	return invalid ? 0 : 1;
}

/*
 * Returns whether the multiport was placed into digital mode and
 * able to communicate its state successfully.
 */

static int multiport_init(struct grip_mp *grip)
{
	int dig_mode, initialized = 0, tries = 0;
	u32 packet;

	dig_mode = dig_mode_start(grip->gameport, &packet);
	while (!dig_mode && tries < 4) {
		dig_mode = dig_mode_start(grip->gameport, &packet);
		tries++;
	}

	if (dig_mode)
		dbg("multiport_init(): digital mode activated.\n");
	else {
		dbg("multiport_init(): unable to activate digital mode.\n");
		return 0;
	}

	/* Get packets, store multiport state, and check state's validity */
	for (tries = 0; tries < 4096; tries++) {
		if ( slots_valid(grip) ) {
			initialized = 1;
			break;
		}
	}
	dbg("multiport_init(): initialized == %d\n", initialized);
	return initialized;
}

/*
 * Reports joystick state to the linux input layer.
 */

static void report_slot(struct grip_mp *grip, int slot)
{
	struct input_dev *dev = &(grip->dev[slot]);
	int i, buttons = grip->buttons[slot];

	/* Store button states with linux input driver */

	for (i = 0; i < 8; i++)
		input_report_key(dev, grip_btn_gp[i], (buttons >> i) & 1);

	/* Store axis states with linux driver */

	input_report_abs(dev, ABS_X, grip->xaxes[slot]);
	input_report_abs(dev, ABS_Y, grip->yaxes[slot]);

	/* Tell the receiver of the events to process them */

	input_sync(dev);

	grip->dirty[slot] = 0;
}

/*
 * Get the multiport state.
 */

static void grip_poll(struct gameport *gameport)
{
	struct grip_mp *grip = gameport_get_drvdata(gameport);
	int i, npkts, flags;

	for (npkts = 0; npkts < 4; npkts++) {
		flags = IO_RETRY;
		for (i = 0; i < 32; i++) {
			flags = get_and_decode_packet(grip, flags);
			if ((flags & IO_GOT_PACKET) || !(flags & IO_RETRY))
				break;
		}
		if (flags & IO_DONE)
			break;
	}

	for (i = 0; i < 4; i++)
		if (grip->dirty[i])
			report_slot(grip, i);
}

/*
 * Called when a joystick device file is opened
 */

static int grip_open(struct input_dev *dev)
{
	struct grip_mp *grip = dev->private;

	gameport_start_polling(grip->gameport);
	return 0;
}

/*
 * Called when a joystick device file is closed
 */

static void grip_close(struct input_dev *dev)
{
	struct grip_mp *grip = dev->private;

	gameport_start_polling(grip->gameport);
}

/*
 * Tell the linux input layer about a newly plugged-in gamepad.
 */

static void register_slot(int slot, struct grip_mp *grip)
{
	int j, t;

	grip->dev[slot].private = grip;
	grip->dev[slot].open = grip_open;
	grip->dev[slot].close = grip_close;
	grip->dev[slot].name = grip_name[grip->mode[slot]];
	grip->dev[slot].id.bustype = BUS_GAMEPORT;
	grip->dev[slot].id.vendor = GAMEPORT_ID_VENDOR_GRAVIS;
	grip->dev[slot].id.product = 0x0100 + grip->mode[slot];
	grip->dev[slot].id.version = 0x0100;
	grip->dev[slot].evbit[0] = BIT(EV_KEY) | BIT(EV_ABS);

	for (j = 0; (t = grip_abs[grip->mode[slot]][j]) >= 0; j++)
		input_set_abs_params(&grip->dev[slot], t, -1, 1, 0, 0);

	for (j = 0; (t = grip_btn[grip->mode[slot]][j]) >= 0; j++)
		if (t > 0)
			set_bit(t, grip->dev[slot].keybit);

	input_register_device(grip->dev + slot);
	grip->registered[slot] = 1;

	if (grip->dirty[slot])	            /* report initial state, if any */
		report_slot(grip, slot);

	printk(KERN_INFO "grip_mp: added %s, slot %d\n",
	       grip_name[grip->mode[slot]], slot);
}

static int grip_connect(struct gameport *gameport, struct gameport_driver *drv)
{
	struct grip_mp *grip;
	int err;

	if (!(grip = kzalloc(sizeof(struct grip_mp), GFP_KERNEL)))
		return -ENOMEM;

	grip->gameport = gameport;

	gameport_set_drvdata(gameport, grip);

	err = gameport_open(gameport, drv, GAMEPORT_MODE_RAW);
	if (err)
		goto fail1;

	gameport_set_poll_handler(gameport, grip_poll);
	gameport_set_poll_interval(gameport, 20);

	if (!multiport_init(grip)) {
		err = -ENODEV;
		goto fail2;
	}

	if (!grip->mode[0] && !grip->mode[1] && !grip->mode[2] && !grip->mode[3]) {
		/* nothing plugged in */
		err = -ENODEV;
		goto fail2;
	}

	return 0;

fail2:	gameport_close(gameport);
fail1:	gameport_set_drvdata(gameport, NULL);
	kfree(grip);
	return err;
}

static void grip_disconnect(struct gameport *gameport)
{
	struct grip_mp *grip = gameport_get_drvdata(gameport);
	int i;

	for (i = 0; i < 4; i++)
		if (grip->registered[i])
			input_unregister_device(grip->dev + i);
	gameport_close(gameport);
	gameport_set_drvdata(gameport, NULL);
	kfree(grip);
}

static struct gameport_driver grip_drv = {
	.driver		= {
		.name	= "grip_mp",
	},
	.description	= DRIVER_DESC,
	.connect	= grip_connect,
	.disconnect	= grip_disconnect,
};

static int __init grip_init(void)
{
	gameport_register_driver(&grip_drv);
	return 0;
}

static void __exit grip_exit(void)
{
	gameport_unregister_driver(&grip_drv);
}

module_init(grip_init);
module_exit(grip_exit);