diff mbox series

Input: synaptics-rmi4 - filter incomplete relative packet.

Message ID 1659944641-2625-1-git-send-email-marge.yang@synaptics.corp-partner.google.com (mailing list archive)
State Superseded
Headers show
Series Input: synaptics-rmi4 - filter incomplete relative packet. | expand

Commit Message

margeyang Aug. 8, 2022, 7:44 a.m. UTC
From: Marge Yang <marge.yang@synaptics.corp-partner.google.com>

RMI4 F03 supports the Stick function,
it's designed to support relative packet.
This patch supports the following case.
When relative packet can't be reported completely,
it may miss one byte or two byte.
New Synaptics firmware will report PARITY error.
When timeout error or parity error happens,
RMI4 driver will sends 0xFE command and
ask FW to Re-send stick packet again.

Signed-off-by: Marge Yang<marge.yang@synaptics.corp-partner.google.com>
---
 drivers/input/rmi4/rmi_f03.c | 82 ++++++++++++++++++++++++++++++++++--
 1 file changed, 78 insertions(+), 4 deletions(-)

Comments

Hans de Goede Aug. 11, 2022, 2:23 p.m. UTC | #1
Hi,

On 8/8/22 09:44, margeyang wrote:
> From: Marge Yang <marge.yang@synaptics.corp-partner.google.com>
> 
> RMI4 F03 supports the Stick function,
> it's designed to support relative packet.
> This patch supports the following case.
> When relative packet can't be reported completely,
> it may miss one byte or two byte.
> New Synaptics firmware will report PARITY error.
> When timeout error or parity error happens,
> RMI4 driver will sends 0xFE command and
> ask FW to Re-send stick packet again.
> 
> Signed-off-by: Marge Yang<marge.yang@synaptics.corp-partner.google.com>
> ---
>  drivers/input/rmi4/rmi_f03.c | 82 ++++++++++++++++++++++++++++++++++--
>  1 file changed, 78 insertions(+), 4 deletions(-)
> 
> diff --git a/drivers/input/rmi4/rmi_f03.c b/drivers/input/rmi4/rmi_f03.c
> index c194b1664b10..57f03dfcb4ff 100644
> --- a/drivers/input/rmi4/rmi_f03.c
> +++ b/drivers/input/rmi4/rmi_f03.c
> @@ -23,8 +23,12 @@
>  #define RMI_F03_BYTES_PER_DEVICE_SHIFT	4
>  #define RMI_F03_QUEUE_LENGTH		0x0F
>  
> +#define RMI_F03_RESET_STYK 0xFE

Please use tabs in front of the 0xFE to align it with the other values.

> +
>  #define PSMOUSE_OOB_EXTRA_BTNS		0x01
>  
> +#define RELATIVE_PACKET_SIZE		0x03

Just "3" please since this is a size (not a register value).

> +
>  struct f03_data {
>  	struct rmi_function *fn;
>  
> @@ -36,7 +40,8 @@ struct f03_data {
>  	u8 device_count;
>  	u8 rx_queue_length;
>  };
> -
> +int iwritecommandcounter;
> +unsigned int ipacketindex;

Please do not use global variables like this, instead store these
e.g. inside struct f03_data.

>  int rmi_f03_overwrite_button(struct rmi_function *fn, unsigned int button,
>  			     int value)
>  {
> @@ -87,7 +92,7 @@ static int rmi_f03_pt_write(struct serio *id, unsigned char val)
>  			__func__, error);
>  		return error;
>  	}
> -
> +	iwritecommandcounter++;
>  	return 0;
>  }
>  
> @@ -197,10 +202,12 @@ static int rmi_f03_register_pt(struct f03_data *f03)
>  
>  static int rmi_f03_probe(struct rmi_function *fn)
>  {
> +
>  	struct device *dev = &fn->dev;
>  	struct f03_data *f03;
>  	int error;
> -
> +	iwritecommandcounter = 0;
> +	ipacketindex = 0;
>  	f03 = devm_kzalloc(dev, sizeof(struct f03_data), GFP_KERNEL);
>  	if (!f03)
>  		return -ENOMEM;

If you put the 2 variables into the f03_data then there will be no need
to zero them.

> @@ -251,9 +258,12 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>  	const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
>  	u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
>  	u8 ob_status;
> +	static u8 ob_dataArry[RELATIVE_PACKET_SIZE];
>  	u8 ob_data;
>  	unsigned int serio_flags;
> +	static unsigned int serio_flagsArry[RELATIVE_PACKET_SIZE];

Please drop these 2 static arrays here and instead store the info in
the f03_data struct.

>  	int i;
> +

Unrelated whitespace change, please drop.

>  	int error;
>  
>  	if (drvdata->attn_data.data) {
> @@ -284,6 +294,22 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>  		ob_data = obs[i + RMI_F03_OB_DATA_OFFSET];
>  		serio_flags = 0;
>  
> +		if (ob_status & (RMI_F03_OB_FLAG_TIMEOUT | RMI_F03_OB_FLAG_PARITY)) {
> +			//  Send resend command to stick when timeout or parity error.
> +			//  Driver can receive the last stick packet.
> +
> +			error = rmi_write(f03->fn->rmi_dev, f03->fn->fd.data_base_addr,
> +			 RMI_F03_RESET_STYK);
> +			if (error) {
> +				dev_err(&f03->fn->dev,
> +					"%s: Failed to rmi_write to F03 TX register (%d).\n",
> +					__func__, error);
> +				return error;
> +			}
> +			ipacketindex = 0;
> +			break;
> +		}
> +
>  		if (!(ob_status & RMI_F03_RX_DATA_OFB))
>  			continue;
>  
> @@ -298,9 +324,57 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>  			serio_flags & SERIO_TIMEOUT ?  'Y' : 'N',
>  			serio_flags & SERIO_PARITY ? 'Y' : 'N');
>  
> -		serio_interrupt(f03->serio, ob_data, serio_flags);
> +		if (iwritecommandcounter > 0) {
> +			// Read Acknowledge Byte after writing the PS2 command.
> +			// It is not trackpoint data.
> +			serio_interrupt(f03->serio, ob_data, serio_flags);
> +
> +		} else {
> +			//   The relative-mode PS/2 packet format is as follows:
> +			//
> +			//              bit position            position (as array of bytes)
> +			//     7   6   5   4   3   2   1   0
> +			//   =================================+
> +			//    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
> +			//                DX[7:0]             | DATA[1]
> +			//                DY[7:0]             | DATA[2]
> +			//   =================================+
> +			//		Yov: Y overflow
> +			//    Xov: X overflow
> +			if ((ipacketindex == 0) && (ob_data & ((BIT(7)|BIT(6))))) {
> +				dev_err(&f03->fn->dev,
> +				"%s: X or Y is overflow. (%x)\n",
> +				__func__, ob_data);
> +				break;
> +			} else if ((ipacketindex == 0) && !(ob_data & BIT(3))) {
> +				dev_err(&f03->fn->dev,
> +				"%s: New BIT 3 is not 1 for the first byte\n",
> +				__func__);

Why no break; here like above ?

> +			} else {
> +				if (ipacketindex >= RELATIVE_PACKET_SIZE) {
> +					ipacketindex = 0;

This means that you are skipping every 4th byte, since you only store
the ob_data + serio_flags the next cycle through the loop!

> +				} else {
> +					ob_dataArry[ipacketindex] = ob_data;
> +					serio_flagsArry[ipacketindex] = serio_flags;
> +					ipacketindex++;
> +				}
> +				if (ipacketindex == RELATIVE_PACKET_SIZE)	{
> +					serio_interrupt(f03->serio, ob_dataArry[0],
> +					 serio_flagsArry[0]);
> +					serio_interrupt(f03->serio, ob_dataArry[1],
> +					 serio_flagsArry[1]);
> +					serio_interrupt(f03->serio, ob_dataArry[2],
> +					 serio_flagsArry[2]);
> +					ipacketindex = 0;
> +				}
> +			}
> +		}
>  	}
>  
> +	if (iwritecommandcounter > 0) {
> +		ipacketindex = 0;
> +		iwritecommandcounter = iwritecommandcounter - 1;
> +	}

You check iwritecommandcounter inside the:

        for (i = 0; i < ob_len; i += RMI_F03_OB_SIZE) {

loop, I understand that you want to forward the entire PS/2 response
data and only decrement iwritecommandcounter once, but what if
the rmi_f03_attention() did not contain any OOB data at all ?

I believe that in that case iwritecommandcounter should not be
decremented ?

Regards,

Hans
Marge Yang Aug. 12, 2022, 8:43 a.m. UTC | #2
Hi Hans,
	Thanks for your suggestion.
I have replied the following questions.
Some questions will be fixed on V2 patch.

Thanks
Marge Yang

-----Original Message-----
From: Hans de Goede <hdegoede@redhat.com>
Sent: Thursday, August 11, 2022 10:23 PM
To: margeyang <marge.yang@synaptics.corp-partner.google.com>; dmitry.torokhov@gmail.com; linux-input@vger.kernel.org; linux-kernel@vger.kernel.org; benjamin.tissoires@redhat.com
Cc: Marge Yang <Marge.Yang@tw.synaptics.com>; derek.cheng@tw.synaptcs.com; Vincent Huang <Vincent.huang@tw.synaptics.com>
Subject: Re: [PATCH] Input: synaptics-rmi4 - filter incomplete relative packet.

CAUTION: Email originated externally, do not click links or open attachments unless you recognize the sender and know the content is safe.


Hi,

On 8/8/22 09:44, margeyang wrote:
> From: Marge Yang <marge.yang@synaptics.corp-partner.google.com>
>
> RMI4 F03 supports the Stick function,
> it's designed to support relative packet.
> This patch supports the following case.
> When relative packet can't be reported completely, it may miss one 
> byte or two byte.
> New Synaptics firmware will report PARITY error.
> When timeout error or parity error happens,
> RMI4 driver will sends 0xFE command and ask FW to Re-send stick packet 
> again.
>
> Signed-off-by: Marge
> Yang<marge.yang@synaptics.corp-partner.google.com>
> ---
>  drivers/input/rmi4/rmi_f03.c | 82
> ++++++++++++++++++++++++++++++++++--
>  1 file changed, 78 insertions(+), 4 deletions(-)
>
> diff --git a/drivers/input/rmi4/rmi_f03.c 
> b/drivers/input/rmi4/rmi_f03.c index c194b1664b10..57f03dfcb4ff 100644
> --- a/drivers/input/rmi4/rmi_f03.c
> +++ b/drivers/input/rmi4/rmi_f03.c
> @@ -23,8 +23,12 @@
>  #define RMI_F03_BYTES_PER_DEVICE_SHIFT       4
>  #define RMI_F03_QUEUE_LENGTH         0x0F
>
> +#define RMI_F03_RESET_STYK 0xFE

Please use tabs in front of the 0xFE to align it with the other values.

[Marge 08/12] Correct it on V2 patch.

> +
>  #define PSMOUSE_OOB_EXTRA_BTNS               0x01
>
> +#define RELATIVE_PACKET_SIZE         0x03

Just "3" please since this is a size (not a register value).


[Marge 08/12] Correct it on V2 patch.

> +
>  struct f03_data {
>       struct rmi_function *fn;
>
> @@ -36,7 +40,8 @@ struct f03_data {
>       u8 device_count;
>       u8 rx_queue_length;
>  };
> -
> +int iwritecommandcounter;
> +unsigned int ipacketindex;

Please do not use global variables like this, instead store these e.g. inside struct f03_data.

[Marge 08/12] Correct it on V2 patch.

>  int rmi_f03_overwrite_button(struct rmi_function *fn, unsigned int button,
>                            int value)
>  {
> @@ -87,7 +92,7 @@ static int rmi_f03_pt_write(struct serio *id, unsigned char val)
>                       __func__, error);
>               return error;
>       }
> -
> +     iwritecommandcounter++;
>       return 0;
>  }
>
> @@ -197,10 +202,12 @@ static int rmi_f03_register_pt(struct f03_data
> *f03)
>
>  static int rmi_f03_probe(struct rmi_function *fn)  {
> +
>       struct device *dev = &fn->dev;
>       struct f03_data *f03;
>       int error;
> -
> +     iwritecommandcounter = 0;
> +     ipacketindex = 0;
>       f03 = devm_kzalloc(dev, sizeof(struct f03_data), GFP_KERNEL);
>       if (!f03)
>               return -ENOMEM;

If you put the 2 variables into the f03_data then there will be no need to zero them.

[Marge 08/12] It is initialized. I will move them to rmi_f03_initialize function.

> @@ -251,9 +258,12 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>       const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
>       u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
>       u8 ob_status;
> +     static u8 ob_dataArry[RELATIVE_PACKET_SIZE];
>       u8 ob_data;
>       unsigned int serio_flags;
> +     static unsigned int serio_flagsArry[RELATIVE_PACKET_SIZE];

Please drop these 2 static arrays here and instead store the info in the f03_data struct.
[Marge 08/12] Correct it on V2 patch.

>       int i;
> +

Unrelated whitespace change, please drop.
[Marge 08/12] Correct it on V2 patch.

>       int error;
>
>       if (drvdata->attn_data.data) {
> @@ -284,6 +294,22 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>               ob_data = obs[i + RMI_F03_OB_DATA_OFFSET];
>               serio_flags = 0;
>
> +             if (ob_status & (RMI_F03_OB_FLAG_TIMEOUT | RMI_F03_OB_FLAG_PARITY)) {
> +                     //  Send resend command to stick when timeout or parity error.
> +                     //  Driver can receive the last stick packet.
> +
> +                     error = rmi_write(f03->fn->rmi_dev, f03->fn->fd.data_base_addr,
> +                      RMI_F03_RESET_STYK);
> +                     if (error) {
> +                             dev_err(&f03->fn->dev,
> +                                     "%s: Failed to rmi_write to F03 TX register (%d).\n",
> +                                     __func__, error);
> +                             return error;
> +                     }
> +                     ipacketindex = 0;
> +                     break;
> +             }
> +
>               if (!(ob_status & RMI_F03_RX_DATA_OFB))
>                       continue;
>
> @@ -298,9 +324,57 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>                       serio_flags & SERIO_TIMEOUT ?  'Y' : 'N',
>                       serio_flags & SERIO_PARITY ? 'Y' : 'N');
>
> -             serio_interrupt(f03->serio, ob_data, serio_flags);
> +             if (iwritecommandcounter > 0) {
> +                     // Read Acknowledge Byte after writing the PS2 command.
> +                     // It is not trackpoint data.
> +                     serio_interrupt(f03->serio, ob_data, 
> + serio_flags);
> +
> +             } else {
> +                     //   The relative-mode PS/2 packet format is as follows:
> +                     //
> +                     //              bit position            position (as array of bytes)
> +                     //     7   6   5   4   3   2   1   0
> +                     //   =================================+
> +                     //    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
> +                     //                DX[7:0]             | DATA[1]
> +                     //                DY[7:0]             | DATA[2]
> +                     //   =================================+
> +                     //              Yov: Y overflow
> +                     //    Xov: X overflow
> +                     if ((ipacketindex == 0) && (ob_data & ((BIT(7)|BIT(6))))) {
> +                             dev_err(&f03->fn->dev,
> +                             "%s: X or Y is overflow. (%x)\n",
> +                             __func__, ob_data);
> +                             break;
> +                     } else if ((ipacketindex == 0) && !(ob_data & BIT(3))) {
> +                             dev_err(&f03->fn->dev,
> +                             "%s: New BIT 3 is not 1 for the first byte\n",
> +                             __func__);

Why no break; here like above ?

[Marge 08/12] Correct it on V2 patch.

> +                     } else {
> +                             if (ipacketindex >= RELATIVE_PACKET_SIZE) {
> +                                     ipacketindex = 0;

This means that you are skipping every 4th byte, since you only store the ob_data + serio_flags the next cycle through the loop!

[Marge 08/12] Correct it on V2 patch.

> +                             } else {
> +                                     ob_dataArry[ipacketindex] = ob_data;
> +                                     serio_flagsArry[ipacketindex] = serio_flags;
> +                                     ipacketindex++;
> +                             }
> +                             if (ipacketindex == RELATIVE_PACKET_SIZE)       {
> +                                     serio_interrupt(f03->serio, ob_dataArry[0],
> +                                      serio_flagsArry[0]);
> +                                     serio_interrupt(f03->serio, ob_dataArry[1],
> +                                      serio_flagsArry[1]);
> +                                     serio_interrupt(f03->serio, ob_dataArry[2],
> +                                      serio_flagsArry[2]);
> +                                     ipacketindex = 0;
> +                             }
> +                     }
> +             }
>       }
>
> +     if (iwritecommandcounter > 0) {
> +             ipacketindex = 0;
> +             iwritecommandcounter = iwritecommandcounter - 1;
> +     }

You check iwritecommandcounter inside the:

        for (i = 0; i < ob_len; i += RMI_F03_OB_SIZE) {

loop, I understand that you want to forward the entire PS/2 response data and only decrement iwritecommandcounter once, but what if the rmi_f03_attention() did not contain any OOB data at all ?

I believe that in that case iwritecommandcounter should not be decremented ?

[Marge 08/12] For Synaptics FW design, FW must have responses after TP driver writes to F03 TX register.
When Touchpad FW gets this information, Touchpad FW needs to sync it with stick device( or TrackPoint device).
After Stick device(or TrackPoint device) gets this information, it will report 0xFA data (Ack) or other information to Touchpad device.
We can get FW's response from F03 RX register(in rmi_f03_attention).
If Synaptics FW has no response, it may be a Synaptics firmware issue.
For RMI_F03_RESET_STYK command, it is a special command.
After TP driver writes RMI_F03_RESET_STYK to F03 TX register, FW will clear fun03 buffer and re-send relative packet again.

Regards,

Hans
Hans de Goede Aug. 12, 2022, 8:53 a.m. UTC | #3
Hi,

On 8/12/22 10:43, Marge Yang wrote:
> Hi Hans,
> 	Thanks for your suggestion.
> I have replied the following questions.
> Some questions will be fixed on V2 patch.
> 
> Thanks
> Marge Yang
> 
> -----Original Message-----
> From: Hans de Goede <hdegoede@redhat.com>
> Sent: Thursday, August 11, 2022 10:23 PM
> To: margeyang <marge.yang@synaptics.corp-partner.google.com>; dmitry.torokhov@gmail.com; linux-input@vger.kernel.org; linux-kernel@vger.kernel.org; benjamin.tissoires@redhat.com
> Cc: Marge Yang <Marge.Yang@tw.synaptics.com>; derek.cheng@tw.synaptcs.com; Vincent Huang <Vincent.huang@tw.synaptics.com>
> Subject: Re: [PATCH] Input: synaptics-rmi4 - filter incomplete relative packet.
> 
> CAUTION: Email originated externally, do not click links or open attachments unless you recognize the sender and know the content is safe.
> 
> 
> Hi,
> 
> On 8/8/22 09:44, margeyang wrote:
>> From: Marge Yang <marge.yang@synaptics.corp-partner.google.com>
>>
>> RMI4 F03 supports the Stick function,
>> it's designed to support relative packet.
>> This patch supports the following case.
>> When relative packet can't be reported completely, it may miss one 
>> byte or two byte.
>> New Synaptics firmware will report PARITY error.
>> When timeout error or parity error happens,
>> RMI4 driver will sends 0xFE command and ask FW to Re-send stick packet 
>> again.
>>
>> Signed-off-by: Marge
>> Yang<marge.yang@synaptics.corp-partner.google.com>
>> ---
>>  drivers/input/rmi4/rmi_f03.c | 82
>> ++++++++++++++++++++++++++++++++++--
>>  1 file changed, 78 insertions(+), 4 deletions(-)
>>
>> diff --git a/drivers/input/rmi4/rmi_f03.c 
>> b/drivers/input/rmi4/rmi_f03.c index c194b1664b10..57f03dfcb4ff 100644
>> --- a/drivers/input/rmi4/rmi_f03.c
>> +++ b/drivers/input/rmi4/rmi_f03.c
>> @@ -23,8 +23,12 @@
>>  #define RMI_F03_BYTES_PER_DEVICE_SHIFT       4
>>  #define RMI_F03_QUEUE_LENGTH         0x0F
>>
>> +#define RMI_F03_RESET_STYK 0xFE
> 
> Please use tabs in front of the 0xFE to align it with the other values.
> 
> [Marge 08/12] Correct it on V2 patch.
> 
>> +
>>  #define PSMOUSE_OOB_EXTRA_BTNS               0x01
>>
>> +#define RELATIVE_PACKET_SIZE         0x03
> 
> Just "3" please since this is a size (not a register value).
> 
> 
> [Marge 08/12] Correct it on V2 patch.
> 
>> +
>>  struct f03_data {
>>       struct rmi_function *fn;
>>
>> @@ -36,7 +40,8 @@ struct f03_data {
>>       u8 device_count;
>>       u8 rx_queue_length;
>>  };
>> -
>> +int iwritecommandcounter;
>> +unsigned int ipacketindex;
> 
> Please do not use global variables like this, instead store these e.g. inside struct f03_data.
> 
> [Marge 08/12] Correct it on V2 patch.
> 
>>  int rmi_f03_overwrite_button(struct rmi_function *fn, unsigned int button,
>>                            int value)
>>  {
>> @@ -87,7 +92,7 @@ static int rmi_f03_pt_write(struct serio *id, unsigned char val)
>>                       __func__, error);
>>               return error;
>>       }
>> -
>> +     iwritecommandcounter++;
>>       return 0;
>>  }
>>
>> @@ -197,10 +202,12 @@ static int rmi_f03_register_pt(struct f03_data
>> *f03)
>>
>>  static int rmi_f03_probe(struct rmi_function *fn)  {
>> +
>>       struct device *dev = &fn->dev;
>>       struct f03_data *f03;
>>       int error;
>> -
>> +     iwritecommandcounter = 0;
>> +     ipacketindex = 0;
>>       f03 = devm_kzalloc(dev, sizeof(struct f03_data), GFP_KERNEL);
>>       if (!f03)
>>               return -ENOMEM;
> 
> If you put the 2 variables into the f03_data then there will be no need to zero them.
> 
> [Marge 08/12] It is initialized. I will move them to rmi_f03_initialize function.
> 
>> @@ -251,9 +258,12 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>       const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
>>       u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
>>       u8 ob_status;
>> +     static u8 ob_dataArry[RELATIVE_PACKET_SIZE];
>>       u8 ob_data;
>>       unsigned int serio_flags;
>> +     static unsigned int serio_flagsArry[RELATIVE_PACKET_SIZE];
> 
> Please drop these 2 static arrays here and instead store the info in the f03_data struct.
> [Marge 08/12] Correct it on V2 patch.
> 
>>       int i;
>> +
> 
> Unrelated whitespace change, please drop.
> [Marge 08/12] Correct it on V2 patch.
> 
>>       int error;
>>
>>       if (drvdata->attn_data.data) {
>> @@ -284,6 +294,22 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>               ob_data = obs[i + RMI_F03_OB_DATA_OFFSET];
>>               serio_flags = 0;
>>
>> +             if (ob_status & (RMI_F03_OB_FLAG_TIMEOUT | RMI_F03_OB_FLAG_PARITY)) {
>> +                     //  Send resend command to stick when timeout or parity error.
>> +                     //  Driver can receive the last stick packet.
>> +
>> +                     error = rmi_write(f03->fn->rmi_dev, f03->fn->fd.data_base_addr,
>> +                      RMI_F03_RESET_STYK);
>> +                     if (error) {
>> +                             dev_err(&f03->fn->dev,
>> +                                     "%s: Failed to rmi_write to F03 TX register (%d).\n",
>> +                                     __func__, error);
>> +                             return error;
>> +                     }
>> +                     ipacketindex = 0;
>> +                     break;
>> +             }
>> +
>>               if (!(ob_status & RMI_F03_RX_DATA_OFB))
>>                       continue;
>>
>> @@ -298,9 +324,57 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>                       serio_flags & SERIO_TIMEOUT ?  'Y' : 'N',
>>                       serio_flags & SERIO_PARITY ? 'Y' : 'N');
>>
>> -             serio_interrupt(f03->serio, ob_data, serio_flags);
>> +             if (iwritecommandcounter > 0) {
>> +                     // Read Acknowledge Byte after writing the PS2 command.
>> +                     // It is not trackpoint data.
>> +                     serio_interrupt(f03->serio, ob_data, 
>> + serio_flags);
>> +
>> +             } else {
>> +                     //   The relative-mode PS/2 packet format is as follows:
>> +                     //
>> +                     //              bit position            position (as array of bytes)
>> +                     //     7   6   5   4   3   2   1   0
>> +                     //   =================================+
>> +                     //    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
>> +                     //                DX[7:0]             | DATA[1]
>> +                     //                DY[7:0]             | DATA[2]
>> +                     //   =================================+
>> +                     //              Yov: Y overflow
>> +                     //    Xov: X overflow
>> +                     if ((ipacketindex == 0) && (ob_data & ((BIT(7)|BIT(6))))) {
>> +                             dev_err(&f03->fn->dev,
>> +                             "%s: X or Y is overflow. (%x)\n",
>> +                             __func__, ob_data);
>> +                             break;
>> +                     } else if ((ipacketindex == 0) && !(ob_data & BIT(3))) {
>> +                             dev_err(&f03->fn->dev,
>> +                             "%s: New BIT 3 is not 1 for the first byte\n",
>> +                             __func__);
> 
> Why no break; here like above ?
> 
> [Marge 08/12] Correct it on V2 patch.
> 
>> +                     } else {
>> +                             if (ipacketindex >= RELATIVE_PACKET_SIZE) {
>> +                                     ipacketindex = 0;
> 
> This means that you are skipping every 4th byte, since you only store the ob_data + serio_flags the next cycle through the loop!
> 
> [Marge 08/12] Correct it on V2 patch.
> 
>> +                             } else {
>> +                                     ob_dataArry[ipacketindex] = ob_data;
>> +                                     serio_flagsArry[ipacketindex] = serio_flags;
>> +                                     ipacketindex++;
>> +                             }
>> +                             if (ipacketindex == RELATIVE_PACKET_SIZE)       {
>> +                                     serio_interrupt(f03->serio, ob_dataArry[0],
>> +                                      serio_flagsArry[0]);
>> +                                     serio_interrupt(f03->serio, ob_dataArry[1],
>> +                                      serio_flagsArry[1]);
>> +                                     serio_interrupt(f03->serio, ob_dataArry[2],
>> +                                      serio_flagsArry[2]);
>> +                                     ipacketindex = 0;
>> +                             }
>> +                     }
>> +             }
>>       }
>>
>> +     if (iwritecommandcounter > 0) {
>> +             ipacketindex = 0;
>> +             iwritecommandcounter = iwritecommandcounter - 1;
>> +     }
> 
> You check iwritecommandcounter inside the:
> 
>         for (i = 0; i < ob_len; i += RMI_F03_OB_SIZE) {
> 
> loop, I understand that you want to forward the entire PS/2 response data and only decrement iwritecommandcounter once, but what if the rmi_f03_attention() did not contain any OOB data at all ?
> 
> I believe that in that case iwritecommandcounter should not be decremented ?
> 
> [Marge 08/12] For Synaptics FW design, FW must have responses after TP driver writes to F03 TX register.
> When Touchpad FW gets this information, Touchpad FW needs to sync it with stick device( or TrackPoint device).
> After Stick device(or TrackPoint device) gets this information, it will report 0xFA data (Ack) or other information to Touchpad device.
> We can get FW's response from F03 RX register(in rmi_f03_attention).
> If Synaptics FW has no response, it may be a Synaptics firmware issue.
> For RMI_F03_RESET_STYK command, it is a special command.
> After TP driver writes RMI_F03_RESET_STYK to F03 TX register, FW will clear fun03 buffer and re-send relative packet again.

Right but what if the following happens:

1. PS/2 command is forward to stick through RMI
2. Regular RMI touchpad data is received, no PS/2 response yet, so no
   OOB data
3. Despite here not being OOB data rmi_f03_attention() still decrements
   iwritecommandcounter
4. PS/2 command reply comes in, but iwritecommandcounter is 0 now
   because of 3, so the code will hit the else part of:

+             if (iwritecommandcounter > 0) {
+                     // Read Acknowledge Byte after writing the PS2 command.
+                     // It is not trackpoint data.
+                     serio_interrupt(f03->serio, ob_data, 
+ serio_flags);
+
+             } else {
+                     //   The relative-mode PS/2 packet format is as follows:
+                     //
+                     //              bit position            position (as array of bytes)
+                     //     7   6   5   4   3   2   1   0
+                     //   =================================+
+                     //    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
+                     //                DX[7:0]             | DATA[1]
+                     //                DY[7:0]             | DATA[2]
+                     //   =================================+
+                     //              Yov: Y overflow
+                     //    Xov: X overflow
                      ...

Treating the command reply as a relative mode PS/2 packet, instead
of just forwarding it to the PS/2 layer.

I believe that to fix this possible race you *must* only decrement
iwritecommandcounter() at the end of rmi_f03_attention() if
the read data actually contains OOB data.

But thinking about this more, I guess this can never happen
because each function has its own attention handler and if
there is only touchpad data then the attention handler for
the f03 function will never run ?

Regards,

Hans
Marge Yang Aug. 12, 2022, 10:10 a.m. UTC | #4
Hi Hans,
	Thanks for your information.
I have replied the following questions.
Thanks
Marge Yang

-----Original Message-----
From: Hans de Goede <hdegoede@redhat.com> 
Sent: Friday, August 12, 2022 4:54 PM
To: Marge Yang <Marge.Yang@tw.synaptics.com>; margeyang <marge.yang@synaptics.corp-partner.google.com>; dmitry.torokhov@gmail.com; linux-input@vger.kernel.org; linux-kernel@vger.kernel.org; benjamin.tissoires@redhat.com
Cc: Derek Cheng <derek.cheng@tw.synaptics.com>; Vincent Huang <Vincent.huang@tw.synaptics.com>
Subject: Re: FW: [PATCH] Input: synaptics-rmi4 - filter incomplete relative packet.

CAUTION: Email originated externally, do not click links or open attachments unless you recognize the sender and know the content is safe.


Hi,

On 8/12/22 10:43, Marge Yang wrote:
> Hi Hans,
>       Thanks for your suggestion.
> I have replied the following questions.
> Some questions will be fixed on V2 patch.
>
> Thanks
> Marge Yang
>
> -----Original Message-----
> From: Hans de Goede <hdegoede@redhat.com>
> Sent: Thursday, August 11, 2022 10:23 PM
> To: margeyang <marge.yang@synaptics.corp-partner.google.com>; 
> dmitry.torokhov@gmail.com; linux-input@vger.kernel.org; 
> linux-kernel@vger.kernel.org; benjamin.tissoires@redhat.com
> Cc: Marge Yang <Marge.Yang@tw.synaptics.com>; 
> derek.cheng@tw.synaptcs.com; Vincent Huang 
> <Vincent.huang@tw.synaptics.com>
> Subject: Re: [PATCH] Input: synaptics-rmi4 - filter incomplete relative packet.
>
> CAUTION: Email originated externally, do not click links or open attachments unless you recognize the sender and know the content is safe.
>
>
> Hi,
>
> On 8/8/22 09:44, margeyang wrote:
>> From: Marge Yang <marge.yang@synaptics.corp-partner.google.com>
>>
>> RMI4 F03 supports the Stick function, it's designed to support 
>> relative packet.
>> This patch supports the following case.
>> When relative packet can't be reported completely, it may miss one 
>> byte or two byte.
>> New Synaptics firmware will report PARITY error.
>> When timeout error or parity error happens,
>> RMI4 driver will sends 0xFE command and ask FW to Re-send stick 
>> packet again.
>>
>> Signed-off-by: Marge
>> Yang<marge.yang@synaptics.corp-partner.google.com>
>> ---
>>  drivers/input/rmi4/rmi_f03.c | 82
>> ++++++++++++++++++++++++++++++++++--
>>  1 file changed, 78 insertions(+), 4 deletions(-)
>>
>> diff --git a/drivers/input/rmi4/rmi_f03.c 
>> b/drivers/input/rmi4/rmi_f03.c index c194b1664b10..57f03dfcb4ff 
>> 100644
>> --- a/drivers/input/rmi4/rmi_f03.c
>> +++ b/drivers/input/rmi4/rmi_f03.c
>> @@ -23,8 +23,12 @@
>>  #define RMI_F03_BYTES_PER_DEVICE_SHIFT       4
>>  #define RMI_F03_QUEUE_LENGTH         0x0F
>>
>> +#define RMI_F03_RESET_STYK 0xFE
>
> Please use tabs in front of the 0xFE to align it with the other values.
>
> [Marge 08/12] Correct it on V2 patch.
>
>> +
>>  #define PSMOUSE_OOB_EXTRA_BTNS               0x01
>>
>> +#define RELATIVE_PACKET_SIZE         0x03
>
> Just "3" please since this is a size (not a register value).
>
>
> [Marge 08/12] Correct it on V2 patch.
>
>> +
>>  struct f03_data {
>>       struct rmi_function *fn;
>>
>> @@ -36,7 +40,8 @@ struct f03_data {
>>       u8 device_count;
>>       u8 rx_queue_length;
>>  };
>> -
>> +int iwritecommandcounter;
>> +unsigned int ipacketindex;
>
> Please do not use global variables like this, instead store these e.g. inside struct f03_data.
>
> [Marge 08/12] Correct it on V2 patch.
>
>>  int rmi_f03_overwrite_button(struct rmi_function *fn, unsigned int button,
>>                            int value)  { @@ -87,7 +92,7 @@ static int 
>> rmi_f03_pt_write(struct serio *id, unsigned char val)
>>                       __func__, error);
>>               return error;
>>       }
>> -
>> +     iwritecommandcounter++;
>>       return 0;
>>  }
>>
>> @@ -197,10 +202,12 @@ static int rmi_f03_register_pt(struct f03_data
>> *f03)
>>
>>  static int rmi_f03_probe(struct rmi_function *fn)  {
>> +
>>       struct device *dev = &fn->dev;
>>       struct f03_data *f03;
>>       int error;
>> -
>> +     iwritecommandcounter = 0;
>> +     ipacketindex = 0;
>>       f03 = devm_kzalloc(dev, sizeof(struct f03_data), GFP_KERNEL);
>>       if (!f03)
>>               return -ENOMEM;
>
> If you put the 2 variables into the f03_data then there will be no need to zero them.
>
> [Marge 08/12] It is initialized. I will move them to rmi_f03_initialize function.
>
>> @@ -251,9 +258,12 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>       const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
>>       u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
>>       u8 ob_status;
>> +     static u8 ob_dataArry[RELATIVE_PACKET_SIZE];
>>       u8 ob_data;
>>       unsigned int serio_flags;
>> +     static unsigned int serio_flagsArry[RELATIVE_PACKET_SIZE];
>
> Please drop these 2 static arrays here and instead store the info in the f03_data struct.
> [Marge 08/12] Correct it on V2 patch.
>
>>       int i;
>> +
>
> Unrelated whitespace change, please drop.
> [Marge 08/12] Correct it on V2 patch.
>
>>       int error;
>>
>>       if (drvdata->attn_data.data) {
>> @@ -284,6 +294,22 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>               ob_data = obs[i + RMI_F03_OB_DATA_OFFSET];
>>               serio_flags = 0;
>>
>> +             if (ob_status & (RMI_F03_OB_FLAG_TIMEOUT | RMI_F03_OB_FLAG_PARITY)) {
>> +                     //  Send resend command to stick when timeout or parity error.
>> +                     //  Driver can receive the last stick packet.
>> +
>> +                     error = rmi_write(f03->fn->rmi_dev, f03->fn->fd.data_base_addr,
>> +                      RMI_F03_RESET_STYK);
>> +                     if (error) {
>> +                             dev_err(&f03->fn->dev,
>> +                                     "%s: Failed to rmi_write to F03 TX register (%d).\n",
>> +                                     __func__, error);
>> +                             return error;
>> +                     }
>> +                     ipacketindex = 0;
>> +                     break;
>> +             }
>> +
>>               if (!(ob_status & RMI_F03_RX_DATA_OFB))
>>                       continue;
>>
>> @@ -298,9 +324,57 @@ static irqreturn_t rmi_f03_attention(int irq, void *ctx)
>>                       serio_flags & SERIO_TIMEOUT ?  'Y' : 'N',
>>                       serio_flags & SERIO_PARITY ? 'Y' : 'N');
>>
>> -             serio_interrupt(f03->serio, ob_data, serio_flags);
>> +             if (iwritecommandcounter > 0) {
>> +                     // Read Acknowledge Byte after writing the PS2 command.
>> +                     // It is not trackpoint data.
>> +                     serio_interrupt(f03->serio, ob_data, 
>> + serio_flags);
>> +
>> +             } else {
>> +                     //   The relative-mode PS/2 packet format is as follows:
>> +                     //
>> +                     //              bit position            position (as array of bytes)
>> +                     //     7   6   5   4   3   2   1   0
>> +                     //   =================================+
>> +                     //    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
>> +                     //                DX[7:0]             | DATA[1]
>> +                     //                DY[7:0]             | DATA[2]
>> +                     //   =================================+
>> +                     //              Yov: Y overflow
>> +                     //    Xov: X overflow
>> +                     if ((ipacketindex == 0) && (ob_data & ((BIT(7)|BIT(6))))) {
>> +                             dev_err(&f03->fn->dev,
>> +                             "%s: X or Y is overflow. (%x)\n",
>> +                             __func__, ob_data);
>> +                             break;
>> +                     } else if ((ipacketindex == 0) && !(ob_data & BIT(3))) {
>> +                             dev_err(&f03->fn->dev,
>> +                             "%s: New BIT 3 is not 1 for the first byte\n",
>> +                             __func__);
>
> Why no break; here like above ?
>
> [Marge 08/12] Correct it on V2 patch.
>
>> +                     } else {
>> +                             if (ipacketindex >= RELATIVE_PACKET_SIZE) {
>> +                                     ipacketindex = 0;
>
> This means that you are skipping every 4th byte, since you only store the ob_data + serio_flags the next cycle through the loop!
>
> [Marge 08/12] Correct it on V2 patch.
>
>> +                             } else {
>> +                                     ob_dataArry[ipacketindex] = ob_data;
>> +                                     serio_flagsArry[ipacketindex] = serio_flags;
>> +                                     ipacketindex++;
>> +                             }
>> +                             if (ipacketindex == RELATIVE_PACKET_SIZE)       {
>> +                                     serio_interrupt(f03->serio, ob_dataArry[0],
>> +                                      serio_flagsArry[0]);
>> +                                     serio_interrupt(f03->serio, ob_dataArry[1],
>> +                                      serio_flagsArry[1]);
>> +                                     serio_interrupt(f03->serio, ob_dataArry[2],
>> +                                      serio_flagsArry[2]);
>> +                                     ipacketindex = 0;
>> +                             }
>> +                     }
>> +             }
>>       }
>>
>> +     if (iwritecommandcounter > 0) {
>> +             ipacketindex = 0;
>> +             iwritecommandcounter = iwritecommandcounter - 1;
>> +     }
>
> You check iwritecommandcounter inside the:
>
>         for (i = 0; i < ob_len; i += RMI_F03_OB_SIZE) {
>
> loop, I understand that you want to forward the entire PS/2 response data and only decrement iwritecommandcounter once, but what if the rmi_f03_attention() did not contain any OOB data at all ?
>
> I believe that in that case iwritecommandcounter should not be decremented ?
>
> [Marge 08/12] For Synaptics FW design, FW must have responses after TP driver writes to F03 TX register.
> When Touchpad FW gets this information, Touchpad FW needs to sync it with stick device( or TrackPoint device).
> After Stick device(or TrackPoint device) gets this information, it will report 0xFA data (Ack) or other information to Touchpad device.
> We can get FW's response from F03 RX register(in rmi_f03_attention).
> If Synaptics FW has no response, it may be a Synaptics firmware issue.
> For RMI_F03_RESET_STYK command, it is a special command.
> After TP driver writes RMI_F03_RESET_STYK to F03 TX register, FW will clear fun03 buffer and re-send relative packet again.

Right but what if the following happens:

1. PS/2 command is forward to stick through RMI 2. Regular RMI touchpad data is received, no PS/2 response yet, so no
   OOB data
3. Despite here not being OOB data rmi_f03_attention() still decrements
   iwritecommandcounter
4. PS/2 command reply comes in, but iwritecommandcounter is 0 now
   because of 3, so the code will hit the else part of:

[Marge 08/12]
RMI4 driver only supports the following interface.
SPI, SMB, HID/I2C or HID/USB.

When Touchpad is changed to "SMB, HID/I2C or HID/USB" interface, Touchpad data (f11 data or f12 data) will be reported to rmi_f11_attention or rmi_f12_attention.
For stick device, it is synced with Touchpad Firmware. Stick data will be reported by rmi_f03.

RMI4 driver doesn't support PS/2, so item 2 case will not happen.
For PS/2, it is supported in drivers/input/mouse/synaptics driver.


+             if (iwritecommandcounter > 0) {
+                     // Read Acknowledge Byte after writing the PS2 command.
+                     // It is not trackpoint data.
+                     serio_interrupt(f03->serio, ob_data, serio_flags);
+
+             } else {
+                     //   The relative-mode PS/2 packet format is as follows:
+                     //
+                     //              bit position            position (as array of bytes)
+                     //     7   6   5   4   3   2   1   0
+                     //   =================================+
+                     //    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
+                     //                DX[7:0]             | DATA[1]
+                     //                DY[7:0]             | DATA[2]
+                     //   =================================+
+                     //              Yov: Y overflow
+                     //    Xov: X overflow
                      ...

Treating the command reply as a relative mode PS/2 packet, instead of just forwarding it to the PS/2 layer.

I believe that to fix this possible race you *must* only decrement
iwritecommandcounter() at the end of rmi_f03_attention() if the read data actually contains OOB data.

But thinking about this more, I guess this can never happen because each function has its own attention handler and if there is only touchpad data then the attention handler for the f03 function will never run ?

[Marge 08/12]
For Touchpad data, it is rmi4 f11/f12 data and is handled in rmi_f11_attention/ rmi_f12_attention function.
Hence, rmi_f03 data will not be impacted by touchpad data. 

Regards,

Hans
diff mbox series

Patch

diff --git a/drivers/input/rmi4/rmi_f03.c b/drivers/input/rmi4/rmi_f03.c
index c194b1664b10..57f03dfcb4ff 100644
--- a/drivers/input/rmi4/rmi_f03.c
+++ b/drivers/input/rmi4/rmi_f03.c
@@ -23,8 +23,12 @@ 
 #define RMI_F03_BYTES_PER_DEVICE_SHIFT	4
 #define RMI_F03_QUEUE_LENGTH		0x0F
 
+#define RMI_F03_RESET_STYK 0xFE
+
 #define PSMOUSE_OOB_EXTRA_BTNS		0x01
 
+#define RELATIVE_PACKET_SIZE		0x03
+
 struct f03_data {
 	struct rmi_function *fn;
 
@@ -36,7 +40,8 @@  struct f03_data {
 	u8 device_count;
 	u8 rx_queue_length;
 };
-
+int iwritecommandcounter;
+unsigned int ipacketindex;
 int rmi_f03_overwrite_button(struct rmi_function *fn, unsigned int button,
 			     int value)
 {
@@ -87,7 +92,7 @@  static int rmi_f03_pt_write(struct serio *id, unsigned char val)
 			__func__, error);
 		return error;
 	}
-
+	iwritecommandcounter++;
 	return 0;
 }
 
@@ -197,10 +202,12 @@  static int rmi_f03_register_pt(struct f03_data *f03)
 
 static int rmi_f03_probe(struct rmi_function *fn)
 {
+
 	struct device *dev = &fn->dev;
 	struct f03_data *f03;
 	int error;
-
+	iwritecommandcounter = 0;
+	ipacketindex = 0;
 	f03 = devm_kzalloc(dev, sizeof(struct f03_data), GFP_KERNEL);
 	if (!f03)
 		return -ENOMEM;
@@ -251,9 +258,12 @@  static irqreturn_t rmi_f03_attention(int irq, void *ctx)
 	const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
 	u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
 	u8 ob_status;
+	static u8 ob_dataArry[RELATIVE_PACKET_SIZE];
 	u8 ob_data;
 	unsigned int serio_flags;
+	static unsigned int serio_flagsArry[RELATIVE_PACKET_SIZE];
 	int i;
+
 	int error;
 
 	if (drvdata->attn_data.data) {
@@ -284,6 +294,22 @@  static irqreturn_t rmi_f03_attention(int irq, void *ctx)
 		ob_data = obs[i + RMI_F03_OB_DATA_OFFSET];
 		serio_flags = 0;
 
+		if (ob_status & (RMI_F03_OB_FLAG_TIMEOUT | RMI_F03_OB_FLAG_PARITY)) {
+			//  Send resend command to stick when timeout or parity error.
+			//  Driver can receive the last stick packet.
+
+			error = rmi_write(f03->fn->rmi_dev, f03->fn->fd.data_base_addr,
+			 RMI_F03_RESET_STYK);
+			if (error) {
+				dev_err(&f03->fn->dev,
+					"%s: Failed to rmi_write to F03 TX register (%d).\n",
+					__func__, error);
+				return error;
+			}
+			ipacketindex = 0;
+			break;
+		}
+
 		if (!(ob_status & RMI_F03_RX_DATA_OFB))
 			continue;
 
@@ -298,9 +324,57 @@  static irqreturn_t rmi_f03_attention(int irq, void *ctx)
 			serio_flags & SERIO_TIMEOUT ?  'Y' : 'N',
 			serio_flags & SERIO_PARITY ? 'Y' : 'N');
 
-		serio_interrupt(f03->serio, ob_data, serio_flags);
+		if (iwritecommandcounter > 0) {
+			// Read Acknowledge Byte after writing the PS2 command.
+			// It is not trackpoint data.
+			serio_interrupt(f03->serio, ob_data, serio_flags);
+
+		} else {
+			//   The relative-mode PS/2 packet format is as follows:
+			//
+			//              bit position            position (as array of bytes)
+			//     7   6   5   4   3   2   1   0
+			//   =================================+
+			//    Yov Xov DY8 DX8  1   M   R   L  | DATA[0]
+			//                DX[7:0]             | DATA[1]
+			//                DY[7:0]             | DATA[2]
+			//   =================================+
+			//		Yov: Y overflow
+			//    Xov: X overflow
+			if ((ipacketindex == 0) && (ob_data & ((BIT(7)|BIT(6))))) {
+				dev_err(&f03->fn->dev,
+				"%s: X or Y is overflow. (%x)\n",
+				__func__, ob_data);
+				break;
+			} else if ((ipacketindex == 0) && !(ob_data & BIT(3))) {
+				dev_err(&f03->fn->dev,
+				"%s: New BIT 3 is not 1 for the first byte\n",
+				__func__);
+			} else {
+				if (ipacketindex >= RELATIVE_PACKET_SIZE) {
+					ipacketindex = 0;
+				} else {
+					ob_dataArry[ipacketindex] = ob_data;
+					serio_flagsArry[ipacketindex] = serio_flags;
+					ipacketindex++;
+				}
+				if (ipacketindex == RELATIVE_PACKET_SIZE)	{
+					serio_interrupt(f03->serio, ob_dataArry[0],
+					 serio_flagsArry[0]);
+					serio_interrupt(f03->serio, ob_dataArry[1],
+					 serio_flagsArry[1]);
+					serio_interrupt(f03->serio, ob_dataArry[2],
+					 serio_flagsArry[2]);
+					ipacketindex = 0;
+				}
+			}
+		}
 	}
 
+	if (iwritecommandcounter > 0) {
+		ipacketindex = 0;
+		iwritecommandcounter = iwritecommandcounter - 1;
+	}
 	return IRQ_HANDLED;
 }