fix(gamepad): add delayed authentication for PDP

bus/bus.h

@@ -56,6 +56,7 @@ struct gip_adapter {
 	spinlock_t send_lock;
 
 	u8 data_sequence;
+  u8 auth_sequence;
 	u8 audio_sequence;
 };
 

bus/protocol.c

@@ -323,6 +323,17 @@ static int gip_decode_header(struct gip_header *hdr, u8 *data, int len)
 	return hdr_len;
 }
 
+static int gip_has_capability(struct gip_info_element *caps, u8 command)
+{
+  for(int i=0; i<caps->count; i++) {
+    if (caps->data[i] == command) {
+      return 0;
+    }
+  }
+  return -ENOTSUPP;
+}
+
+
 static int gip_init_chunk_buffer(struct gip_client *client,
 				 struct gip_header *hdr,
 				 struct gip_chunk_buffer **buf,
@@ -389,6 +400,15 @@ static int gip_send_pkt_simple(struct gip_client *client,
 	/* set actual length */
 	buf.length = hdr_len + hdr->packet_length;
 
+	gip_dbg(client, "%s:  seq=0x%02x, cmd=0x%02x, len=0x%04x flags=[%s %s %s] pkt=[%*ph]\n",
+		__func__,
+    hdr->sequence,
+		hdr->command, hdr->packet_length,
+		hdr->options & GIP_OPT_CHUNK_START? "Ini":"...",
+		hdr->options & GIP_OPT_ACKNOWLEDGE? "Ack":"...",
+		hdr->options & GIP_OPT_INTERNAL? "Sys":"...",
+    buf.length,buf.data);
+
 	/* debug message sent */
 	// gip_dbg(client, "%s: cmd=0x%02x len=0x%04x seq=0x%02x offset=0x%04x\n",
 	// 	__func__, hdr->command, buf.length, hdr->sequence,
@@ -455,8 +475,8 @@ static int gip_acknowledge_pkt(struct gip_client *client,
 	if ((ack->options & GIP_OPT_CHUNK) && buf)
 		pkt.remaining = cpu_to_le16(buf->length - len);
 
-	// gip_dbg(client, "%s: ACME(host) command=0x%02x, length=0x%04x\n",
-	// 	__func__, pkt.command, len);
+	gip_dbg(client, "%s:  seq=0x%02x, cmd=0x%02x, len=0x%04x ACME(host)\n",
+	        __func__, hdr.sequence, pkt.command, len);
 
 	return gip_send_pkt(client, &hdr, &pkt);
 }
@@ -495,6 +515,12 @@ int gip_send_authenticate(struct gip_client *client, void *pkt, u32 len,
 	hdr.options = client->id | GIP_OPT_INTERNAL;
 	hdr.packet_length = len;
 
+  /* sequence number is always greater than zero */
+  if (!++client->adapter->auth_sequence)
+    ++client->adapter->auth_sequence;
+
+  hdr.sequence = client->adapter->auth_sequence;
+
 	if (acknowledge)
 		hdr.options |= GIP_OPT_ACKNOWLEDGE;
 
@@ -611,6 +637,11 @@ EXPORT_SYMBOL_GPL(gip_set_led_mode);
 
 int gip_send_get_serial_number(struct gip_client *client)
 {
+  int ierr = gip_has_capability(client->capabilities_out, GIP_CMD_EXTENDED);
+  if (ierr) {
+    gip_warn(client,"%s: no GIP_OPT_INTERNAL capability, skipping message.",__func__);
+    return ierr;
+  }
 	struct gip_header hdr = {
 		.command = GIP_CMD_EXTENDED,
 		.options = client->id | GIP_OPT_INTERNAL,
@@ -1118,11 +1149,15 @@ static int gip_handle_pkt_acknowledge(struct gip_client *client,
 	if (len != sizeof(*pkt))
 		return -EINVAL;
 
-	if (!buf)
+	if (!buf) {
+    gip_dbg(client, "%s:     cmd=0x%02x, len=0x%04x ACME(dev)\n",
+      __func__, pkt->command, le16_to_cpu(pkt->length));
+
 		return 0;
+  }
 
-	gip_dbg(client, "%s: ACME(dev) cmd=0x%02x/0x%02x, len=0x%04x/0x%04x\n",
-		__func__, pkt->command, buf->header.command,
+	gip_dbg(client, "%s: seq=0x%02x, cmd=0x%02x, len=0x%04x/0x%04x ACME(dev)\n",
+		__func__, buf->header.sequence, pkt->command, 
 		le16_to_cpu(pkt->length), buf->length);
 
 	/* acknowledgment for different command */
@@ -1609,11 +1644,8 @@ static int gip_process_pkt_chunked(struct gip_client *client,
 	int err;
 	u32 len;
 
-	gip_dbg(client, "%s: flags=[%s %s %s], offset=0x%04x, length=0x%04x\n",
+	gip_dbg(client, "%s: offset=0x%04x, length=0x%04x\n",
 		__func__,
-		hdr->options & GIP_OPT_CHUNK_START? "Ini":"...",
-		hdr->options & GIP_OPT_ACKNOWLEDGE? "Ack":"...",
-		hdr->options & GIP_OPT_INTERNAL? "Sys":"...",
 		hdr->chunk_offset, hdr->packet_length);
 
 	if (!buf) {
@@ -1674,6 +1706,7 @@ static int gip_process_pkt(struct gip_client *client,
 		hdr->chunk_offset = 0;
 	}
 
+	/* some gamepads send empty packets with chunk flag : dispatch them */
 	if (hdr->options & GIP_OPT_CHUNK)
 		return gip_process_pkt_chunked(client, hdr, data);
 
@@ -1701,6 +1734,15 @@ int gip_process_buffer(struct gip_adapter *adap, void *data, int len)
 		if (IS_ERR(client))
 			return PTR_ERR(client);
 
+    gip_dbg(client, "%s:   seq=0x%02x, cmd=0x%02x, len=0x%04x flags=[%s %s %s] data=[%*ph]\n",
+      __func__,
+      hdr.sequence,
+      hdr.command, hdr.packet_length,
+      hdr.options & GIP_OPT_CHUNK_START? "Ini":"...",
+      hdr.options & GIP_OPT_ACKNOWLEDGE? "Ack":"...",
+      hdr.options & GIP_OPT_INTERNAL? "Sys":"...",
+      len,data);
+
 		err = gip_process_pkt(client, &hdr, data + hdr_len);
 		if (err)
 			return err;

driver/gamepad.c

@@ -27,6 +27,7 @@
 
 #define GIP_GP_RUMBLE_DELAY msecs_to_jiffies(10)
 #define GIP_GP_RUMBLE_MAX 100
+#define GIP_GP_AUTH_DELAY msecs_to_jiffies(50)
 
 /* button offset from end of packet */
 #define GIP_GP_BTN_SHARE_OFFSET 18
@@ -78,6 +79,11 @@ enum gip_gamepad_motor {
 	GIP_GP_MOTOR_LT = BIT(3),
 };
 
+enum gip_init_state {
+	GIP_GP_HANDSHAKE = 0x10,
+	GIP_GP_AUTHENTICATING = 0x20,
+	GIP_GP_READY = 0xFF,
+};
 /*
  * Remember, xpad keeps the 4 bytes.
  * Paddles are at [18] in xpad, so, [14] here.
@@ -141,11 +147,17 @@ struct gip_gamepad {
 	struct gip_led led;
 	struct gip_input input;
 
+  u8 state;
+  void *auth_pkt;
+  u32 auth_pkt_sz;
+
 	bool supports_share;
 	bool supports_dli;
 	PaddleCapability paddle_support;
 
 	struct gip_gamepad_rumble rumble;
+
+  struct delayed_work state_work;
 };
 
 static void gip_gamepad_send_rumble(struct timer_list *timer)
@@ -296,29 +308,30 @@ static int gip_gamepad_init_input(struct gip_gamepad *gamepad)
 	input_set_abs_params(dev, ABS_HAT0X, -1, 1, 0, 0);
 	input_set_abs_params(dev, ABS_HAT0Y, -1, 1, 0, 0);
 
-	err = gip_gamepad_init_rumble(gamepad);
+/*	err = gip_gamepad_init_rumble(gamepad);
 	if (err) {
 		dev_err(&gamepad->client->dev, "%s: init rumble failed: %d\n",
 			__func__, err);
 		goto err_delete_timer;
 	}
-
+*/
 	err = input_register_device(dev);
 	if (err) {
 		dev_err(&gamepad->client->dev, "%s: register failed: %d\n",
 			__func__, err);
-		goto err_delete_timer;
+/*		goto err_delete_timer;*/
+    return err;
 	}
 
 	return 0;
-
+/*
 err_delete_timer:
 #if LINUX_VERSION_CODE < KERNEL_VERSION(6,15,0)
 	del_timer_sync(&gamepad->rumble.timer);
 #else
 	timer_delete_sync(&gamepad->rumble.timer);
 #endif
-	return err;
+	return err;*/
 }
 
 static int gip_gamepad_op_battery(struct gip_client *client,
@@ -329,6 +342,13 @@ static int gip_gamepad_op_battery(struct gip_client *client,
 
 	gip_report_battery(&gamepad->battery, type, level);
 
+  // handle pdp gamepad that need delayed authentication
+  if (gamepad->state == GIP_GP_HANDSHAKE)
+  {
+  	dev_dbg(&gamepad->client->dev, "%s: before handshake (delayed).\n", __func__);
+    schedule_delayed_work(&gamepad->state_work, GIP_GP_AUTH_DELAY);
+  }
+
 	return 0;
 }
 
@@ -337,7 +357,15 @@ static int gip_gamepad_op_authenticate(struct gip_client *client,
 {
 	struct gip_gamepad *gamepad = dev_get_drvdata(&client->dev);
 
-	return gip_auth_process_pkt(&gamepad->auth, data, len);
+	gamepad->auth_pkt = kzalloc(len, GFP_ATOMIC);
+	if (!gamepad->auth_pkt)
+		return -ENOMEM;
+
+  memcpy(gamepad->auth_pkt, data, len);
+  gamepad->auth_pkt_sz = len;
+  schedule_delayed_work(&gamepad->state_work, GIP_GP_AUTH_DELAY);
+
+  return 0;
 }
 
 static int gip_gamepad_op_guide_button(struct gip_client *client, bool down)
@@ -352,9 +380,63 @@ static int gip_gamepad_op_guide_button(struct gip_client *client, bool down)
 
 static int gip_gamepad_op_authenticated(struct gip_client *client)
 {
+  struct gip_gamepad *gamepad = dev_get_drvdata(&client->dev);
+  /*
+	int err = gip_gamepad_init_input(gamepad);
+	if (err)
+		return err;
+  */
+  gamepad->state = GIP_GP_READY;
+	int err = gip_gamepad_init_rumble(gamepad);
+	if (err) {
+		dev_err(&gamepad->client->dev, "%s: init rumble failed: %d\n",
+			__func__, err);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6,15,0)
+    del_timer_sync(&gamepad->rumble.timer);
+#else
+    timer_delete_sync(&gamepad->rumble.timer);
+#endif
+		return err;
+	}
 	return 0;
 }
 
+static void gip_gamepad_start_handshake(struct work_struct *work)
+{
+	struct gip_gamepad *gamepad = container_of(to_delayed_work(work),
+	                                           struct gip_gamepad, state_work);
+  if(gamepad->state == GIP_GP_HANDSHAKE)
+  {
+    gamepad->state = GIP_GP_AUTHENTICATING;
+    int err = gip_auth_start_handshake(&gamepad->auth, gamepad->client);
+    if (err) {
+      dev_err(&gamepad->client->dev, "%s: gamepad handshake failed err=%d.\n", __func__, err);
+      return;
+    }
+  }
+  else if(gamepad->state == GIP_GP_AUTHENTICATING)
+  {
+    if (!gamepad->auth_pkt)
+    {
+      dev_err(&gamepad->client->dev, "%s: gamepad delayed auth msg has no buffer allocated.\n",
+              __func__);
+      return;
+    }
+
+    int err = gip_auth_process_pkt(&gamepad->auth, gamepad->auth_pkt, gamepad->auth_pkt_sz);
+    if (err)
+    {
+      dev_err(&gamepad->client->dev, "%s: gamepad auth command failed err=%d.\n", __func__, err);
+    }
+
+    kfree(gamepad->auth_pkt);
+    gamepad->auth_pkt = NULL;
+    gamepad->auth_pkt_sz = 0;
+
+    return;
+  }
+}
+
 static int gip_gamepad_op_firmware(struct gip_client *client, void *data,
 				   u32 len)
 {
@@ -485,6 +567,10 @@ static int gip_gamepad_probe(struct gip_client *client)
 	if (!gamepad)
 		return -ENOMEM;
 
+
+  INIT_DELAYED_WORK(&gamepad->state_work, gip_gamepad_start_handshake);
+  gamepad->state = GIP_GP_HANDSHAKE;
+
 	gamepad->client = client;
 
 	err = gip_set_power_mode(client, GIP_PWR_ON);
@@ -512,18 +598,22 @@ static int gip_gamepad_probe(struct gip_client *client)
 	if (err)
 		return err;
 
-	err = gip_auth_start_handshake(&gamepad->auth, client);
+  dev_dbg(&gamepad->client->dev, "%s: before gip_init_input.\n", __func__);
+	err = gip_init_input(&gamepad->input, client, GIP_GP_NAME);
 	if (err)
 		return err;
 
-	err = gip_init_input(&gamepad->input, client, GIP_GP_NAME);
-	if (err)
+/*  dev_dbg(&gamepad->client->dev, "%s: before handshake.\n", __func__);
+	err = gip_auth_start_handshake(&gamepad->auth, gamepad->client);
+	if (err) {
+		dev_dbg(&gamepad->client->dev, "%s: gamepad handshake failed err=%d.\n", __func__, err);
 		return err;
+	}*/
 
 	err = gip_gamepad_init_input(gamepad);
 	if (err)
 		return err;
-
+  
 	dev_set_drvdata(&client->dev, gamepad);
 
 	return 0;
@@ -532,6 +622,7 @@ static int gip_gamepad_probe(struct gip_client *client)
 static void gip_gamepad_remove(struct gip_client *client)
 {
 	struct gip_gamepad *gamepad = dev_get_drvdata(&client->dev);
+  cancel_delayed_work_sync(&gamepad->state_work);
 
 #if LINUX_VERSION_CODE < KERNEL_VERSION(6,15,0)
 	del_timer_sync(&gamepad->rumble.timer);