Fixed AVRISP V2 Protocol handler for the READ_FLASH_ISP and READ_EEPROM_ISP handler...

[pub/USBasp.git] / Projects / Incomplete / AVRISP / Lib / V2Protocol.c

diff --git a/Projects/Incomplete/AVRISP/Lib/V2Protocol.c b/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
index 8d14802..f2089c6 100644 (file)
--- a/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
+++ b/Projects/Incomplete/AVRISP/Lib/V2Protocol.c
@@ -286,12 +286,21 @@ static void V2Protocol_Command_ProgramMemory(uint8_t V2Command)
        \r
                /* Paged mode memory programming */\r
                for (uint16_t CurrentByte = 0; CurrentByte < Write_Memory_Params.BytesToWrite; CurrentByte++)\r
-               {\r
+               {                               \r
+                       /* Check if the endpoint bank is currently empty */\r
+                       if (!(Endpoint_IsReadWriteAllowed()))\r
+                       {\r
+                               Endpoint_ClearOUT();\r
+                               Endpoint_WaitUntilReady();\r
+                       }       \r
+\r
                        bool    IsOddByte   = (CurrentByte & 0x01);\r
                        uint8_t ByteToWrite = Endpoint_Read_Byte();\r
                \r
                        if (IsOddByte && (V2Command == CMD_PROGRAM_FLASH_ISP))\r
-                         Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_HIGH_BYTE_MASK;\r
+                         Write_Memory_Params.ProgrammingCommands[0] |= READ_WRITE_HIGH_BYTE_MASK;\r
+                       else\r
+                         Write_Memory_Params.ProgrammingCommands[0] &= ~READ_WRITE_HIGH_BYTE_MASK;\r
                          \r
                        SPI_SendByte(Write_Memory_Params.ProgrammingCommands[0]);\r
                        SPI_SendByte(CurrentAddress >> 8);\r
@@ -304,14 +313,7 @@ static void V2Protocol_Command_ProgramMemory(uint8_t V2Command)
                                  Write_Memory_Params.ProgrammingCommands[2] |= READ_WRITE_HIGH_BYTE_MASK;\r
                                  \r
                                PollAddress = (CurrentAddress & 0xFFFF);                                \r
-                       }\r
-                               \r
-                       /* Check if the endpoint bank is currently empty */\r
-                       if (!(Endpoint_IsReadWriteAllowed()))\r
-                       {\r
-                               Endpoint_ClearOUT();\r
-                               Endpoint_WaitUntilReady();\r
-                       }                       \r
+                       }               \r
 \r
                        if (IsOddByte || (V2Command == CMD_PROGRAM_EEPROM_ISP))\r
                          CurrentAddress++;\r
@@ -341,11 +343,20 @@ static void V2Protocol_Command_ProgramMemory(uint8_t V2Command)
                /* Word/byte mode memory programming */\r
                for (uint16_t CurrentByte = 0; CurrentByte < Write_Memory_Params.BytesToWrite; CurrentByte++)\r
                {\r
+                       /* Check if the endpoint bank is currently empty */\r
+                       if (!(Endpoint_IsReadWriteAllowed()))\r
+                       {\r
+                               Endpoint_ClearOUT();\r
+                               Endpoint_WaitUntilReady();\r
+                       }       \r
+\r
                        bool    IsOddByte   = (CurrentByte & 0x01);\r
                        uint8_t ByteToWrite = Endpoint_Read_Byte();\r
                \r
                        if (IsOddByte && (V2Command == CMD_READ_FLASH_ISP))\r
-                         Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_HIGH_BYTE_MASK;\r
+                         Write_Memory_Params.ProgrammingCommands[0] |= READ_WRITE_HIGH_BYTE_MASK;\r
+                       else\r
+                         Write_Memory_Params.ProgrammingCommands[0] &= ~READ_WRITE_HIGH_BYTE_MASK;                     \r
                          \r
                        SPI_SendByte(Write_Memory_Params.ProgrammingCommands[0]);\r
                        SPI_SendByte(CurrentAddress >> 8);\r
@@ -359,13 +370,6 @@ static void V2Protocol_Command_ProgramMemory(uint8_t V2Command)
                                  \r
                                PollAddress = (CurrentAddress & 0xFFFF);\r
                        }\r
-                       \r
-                       /* Check if the endpoint bank is currently empty */\r
-                       if (!(Endpoint_IsReadWriteAllowed()))\r
-                       {\r
-                               Endpoint_ClearOUT();\r
-                               Endpoint_WaitUntilReady();\r
-                       }       \r
 \r
                        if (IsOddByte || (V2Command == CMD_PROGRAM_EEPROM_ISP))\r
                          CurrentAddress++;\r
@@ -406,8 +410,12 @@ static void V2Protocol_Command_ReadMemory(uint8_t V2Command)
        \r
        for (uint16_t CurrentByte = 0; CurrentByte < Read_Memory_Params.BytesToRead; CurrentByte++)\r
        {\r
-               if ((V2Command == CMD_READ_FLASH_ISP) && (CurrentByte & 0x01))\r
-                 Read_Memory_Params.ReadMemoryCommand ^= READ_WRITE_HIGH_BYTE_MASK;\r
+               bool IsOddByte = (CurrentByte & 0x01);\r
+\r
+               if (IsOddByte && (V2Command == CMD_READ_FLASH_ISP))\r
+                 Read_Memory_Params.ReadMemoryCommand |= READ_WRITE_HIGH_BYTE_MASK;\r
+               else\r
+                 Read_Memory_Params.ReadMemoryCommand &= ~READ_WRITE_HIGH_BYTE_MASK;\r
 \r
                SPI_SendByte(Read_Memory_Params.ReadMemoryCommand);\r
                SPI_SendByte(CurrentAddress >> 8);\r
@@ -421,7 +429,7 @@ static void V2Protocol_Command_ReadMemory(uint8_t V2Command)
                        Endpoint_WaitUntilReady();\r
                }\r
                \r
-               if (((V2Command == CMD_READ_FLASH_ISP) && (CurrentByte & 0x01)) || (V2Command == CMD_READ_EEPROM_ISP))\r
+               if ((IsOddByte && (V2Command == CMD_READ_FLASH_ISP)) || (V2Command == CMD_READ_EEPROM_ISP))\r
                  CurrentAddress++;\r
        }\r
 \r