1 files changed, 137 insertions, 75 deletions

diff --git a/drivers/input/joystick/iforce/iforce-serio.c b/drivers/input/joystick/iforce/iforce-serio.c
index 46d5041d2d9d..75b85c46dfa4 100644
--- a/drivers/input/joystick/iforce/iforce-serio.c
+++ b/drivers/input/joystick/iforce/iforce-serio.c
@@ -1,3 +1,4 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
 /*
  *  Copyright (c) 2000-2001 Vojtech Pavlik <vojtech@ucw.cz>
  *  Copyright (c) 2001, 2007 Johann Deneux <johann.deneux@gmail.com>
@@ -5,120 +6,175 @@
  *  USB/RS232 I-Force joysticks and wheels.
  */
 
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Should you need to contact me, the author, you can do so either by
- * e-mail - mail your message to <vojtech@ucw.cz>, or by paper mail:
- * Vojtech Pavlik, Simunkova 1594, Prague 8, 182 00 Czech Republic
- */
-
+#include <linux/serio.h>
 #include "iforce.h"
 
-void iforce_serial_xmit(struct iforce *iforce)
+struct iforce_serio {
+	struct iforce iforce;
+
+	struct serio *serio;
+	int idx, pkt, len, id;
+	u8 csum;
+	u8 expect_packet;
+	u8 cmd_response[IFORCE_MAX_LENGTH];
+	u8 cmd_response_len;
+	u8 data_in[IFORCE_MAX_LENGTH];
+};
+
+static void iforce_serio_xmit(struct iforce *iforce)
 {
+	struct iforce_serio *iforce_serio = container_of(iforce,
+							 struct iforce_serio,
+							 iforce);
 	unsigned char cs;
 	int i;
-	unsigned long flags;
 
 	if (test_and_set_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags)) {
 		set_bit(IFORCE_XMIT_AGAIN, iforce->xmit_flags);
 		return;
 	}
 
-	spin_lock_irqsave(&iforce->xmit_lock, flags);
-
-again:
-	if (iforce->xmit.head == iforce->xmit.tail) {
-		clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
-		spin_unlock_irqrestore(&iforce->xmit_lock, flags);
-		return;
-	}
+	guard(spinlock_irqsave)(&iforce->xmit_lock);
 
-	cs = 0x2b;
+	do {
+		if (iforce->xmit.head == iforce->xmit.tail)
+			break;
 
-	serio_write(iforce->serio, 0x2b);
+		cs = 0x2b;
 
-	serio_write(iforce->serio, iforce->xmit.buf[iforce->xmit.tail]);
-	cs ^= iforce->xmit.buf[iforce->xmit.tail];
-	XMIT_INC(iforce->xmit.tail, 1);
+		serio_write(iforce_serio->serio, 0x2b);
 
-	for (i=iforce->xmit.buf[iforce->xmit.tail]; i >= 0; --i) {
-		serio_write(iforce->serio, iforce->xmit.buf[iforce->xmit.tail]);
+		serio_write(iforce_serio->serio,
+			    iforce->xmit.buf[iforce->xmit.tail]);
 		cs ^= iforce->xmit.buf[iforce->xmit.tail];
 		XMIT_INC(iforce->xmit.tail, 1);
+
+		for (i = iforce->xmit.buf[iforce->xmit.tail]; i >= 0; --i) {
+			serio_write(iforce_serio->serio,
+				    iforce->xmit.buf[iforce->xmit.tail]);
+			cs ^= iforce->xmit.buf[iforce->xmit.tail];
+			XMIT_INC(iforce->xmit.tail, 1);
+		}
+
+		serio_write(iforce_serio->serio, cs);
+
+	} while (test_and_clear_bit(IFORCE_XMIT_AGAIN, iforce->xmit_flags));
+
+	iforce_clear_xmit_and_wake(iforce);
+}
+
+static int iforce_serio_get_id(struct iforce *iforce, u8 id,
+			       u8 *response_data, size_t *response_len)
+{
+	struct iforce_serio *iforce_serio = container_of(iforce,
+							 struct iforce_serio,
+							 iforce);
+
+	iforce_serio->expect_packet = HI(FF_CMD_QUERY);
+	iforce_serio->cmd_response_len = 0;
+
+	iforce_send_packet(iforce, FF_CMD_QUERY, &id);
+
+	wait_event_interruptible_timeout(iforce->wait,
+					 !iforce_serio->expect_packet, HZ);
+
+	if (iforce_serio->expect_packet) {
+		iforce_serio->expect_packet = 0;
+		return -ETIMEDOUT;
 	}
 
-	serio_write(iforce->serio, cs);
+	if (iforce_serio->cmd_response[0] != id)
+		return -EIO;
 
-	if (test_and_clear_bit(IFORCE_XMIT_AGAIN, iforce->xmit_flags))
-		goto again;
+	memcpy(response_data, iforce_serio->cmd_response,
+	       iforce_serio->cmd_response_len);
+	*response_len = iforce_serio->cmd_response_len;
 
-	clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
+	return 0;
+}
 
-	spin_unlock_irqrestore(&iforce->xmit_lock, flags);
+static int iforce_serio_start_io(struct iforce *iforce)
+{
+	/* No special handling required */
+	return 0;
 }
 
+static void iforce_serio_stop_io(struct iforce *iforce)
+{
+	//TODO: Wait for the last packets to be sent
+}
+
+static const struct iforce_xport_ops iforce_serio_xport_ops = {
+	.xmit		= iforce_serio_xmit,
+	.get_id		= iforce_serio_get_id,
+	.start_io	= iforce_serio_start_io,
+	.stop_io	= iforce_serio_stop_io,
+};
+
 static void iforce_serio_write_wakeup(struct serio *serio)
 {
 	struct iforce *iforce = serio_get_drvdata(serio);
 
-	iforce_serial_xmit(iforce);
+	iforce_serio_xmit(iforce);
 }
 
 static irqreturn_t iforce_serio_irq(struct serio *serio,
-		unsigned char data, unsigned int flags)
+				    unsigned char data, unsigned int flags)
 {
-	struct iforce *iforce = serio_get_drvdata(serio);
+	struct iforce_serio *iforce_serio = serio_get_drvdata(serio);
+	struct iforce *iforce = &iforce_serio->iforce;
 
-	if (!iforce->pkt) {
+	if (!iforce_serio->pkt) {
 		if (data == 0x2b)
-			iforce->pkt = 1;
+			iforce_serio->pkt = 1;
 		goto out;
 	}
 
-	if (!iforce->id) {
+	if (!iforce_serio->id) {
 		if (data > 3 && data != 0xff)
-			iforce->pkt = 0;
+			iforce_serio->pkt = 0;
 		else
-			iforce->id = data;
+			iforce_serio->id = data;
 		goto out;
 	}
 
-	if (!iforce->len) {
+	if (!iforce_serio->len) {
 		if (data > IFORCE_MAX_LENGTH) {
-			iforce->pkt = 0;
-			iforce->id = 0;
+			iforce_serio->pkt = 0;
+			iforce_serio->id = 0;
 		} else {
-			iforce->len = data;
+			iforce_serio->len = data;
 		}
 		goto out;
 	}
 
-	if (iforce->idx < iforce->len) {
-		iforce->csum += iforce->data[iforce->idx++] = data;
+	if (iforce_serio->idx < iforce_serio->len) {
+		iforce_serio->data_in[iforce_serio->idx++] = data;
+		iforce_serio->csum += data;
 		goto out;
 	}
 
-	if (iforce->idx == iforce->len) {
-		iforce_process_packet(iforce, (iforce->id << 8) | iforce->idx, iforce->data);
-		iforce->pkt = 0;
-		iforce->id  = 0;
-		iforce->len = 0;
-		iforce->idx = 0;
-		iforce->csum = 0;
+	if (iforce_serio->idx == iforce_serio->len) {
+		/* Handle command completion */
+		if (iforce_serio->expect_packet == iforce_serio->id) {
+			iforce_serio->expect_packet = 0;
+			memcpy(iforce_serio->cmd_response,
+			       iforce_serio->data_in, IFORCE_MAX_LENGTH);
+			iforce_serio->cmd_response_len = iforce_serio->len;
+
+			/* Signal that command is done */
+			wake_up_all(&iforce->wait);
+		} else if (likely(iforce->type)) {
+			iforce_process_packet(iforce, iforce_serio->id,
+					      iforce_serio->data_in,
+					      iforce_serio->len);
+		}
+
+		iforce_serio->pkt = 0;
+		iforce_serio->id  = 0;
+		iforce_serio->len = 0;
+		iforce_serio->idx = 0;
+		iforce_serio->csum = 0;
 	}
 out:
 	return IRQ_HANDLED;
@@ -126,23 +182,23 @@ out:
 
 static int iforce_serio_connect(struct serio *serio, struct serio_driver *drv)
 {
-	struct iforce *iforce;
+	struct iforce_serio *iforce_serio;
 	int err;
 
-	iforce = kzalloc(sizeof(struct iforce), GFP_KERNEL);
-	if (!iforce)
+	iforce_serio = kzalloc(sizeof(*iforce_serio), GFP_KERNEL);
+	if (!iforce_serio)
 		return -ENOMEM;
 
-	iforce->bus = IFORCE_232;
-	iforce->serio = serio;
+	iforce_serio->iforce.xport_ops = &iforce_serio_xport_ops;
 
-	serio_set_drvdata(serio, iforce);
+	iforce_serio->serio = serio;
+	serio_set_drvdata(serio, iforce_serio);
 
 	err = serio_open(serio, drv);
 	if (err)
 		goto fail1;
 
-	err = iforce_init_device(iforce);
+	err = iforce_init_device(&serio->dev, BUS_RS232, &iforce_serio->iforce);
 	if (err)
 		goto fail2;
 
@@ -150,21 +206,21 @@ static int iforce_serio_connect(struct serio *serio, struct serio_driver *drv)
 
  fail2:	serio_close(serio);
  fail1:	serio_set_drvdata(serio, NULL);
-	kfree(iforce);
+	kfree(iforce_serio);
 	return err;
 }
 
 static void iforce_serio_disconnect(struct serio *serio)
 {
-	struct iforce *iforce = serio_get_drvdata(serio);
+	struct iforce_serio *iforce_serio = serio_get_drvdata(serio);
 
-	input_unregister_device(iforce->dev);
+	input_unregister_device(iforce_serio->iforce.dev);
 	serio_close(serio);
 	serio_set_drvdata(serio, NULL);
-	kfree(iforce);
+	kfree(iforce_serio);
 }
 
-static struct serio_device_id iforce_serio_ids[] = {
+static const struct serio_device_id iforce_serio_ids[] = {
 	{
 		.type	= SERIO_RS232,
 		.proto	= SERIO_IFORCE,
@@ -187,3 +243,9 @@ struct serio_driver iforce_serio_drv = {
 	.connect	= iforce_serio_connect,
 	.disconnect	= iforce_serio_disconnect,
 };
+
+module_serio_driver(iforce_serio_drv);
+
+MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>, Johann Deneux <johann.deneux@gmail.com>");
+MODULE_DESCRIPTION("RS232 I-Force joysticks and wheels driver");
+MODULE_LICENSE("GPL");