+ asm volatile (
+ "sbrc %[len], 0\n\t"
+ "inc %[len]\n\t"
+"usbFunctionWrite_flashloop:\n\t"
+ "subi %[len], 2\n\t"
+ "brlo usbFunctionWrite_finished\n\t"
+
+#if HAVE_BLB11_SOFTW_LOCKBIT
+ "cpi r31, %[blsaddrhi]\n\t" /* accelerated BLB11_SOFTW_LOCKBIT check */
+ "brsh usbFunctionWrite_finished\n\t"
+// "brlo usbFunctionWrite_addrunlock_ok\n\t"
+// "brne usbFunctionWrite_finished\n\t"
+// "cpi r30, %[blsaddrlo]\n\t"
+// "brlo usbFunctionWrite_addrunlock_ok\n\t"
+// "rjmp usbFunctionWrite_finished\n\t"
+// "usbFunctionWrite_addrunlock_ok:\n\t"
+#endif
+ "rcall usbFunctionWrite_waitA\n\t"
+ "cli\n\t" /* r0 or r1 may be __zero_reg__ and may become dangerous nonzero within interrupts */
+ "ld r0, X+\n\t"
+ "ld r1, X+\n\t"
+
+ "ldi r18, %[pagfillval]\n\t"
+ "rcall usbFunctionWrite_saveflash\n\t" /* page fill */
+
+ "mov r18, r30\n\t"
+ "subi r18, 0xfe\n\t" /* add with 2 */
+ "andi r18, %[pagemask]\n\t"
+ "breq usbFunctionWrite_pageisfull\n\t"
+ "tst %[islast]\n\t"
+ "breq usbFunctionWrite_skippageisfull\n\t"
+ "tst %[isLastPage]\n\t"
+ "breq usbFunctionWrite_skippageisfull\n\t"
+ "cpi %[len], 0\n\t"
+ "brne usbFunctionWrite_skippageisfull\n\t"
+
+"usbFunctionWrite_pageisfull:\n\t" /* start writing the page */
+ "ldi r18, %[pageraseval]\n\t"
+ "rcall usbFunctionWrite_saveflash\n\t" /* page erase */
+ "rcall usbFunctionWrite_waitA\n\t"
+
+ "ldi r18, %[pagwriteval]\n\t"
+ "rcall usbFunctionWrite_saveflash\n\t" /* page write */
+ "rcall usbFunctionWrite_waitA\n\t"
+
+ "in __tmp_reg__, %[spmcr]\n\t"
+ "sbrs __tmp_reg__, %[rwwsbbit]\n\t"
+ "rjmp usbFunctionWrite_skippageisfull\n\t"
+ "ldi r18, %[rwwenrval]\n\t"
+ "rcall usbFunctionWrite_saveflash\n\t" /* reenable rww*/
+// "rcall usbFunctionWrite_waitA\n\t"
+
+
+"usbFunctionWrite_skippageisfull:\n\t"
+ "adiw r30, 0x2\n\t"
+ "rjmp usbFunctionWrite_flashloop\n\t"
+
+"usbFunctionWrite_saveflash:\n\t"
+ "cli\n\t"
+ "out %[spmcr], r18\n\t"
+ "spm\n\t"
+ "clr __zero_reg__\n\t" /* if r0 or r1 is __zero_reg__ it may have become inconsisten while page-fill */
+ "sei\n\t"
+ "ret\n\t"
+
+"usbFunctionWrite_waitA:\n\t"
+ "in __tmp_reg__, %[spmcr]\n\t"
+ "sbrc __tmp_reg__, %[spmenbit]\n\t"
+ "rjmp usbFunctionWrite_waitA\n\t"
+ "ret\n\t"
+
+"usbFunctionWrite_finished:\n\t"
+ : [addr] "+z" (currentAddress.l)
+
+ : [spmenbit] "I" (SPMEN),
+ [rwwsbbit] "I" (RWWSB),
+ [spmcr] "I" (_SFR_IO_ADDR(__SPM_REG)),
+ [pagfillval] "M" ((1<<SPMEN)),
+ [pageraseval] "M" ((1<<PGERS) | (1<<SPMEN)),
+ [pagwriteval] "M" ((1<<PGWRT) | (1<<SPMEN)),
+ [rwwenrval] "M" ((1<<RWWSRE) | (1<<SPMEN)),
+ [pagemask] "M" (SPM_PAGESIZE-1),
+#if HAVE_BLB11_SOFTW_LOCKBIT
+ [blsaddrhi] "M" ((uint8_t)((BOOTLOADER_PAGEADDR>>8)&0xff)),
+// [blsaddrlo] "M" ((uint8_t)((BOOTLOADER_PAGEADDR>>0)&0xff)),
+#endif
+ [islast] "r" (isLast),
+ [isLastPage] "r" (isLastPage),
+ [len] "d" (len),
+ [dataptr] "x" (data)
+
+ : "r0", "r1", "r18"
+ );
+ }
+ return isLast;
+}
+#else
+uchar usbFunctionWrite(uchar *data, uchar len)
+{
+uchar i,isLast;
+
+ DBG1(0x31, (void *)¤tAddress.l, 4);
+ if(len > bytesRemaining)
+ len = bytesRemaining;
+ bytesRemaining -= len;
+ isLast = bytesRemaining == 0;
+ for(i = 0; i < len;) {
+ if(currentRequest >= USBASP_FUNC_READEEPROM){
+ eeprom_write_byte((void *)(currentAddress.w[0]++), *data++);
+ i++;
+ } else {
+#if HAVE_BLB11_SOFTW_LOCKBIT
+ if (CURRENT_ADDRESS >= (addr_t)(BOOTLOADER_PAGEADDR)) {
+ return 1;
+ }
+#endif
+ i += 2;
+ DBG1(0x32, 0, 0);
+ cli();
+ boot_page_fill(CURRENT_ADDRESS, *(short *)data);
+ sei();
+ CURRENT_ADDRESS += 2;
+ data += 2;
+ /* write page when we cross page boundary or we have the last partial page */
+ if((currentAddress.w[0] & (SPM_PAGESIZE - 1)) == 0 || (isLast && i >= len && isLastPage)){
+#if (!HAVE_CHIP_ERASE) || (HAVE_ONDEMAND_PAGEERASE)
+ DBG1(0x33, 0, 0);
projects / pub / USBaspLoader.git / blobdiff