Switch back to C based timeout ISR in the AVRISP project - assembly version was faili...

[pub/USBasp.git] / Projects / AVRISP-MKII / Lib / XPROG / XPROGTarget.c

diff --git a/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c b/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
index c1f1a63..6adb24f 100644 (file)
--- a/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
+++ b/Projects/AVRISP-MKII/Lib/XPROG/XPROGTarget.c
@@ -1,13 +1,13 @@
 /*\r
              LUFA Library\r
 /*\r
              LUFA Library\r

-     Copyright (C) Dean Camera, 2009.\r
+     Copyright (C) Dean Camera, 2010.\r

               \r
   dean [at] fourwalledcubicle [dot] com\r
       www.fourwalledcubicle.com\r
 */\r
 \r
 /*\r
               \r
   dean [at] fourwalledcubicle [dot] com\r
       www.fourwalledcubicle.com\r
 */\r
 \r
 /*\r

-  Copyright 2009  Dean Camera (dean [at] fourwalledcubicle [dot] com)\r
+  Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)\r

 \r
   Permission to use, copy, modify, distribute, and sell this \r
   software and its documentation for any purpose is hereby granted\r
 \r
   Permission to use, copy, modify, distribute, and sell this \r
   software and its documentation for any purpose is hereby granted\r


@@ -53,7 +53,7 @@ volatile uint16_t           SoftUSART_Data;
 ISR(TIMER1_COMPA_vect, ISR_BLOCK)\r
 {\r
        /* Toggle CLOCK pin in a single cycle (see AVR datasheet) */\r
 ISR(TIMER1_COMPA_vect, ISR_BLOCK)\r
 {\r
        /* Toggle CLOCK pin in a single cycle (see AVR datasheet) */\r

-       BITBANG_PDICLOCK_PIN |= BITBANG_PDICLOCK_MASK;\r
+       BITBANG_PDICLOCK_PIN = BITBANG_PDICLOCK_MASK;\r

 \r
        /* If not sending or receiving, just exit */\r
        if (!(SoftUSART_BitCount))\r
 \r
        /* If not sending or receiving, just exit */\r
        if (!(SoftUSART_BitCount))\r


@@ -69,9 +69,9 @@ ISR(TIMER1_COMPA_vect, ISR_BLOCK)
                /* Wait for the start bit when receiving */\r
                if ((SoftUSART_BitCount == BITS_IN_USART_FRAME) && (BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK))\r
                  return;\r
                /* Wait for the start bit when receiving */\r
                if ((SoftUSART_BitCount == BITS_IN_USART_FRAME) && (BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK))\r
                  return;\r

-       \r
+\r

                /* Shift in the bit one less than the frame size in position, so that the start bit will eventually\r
                /* Shift in the bit one less than the frame size in position, so that the start bit will eventually\r

-                * be discarded leaving the data to be byte-aligned for quick access */\r
+                * be discarded leaving the data to be byte-aligned for quick access (subtract 9 as we are ORing to the MSB) */\r

                if (BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK)\r
                  ((uint8_t*)&SoftUSART_Data)[1] |= (1 << (BITS_IN_USART_FRAME - 9));\r
 \r
                if (BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK)\r
                  ((uint8_t*)&SoftUSART_Data)[1] |= (1 << (BITS_IN_USART_FRAME - 9));\r
 \r


@@ -91,15 +91,15 @@ ISR(TIMER1_COMPA_vect, ISR_BLOCK)
                  BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;                  \r
 \r
                SoftUSART_Data >>= 1;\r
                  BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;                  \r
 \r
                SoftUSART_Data >>= 1;\r

-               SoftUSART_BitCount--;\r
+               SoftUSART_BitCount--;   \r

        }\r
 }\r
 \r
 /** ISR to manage the TPI software USART when bit-banged TPI USART mode is selected. */\r
        }\r
 }\r
 \r
 /** ISR to manage the TPI software USART when bit-banged TPI USART mode is selected. */\r

-ISR(TIMER1_COMPB_vect, ISR_BLOCK)\r
+ISR(TIMER1_CAPT_vect, ISR_BLOCK)\r

 {\r
        /* Toggle CLOCK pin in a single cycle (see AVR datasheet) */\r
 {\r
        /* Toggle CLOCK pin in a single cycle (see AVR datasheet) */\r

-       BITBANG_TPICLOCK_PIN |= BITBANG_TPICLOCK_MASK;\r
+       BITBANG_TPICLOCK_PIN = BITBANG_TPICLOCK_MASK;\r

 \r
        /* If not sending or receiving, just exit */\r
        if (!(SoftUSART_BitCount))\r
 \r
        /* If not sending or receiving, just exit */\r
        if (!(SoftUSART_BitCount))\r


@@ -117,7 +117,7 @@ ISR(TIMER1_COMPB_vect, ISR_BLOCK)
                  return;\r
        \r
                /* Shift in the bit one less than the frame size in position, so that the start bit will eventually\r
                  return;\r
        \r
                /* Shift in the bit one less than the frame size in position, so that the start bit will eventually\r

-                * be discarded leaving the data to be byte-aligned for quick access */\r
+                * be discarded leaving the data to be byte-aligned for quick access (subtract 9 as we are ORing to the MSB) */\r

                if (BITBANG_TPIDATA_PIN & BITBANG_TPIDATA_MASK)\r
                 ((uint8_t*)&SoftUSART_Data)[1] |= (1 << (BITS_IN_USART_FRAME - 9));\r
 \r
                if (BITBANG_TPIDATA_PIN & BITBANG_TPIDATA_MASK)\r
                 ((uint8_t*)&SoftUSART_Data)[1] |= (1 << (BITS_IN_USART_FRAME - 9));\r
 \r


@@ -154,12 +154,10 @@ void XPROGTarget_EnableTargetPDI(void)
        \r
        /* Set DATA line high for at least 90ns to disable /RESET functionality */\r
        PORTD |= (1 << 3);\r
        \r
        /* Set DATA line high for at least 90ns to disable /RESET functionality */\r
        PORTD |= (1 << 3);\r

-       asm volatile ("NOP"::);\r
-       asm volatile ("NOP"::);\r
+       _delay_us(1);\r

        \r
        \r

-       /* Set up the synchronous USART for XMEGA communications - \r
-          8 data bits, even parity, 2 stop bits */\r
-       UBRR1  = (F_CPU / 1000000UL);\r
+       /* Set up the synchronous USART for XMEGA communications - 8 data bits, even parity, 2 stop bits */\r
+       UBRR1  = (F_CPU / XPROG_HARDWARE_SPEED);\r

        UCSR1B = (1 << TXEN1);\r
        UCSR1C = (1 << UMSEL10) | (1 << UPM11) | (1 << USBS1) | (1 << UCSZ11) | (1 << UCSZ10) | (1 << UCPOL1);\r
 #else\r
        UCSR1B = (1 << TXEN1);\r
        UCSR1C = (1 << UMSEL10) | (1 << UPM11) | (1 << USBS1) | (1 << UCSZ11) | (1 << UCSZ10) | (1 << UCPOL1);\r
 #else\r


@@ -167,15 +165,18 @@ void XPROGTarget_EnableTargetPDI(void)
        BITBANG_PDIDATA_DDR  |= BITBANG_PDIDATA_MASK;\r
        BITBANG_PDICLOCK_DDR |= BITBANG_PDICLOCK_MASK;\r
        \r
        BITBANG_PDIDATA_DDR  |= BITBANG_PDIDATA_MASK;\r
        BITBANG_PDICLOCK_DDR |= BITBANG_PDICLOCK_MASK;\r
        \r

+       /* Set DATA line low for at least 90ns to ensure that the device is ready for PDI mode to be entered */\r
+       BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;\r
+       _delay_us(1);\r
+\r

        /* Set DATA line high for at least 90ns to disable /RESET functionality */\r
        BITBANG_PDIDATA_PORT |= BITBANG_PDIDATA_MASK;\r
        /* Set DATA line high for at least 90ns to disable /RESET functionality */\r
        BITBANG_PDIDATA_PORT |= BITBANG_PDIDATA_MASK;\r

-       asm volatile ("NOP"::);\r
-       asm volatile ("NOP"::);\r
+       _delay_us(1);\r

 \r
        /* Fire timer compare channel A ISR to manage the software USART */\r
 \r
        /* Fire timer compare channel A ISR to manage the software USART */\r

-       OCR1A   = BITS_BETWEEN_USART_CLOCKS;\r
-       TCCR1B  = (1 << WGM12) | (1 << CS10);\r
-       TIMSK1  = (1 << OCIE1A);\r
+       OCR1A  = BITS_BETWEEN_USART_CLOCKS;\r
+       TCCR1B = (1 << WGM12) | (1 << CS10);\r
+       TIMSK1 = (1 << OCIE1A);\r

 #endif\r
 \r
        /* Send two BREAKs of 12 bits each to enable PDI interface (need at least 16 idle bits) */\r
 #endif\r
 \r
        /* Send two BREAKs of 12 bits each to enable PDI interface (need at least 16 idle bits) */\r


@@ -188,20 +189,18 @@ void XPROGTarget_EnableTargetTPI(void)
 {\r
        IsSending = false;\r
 \r
 {\r
        IsSending = false;\r
 \r

-       /* Set /RESET line low for at least 90ns to enable TPI functionality */\r
+       /* Set /RESET line low for at least 400ns to enable TPI functionality */\r

        AUX_LINE_DDR  |=  AUX_LINE_MASK;\r
        AUX_LINE_PORT &= ~AUX_LINE_MASK;\r
        AUX_LINE_DDR  |=  AUX_LINE_MASK;\r
        AUX_LINE_PORT &= ~AUX_LINE_MASK;\r

-       asm volatile ("NOP"::);\r
-       asm volatile ("NOP"::);\r
+       _delay_us(1);\r

 \r
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Set Tx and XCK as outputs, Rx as input */\r
        DDRD |=  (1 << 5) | (1 << 3);\r
        DDRD &= ~(1 << 2);\r
                \r
 \r
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Set Tx and XCK as outputs, Rx as input */\r
        DDRD |=  (1 << 5) | (1 << 3);\r
        DDRD &= ~(1 << 2);\r
                \r

-       /* Set up the synchronous USART for XMEGA communications - \r
-          8 data bits, even parity, 2 stop bits */\r
-       UBRR1  = (F_CPU / 1000000UL);\r
+       /* Set up the synchronous USART for TINY communications - 8 data bits, even parity, 2 stop bits */\r
+       UBRR1  = (F_CPU / XPROG_HARDWARE_SPEED);\r

        UCSR1B = (1 << TXEN1);\r
        UCSR1C = (1 << UMSEL10) | (1 << UPM11) | (1 << USBS1) | (1 << UCSZ11) | (1 << UCSZ10) | (1 << UCPOL1);\r
 #else\r
        UCSR1B = (1 << TXEN1);\r
        UCSR1C = (1 << UMSEL10) | (1 << UPM11) | (1 << USBS1) | (1 << UCSZ11) | (1 << UCSZ10) | (1 << UCPOL1);\r
 #else\r


@@ -212,10 +211,10 @@ void XPROGTarget_EnableTargetTPI(void)
        /* Set DATA line high for idle state */\r
        BITBANG_TPIDATA_PORT |= BITBANG_TPIDATA_MASK;\r
 \r
        /* Set DATA line high for idle state */\r
        BITBANG_TPIDATA_PORT |= BITBANG_TPIDATA_MASK;\r
 \r

-       /* Fire timer capture channel B ISR to manage the software USART */\r
-       OCR1B   = BITS_BETWEEN_USART_CLOCKS;\r
-       TCCR1B  = (1 << WGM12) | (1 << CS10);\r
-       TIMSK1  = (1 << OCIE1B);\r
+       /* Fire timer capture channel ISR to manage the software USART */\r
+       ICR1   = BITS_BETWEEN_USART_CLOCKS;\r
+       TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS10);\r
+       TIMSK1 = (1 << ICIE1);\r

 #endif\r
 \r
        /* Send two BREAKs of 12 bits each to enable TPI interface (need at least 16 idle bits) */\r
 #endif\r
 \r
        /* Send two BREAKs of 12 bits each to enable TPI interface (need at least 16 idle bits) */\r


@@ -226,29 +225,38 @@ void XPROGTarget_EnableTargetTPI(void)
 /** Disables the target's PDI interface, exits programming mode and starts the target's application. */\r
 void XPROGTarget_DisableTargetPDI(void)\r
 {\r
 /** Disables the target's PDI interface, exits programming mode and starts the target's application. */\r
 void XPROGTarget_DisableTargetPDI(void)\r
 {\r

+       /* Switch to Rx mode to ensure that all pending transmissions are complete */\r
+       XPROGTarget_SetRxMode();\r
+\r

 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Turn off receiver and transmitter of the USART, clear settings */\r
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Turn off receiver and transmitter of the USART, clear settings */\r

-       UCSR1A |= (1 << TXC1) | (1 << RXC1);\r
+       UCSR1A  = ((1 << TXC1) | (1 << RXC1));\r

        UCSR1B  = 0;\r
        UCSR1C  = 0;\r
 \r
        UCSR1B  = 0;\r
        UCSR1C  = 0;\r
 \r

-       /* Set all USART lines as input, tristate */\r
+       /* Tristate all pins */\r

        DDRD  &= ~((1 << 5) | (1 << 3));\r
        PORTD &= ~((1 << 5) | (1 << 3) | (1 << 2));\r
 #else\r
        DDRD  &= ~((1 << 5) | (1 << 3));\r
        PORTD &= ~((1 << 5) | (1 << 3) | (1 << 2));\r
 #else\r

+       /* Turn off software USART management timer */\r
+       TCCR1B = 0;\r
+\r

        /* Set DATA and CLOCK lines to inputs */\r
        BITBANG_PDIDATA_DDR   &= ~BITBANG_PDIDATA_MASK;\r
        BITBANG_PDICLOCK_DDR  &= ~BITBANG_PDICLOCK_MASK;\r
        \r
        /* Tristate DATA and CLOCK lines */\r
        BITBANG_PDIDATA_PORT  &= ~BITBANG_PDIDATA_MASK;\r
        /* Set DATA and CLOCK lines to inputs */\r
        BITBANG_PDIDATA_DDR   &= ~BITBANG_PDIDATA_MASK;\r
        BITBANG_PDICLOCK_DDR  &= ~BITBANG_PDICLOCK_MASK;\r
        \r
        /* Tristate DATA and CLOCK lines */\r
        BITBANG_PDIDATA_PORT  &= ~BITBANG_PDIDATA_MASK;\r

-       BITBANG_PDICLOCK_PORT &= ~BITBANG_PDICLOCK_MASK;\r
+       BITBANG_PDICLOCK_PORT &= ~BITBANG_PDICLOCK_MASK;        \r

 #endif\r
 }\r
 \r
 /** Disables the target's TPI interface, exits programming mode and starts the target's application. */\r
 void XPROGTarget_DisableTargetTPI(void)\r
 {\r
 #endif\r
 }\r
 \r
 /** Disables the target's TPI interface, exits programming mode and starts the target's application. */\r
 void XPROGTarget_DisableTargetTPI(void)\r
 {\r

+       /* Switch to Rx mode to ensure that all pending transmissions are complete */\r
+       XPROGTarget_SetRxMode();\r
+\r

 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Turn off receiver and transmitter of the USART, clear settings */\r
        UCSR1A |= (1 << TXC1) | (1 << RXC1);\r
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Turn off receiver and transmitter of the USART, clear settings */\r
        UCSR1A |= (1 << TXC1) | (1 << RXC1);\r


@@ -259,6 +267,9 @@ void XPROGTarget_DisableTargetTPI(void)
        DDRD  &= ~((1 << 5) | (1 << 3));\r
        PORTD &= ~((1 << 5) | (1 << 3) | (1 << 2));\r
 #else\r
        DDRD  &= ~((1 << 5) | (1 << 3));\r
        PORTD &= ~((1 << 5) | (1 << 3) | (1 << 2));\r
 #else\r

+       /* Turn off software USART management timer */\r
+       TCCR1B = 0;\r
+\r

        /* Set DATA and CLOCK lines to inputs */\r
        BITBANG_TPIDATA_DDR   &= ~BITBANG_TPIDATA_MASK;\r
        BITBANG_TPICLOCK_DDR  &= ~BITBANG_TPICLOCK_MASK;\r
        /* Set DATA and CLOCK lines to inputs */\r
        BITBANG_TPIDATA_DDR   &= ~BITBANG_TPIDATA_MASK;\r
        BITBANG_TPICLOCK_DDR  &= ~BITBANG_TPICLOCK_MASK;\r


@@ -293,7 +304,7 @@ void XPROGTarget_SendByte(const uint8_t Byte)
        uint16_t NewUSARTData = ((1 << 11) | (1 << 10) | (0 << 9) | ((uint16_t)Byte << 1) | (0 << 0));\r
 \r
        /* Compute Even parity - while a bit is still set, chop off lowest bit and toggle parity bit */\r
        uint16_t NewUSARTData = ((1 << 11) | (1 << 10) | (0 << 9) | ((uint16_t)Byte << 1) | (0 << 0));\r
 \r
        /* Compute Even parity - while a bit is still set, chop off lowest bit and toggle parity bit */\r

-       uint8_t ParityData    = Byte;\r
+       uint8_t ParityData = Byte;\r

        while (ParityData)\r
        {\r
                NewUSARTData ^= (1 << 9);\r
        while (ParityData)\r
        {\r
                NewUSARTData ^= (1 << 9);\r


@@ -307,6 +318,9 @@ void XPROGTarget_SendByte(const uint8_t Byte)
        SoftUSART_Data     = NewUSARTData;\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
 #endif\r
        SoftUSART_Data     = NewUSARTData;\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
 #endif\r

+\r
+    if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r

 }\r
 \r
 /** Receives a byte via the software USART, blocking until data is received.\r
 }\r
 \r
 /** Receives a byte via the software USART, blocking until data is received.\r


@@ -322,12 +336,19 @@ uint8_t XPROGTarget_ReceiveByte(void)
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Wait until a byte has been received before reading */\r
        while (!(UCSR1A & (1 << RXC1)) && TimeoutMSRemaining);\r
 #if defined(XPROG_VIA_HARDWARE_USART)\r
        /* Wait until a byte has been received before reading */\r
        while (!(UCSR1A & (1 << RXC1)) && TimeoutMSRemaining);\r

+       \r
+       if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r
+\r

        return UDR1;\r
 #else\r
        /* Wait until a byte has been received before reading */\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
        while (SoftUSART_BitCount && TimeoutMSRemaining);\r
 \r
        return UDR1;\r
 #else\r
        /* Wait until a byte has been received before reading */\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
        while (SoftUSART_BitCount && TimeoutMSRemaining);\r
 \r

+       if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r
+\r

        /* Throw away the parity and stop bits to leave only the data (start bit is already discarded) */\r
        return (uint8_t)SoftUSART_Data;\r
 #endif\r
        /* Throw away the parity and stop bits to leave only the data (start bit is already discarded) */\r
        return (uint8_t)SoftUSART_Data;\r
 #endif\r


@@ -355,6 +376,9 @@ void XPROGTarget_SendBreak(void)
        SoftUSART_Data     = 0x0FFF;\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
 #endif\r
        SoftUSART_Data     = 0x0FFF;\r
        SoftUSART_BitCount = BITS_IN_USART_FRAME;\r
 #endif\r

+\r
+    if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r

 }\r
 \r
 static void XPROGTarget_SetTxMode(void)\r
 }\r
 \r
 static void XPROGTarget_SetTxMode(void)\r


@@ -372,8 +396,8 @@ static void XPROGTarget_SetTxMode(void)
                \r
        IsSending = true;\r
 #else\r
                \r
        IsSending = true;\r
 #else\r

-       while (SoftUSART_BitCount);\r
-\r
+       while (SoftUSART_BitCount && TimeoutMSRemaining);\r
+       \r

        /* Wait for a full cycle of the clock */\r
        SoftUSART_Data     = 0x0001;\r
        SoftUSART_BitCount = 1;\r
        /* Wait for a full cycle of the clock */\r
        SoftUSART_Data     = 0x0001;\r
        SoftUSART_BitCount = 1;\r


@@ -391,6 +415,9 @@ static void XPROGTarget_SetTxMode(void)
        }\r
 #endif\r
 \r
        }\r
 #endif\r
 \r

+    if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r
+\r

        IsSending = true;\r
 }\r
 \r
        IsSending = true;\r
 }\r
 \r


@@ -406,23 +433,29 @@ static void XPROGTarget_SetRxMode(void)
        DDRD   &= ~(1 << 3);\r
        PORTD  &= ~(1 << 3);\r
 #else\r
        DDRD   &= ~(1 << 3);\r
        PORTD  &= ~(1 << 3);\r
 #else\r

-       while (SoftUSART_BitCount);\r
+       while (SoftUSART_BitCount && TimeoutMSRemaining);\r

 \r
        if (XPROG_SelectedProtocol == XPRG_PROTOCOL_PDI)\r
        {\r
                BITBANG_PDIDATA_DDR  &= ~BITBANG_PDIDATA_MASK;\r
                BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;\r
 \r
        if (XPROG_SelectedProtocol == XPRG_PROTOCOL_PDI)\r
        {\r
                BITBANG_PDIDATA_DDR  &= ~BITBANG_PDIDATA_MASK;\r
                BITBANG_PDIDATA_PORT &= ~BITBANG_PDIDATA_MASK;\r

+\r
+               /* Wait until DATA line has been pulled up to idle by the target */\r
+               while (!(BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK) && TimeoutMSRemaining);\r

        }\r
        else\r
        {\r
                BITBANG_TPIDATA_DDR  &= ~BITBANG_TPIDATA_MASK;\r
        }\r
        else\r
        {\r
                BITBANG_TPIDATA_DDR  &= ~BITBANG_TPIDATA_MASK;\r

-               BITBANG_TPIDATA_PORT &= ~BITBANG_TPIDATA_MASK;  \r
-       }\r
-       \r
-       /* Wait until DATA line has been pulled up to idle by the target */\r
-       while (!(BITBANG_PDIDATA_PIN & BITBANG_PDIDATA_MASK));\r
+               BITBANG_TPIDATA_PORT &= ~BITBANG_TPIDATA_MASK;\r
+\r
+               /* Wait until DATA line has been pulled up to idle by the target */\r
+               while (!(BITBANG_TPIDATA_PIN & BITBANG_TPIDATA_MASK) && TimeoutMSRemaining);\r
+       }       \r

 #endif\r
 \r
 #endif\r
 \r

+    if (TimeoutMSRemaining)\r
+         TimeoutMSRemaining = COMMAND_TIMEOUT_MS;\r
+\r

        IsSending = false;\r
 }\r
 \r
        IsSending = false;\r
 }\r
 \r