elantech: implement data check for 6-byte protocol

index badb2ea..8f0a1ee 100644
@@ -348,6 +348,57 @@ static int elantech_check_parity_v1(struct psmouse *psmouse)
 	       etd->parity[packet[3]] == p3;
 }
 
+static int elantech_check_parity_ef113(struct psmouse *psmouse)
+{
+	unsigned char *packet = psmouse->packet;
+
+	/* Consistency checks as in the Dell/Ubuntu driver */
+	return !((((packet[0] & 0x3c) != 0x3c) && ((packet[0] & 0xc0) != 0x80)) ||
+		 (((packet[0] & 0x0c) != 0x0c) && ((packet[0] & 0xc0) == 0x80)) ||
+		 (((packet[0] & 0xc0) != 0x80) && ((packet[1] & 0xf0) != 0x00)) ||
+		 (((packet[3] & 0x3e) != 0x38) && ((packet[0] & 0xc0) != 0x80)) ||
+		 (((packet[3] & 0x0e) != 0x08) && ((packet[0] & 0xc0) == 0x80)) ||
+		 (((packet[0] & 0xc0) != 0x80) && ((packet[4] & 0xf0) != 0x00)));
+}
+
+static int elantech_check_parity_const(struct psmouse *psmouse)
+{
+	unsigned char *packet = psmouse->packet;
+
+	/* Some bits which are constant in the 6 packets */
+	return (((packet[0] & 0x0c) == 0x04) && ((packet[3] & 0x0f) == 0x02));
+}
+
+/*
+ * Returns 0 if an error is detected in the packets.
+ */
+static int elantech_check_parity(struct psmouse *psmouse)
+{
+	struct elantech_data *etd = psmouse->private;
+
+	switch (etd->paritycheck) {
+	case ETP_FULL_PC:
+		return elantech_check_parity_v1(psmouse);
+	case ETP_EF113_CHECK:
+		return elantech_check_parity_ef113(psmouse);
+	case ETP_CONST_CHECK:
+		return elantech_check_parity_const(psmouse);
+	case ETP_NOT_CHECK:
+	default:
+		return 1;
+	}
+}
+
+static void elantech_shift_packets(struct psmouse *psmouse)
+{
+	unsigned char *packet = psmouse->packet;
+	int i;
+
+	for (i = 0; i < (psmouse->pktsize - 1); i++)
+		packet[i] = packet[i + 1];
+	psmouse->pktcnt--;
+}
+
 /*
  * Process byte stream from mouse and handle complete packets
  */

@@ -361,16 +412,18 @@ static psmouse_ret_t elantech_process_byte(struct psmouse *psmouse)
 	if (etd->debug > 1)
 		elantech_packet_dump(psmouse->packet, psmouse->pktsize);
 
+	if (!elantech_check_parity(psmouse)) {
+		/* Try to re-synchronize */
+		elantech_shift_packets(psmouse);
+		return PSMOUSE_GOOD_DATA;
+	}
+
 	switch (etd->hw_version) {
 	case 1:
-		if (etd->paritycheck && !elantech_check_parity_v1(psmouse))
-			return PSMOUSE_BAD_DATA;
-
 		elantech_report_absolute_v1(psmouse);
 		break;
 
 	case 2:
-		/* We don't know how to check parity in protocol v2 */
 		elantech_report_absolute_v2(psmouse);
 		break;
 	}