- bool IsOddByte = (CurrentByte & 0x01);
- uint8_t ByteToWrite = *(NextWriteByte++);
-
- /* Check to see if we need to send a LOAD EXTENDED ADDRESS command to the target */
- if (MustLoadExtendedAddress)
- {
- ISPTarget_LoadExtendedAddress();
- MustLoadExtendedAddress = false;
- }
-
- ISPTarget_SendByte(Write_Memory_Params.ProgrammingCommands[0]);
- ISPTarget_SendByte(CurrentAddress >> 8);
- ISPTarget_SendByte(CurrentAddress & 0xFF);
- ISPTarget_SendByte(ByteToWrite);
-
- /* AVR FLASH addressing requires us to modify the write command based on if we are writing a high
- * or low byte at the current word address */
- if (V2Command == CMD_PROGRAM_FLASH_ISP)
- Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_HIGH_BYTE_MASK;
-
- /* Save previous programming mode in case we modify it for the current word */
- uint8_t PreviousProgrammingMode = Write_Memory_Params.ProgrammingMode;
-
- if (ByteToWrite != PollValue)
- {
- if (IsOddByte && (V2Command == CMD_PROGRAM_FLASH_ISP))
- Write_Memory_Params.ProgrammingCommands[2] |= READ_WRITE_HIGH_BYTE_MASK;
- else
- Write_Memory_Params.ProgrammingCommands[2] &= ~READ_WRITE_HIGH_BYTE_MASK;
-
- PollAddress = (CurrentAddress & 0xFFFF);
- }
- else if (!(Write_Memory_Params.ProgrammingMode & PROG_MODE_WORD_READYBUSY_MASK))
- {
- Write_Memory_Params.ProgrammingMode = (Write_Memory_Params.ProgrammingMode & ~PROG_MODE_WORD_VALUE_MASK) |
- PROG_MODE_WORD_TIMEDELAY_MASK;
- }
-
- ProgrammingStatus = ISPTarget_WaitForProgComplete(Write_Memory_Params.ProgrammingMode, PollAddress, PollValue,
+ /* If the current polling address is invalid, switch to timed delay write completion mode */
+ if (!(PollAddress) && !(ProgrammingMode & PROG_MODE_WORD_READYBUSY_MASK))
+ ProgrammingMode = (ProgrammingMode & ~PROG_MODE_WORD_VALUE_MASK) | PROG_MODE_WORD_TIMEDELAY_MASK;
+
+ ProgrammingStatus = ISPTarget_WaitForProgComplete(ProgrammingMode, PollAddress, PollValue,