volatile void *flashc_memset64(volatile void *dst, uint64_t src, size_t nbytes, bool erase) { // Use aggregated pointers to have several alignments available for a same address. UnionCVPtr flash_array_end; UnionVPtr dest; Union64 source = {0}; StructCVPtr dest_end; UnionCVPtr flash_page_source_end; bool incomplete_flash_page_end; Union64 flash_dword; UnionVPtr tmp; unsigned int error_status = 0; unsigned int i; // Reformat arguments. flash_array_end.u8ptr = AVR32_FLASH + flashc_get_flash_size(); dest.u8ptr = dst; for (i = (Get_align((uint32_t)dest.u8ptr, sizeof(uint64_t)) - 1) & (sizeof(uint64_t) - 1); src; i = (i - 1) & (sizeof(uint64_t) - 1)) { source.u8[i] = src; src >>= 8; } dest_end.u8ptr = dest.u8ptr + nbytes; // If destination is outside flash, go to next flash page if any. if (dest.u8ptr < AVR32_FLASH) { dest.u8ptr = AVR32_FLASH; } else if (flash_array_end.u8ptr <= dest.u8ptr && dest.u8ptr < AVR32_FLASHC_USER_PAGE) { dest.u8ptr = AVR32_FLASHC_USER_PAGE; } // If end of destination is outside flash, move it to the end of the previous flash page if any. if (dest_end.u8ptr > AVR32_FLASHC_USER_PAGE + AVR32_FLASHC_USER_PAGE_SIZE) { dest_end.u8ptr = AVR32_FLASHC_USER_PAGE + AVR32_FLASHC_USER_PAGE_SIZE; } else if (AVR32_FLASHC_USER_PAGE >= dest_end.u8ptr && dest_end.u8ptr > flash_array_end.u8ptr) { dest_end.u8ptr = flash_array_end.u8ptr; } // Align each end of destination pointer with its natural boundary. dest_end.u16ptr = (uint16_t *)Align_down((uint32_t)dest_end.u8ptr, sizeof(uint16_t)); dest_end.u32ptr = (uint32_t *)Align_down((uint32_t)dest_end.u16ptr, sizeof(uint32_t)); dest_end.u64ptr = (uint64_t *)Align_down((uint32_t)dest_end.u32ptr, sizeof(uint64_t)); // While end of destination is not reached... while (dest.u8ptr < dest_end.u8ptr) { // Clear the page buffer in order to prepare data for a flash page write. flashc_clear_page_buffer(); error_status |= flashc_error_status; // Determine where the source data will end in the current flash page. flash_page_source_end.u64ptr = (uint64_t *)min((uint32_t)dest_end.u64ptr, Align_down((uint32_t)dest.u8ptr, AVR32_FLASHC_PAGE_SIZE) + AVR32_FLASHC_PAGE_SIZE); // Determine if the current destination page has an incomplete end. incomplete_flash_page_end = (Align_down((uint32_t)dest.u8ptr, AVR32_FLASHC_PAGE_SIZE) >= Align_down((uint32_t)dest_end.u8ptr, AVR32_FLASHC_PAGE_SIZE)); // Use a flash double-word buffer to manage unaligned accesses. flash_dword.u64 = source.u64; // If destination does not point to the beginning of the current flash page... if (!Test_align((uint32_t)dest.u8ptr, AVR32_FLASHC_PAGE_SIZE)) { // Fill the beginning of the page buffer with the current flash page data. // This is required by the hardware, even if page erase is not requested, // in order to be able to write successfully to erased parts of flash // pages that have already been written to. for (tmp.u8ptr = (uint8_t *)Align_down((uint32_t)dest.u8ptr, AVR32_FLASHC_PAGE_SIZE); tmp.u64ptr < (uint64_t *)Align_down((uint32_t)dest.u8ptr, sizeof(uint64_t)); tmp.u64ptr++) { *tmp.u64ptr = *tmp.u64ptr; } // If destination is not 64-bit aligned... if (!Test_align((uint32_t)dest.u8ptr, sizeof(uint64_t))) { // Fill the beginning of the flash double-word buffer with the current // flash page data. // This is required by the hardware, even if page erase is not // requested, in order to be able to write successfully to erased parts // of flash pages that have already been written to. for (i = 0; i < Get_align((uint32_t)dest.u8ptr, sizeof(uint64_t)); i++) { flash_dword.u8[i] = *tmp.u8ptr++; } // Align the destination pointer with its 64-bit boundary. dest.u64ptr = (uint64_t *)Align_down((uint32_t)dest.u8ptr, sizeof(uint64_t)); // If the current destination double-word is not the last one... if (dest.u64ptr < dest_end.u64ptr) { // Write the flash double-word buffer to the page buffer and reinitialize it. *dest.u64ptr++ = flash_dword.u64; flash_dword.u64 = source.u64; } } } // Write the source data to the page buffer with 64-bit alignment. for (i = flash_page_source_end.u64ptr - dest.u64ptr; i; i--) { *dest.u64ptr++ = source.u64; } // If the current destination page has an incomplete end... if (incomplete_flash_page_end) { // This is required by the hardware, even if page erase is not requested, // in order to be able to write successfully to erased parts of flash // pages that have already been written to. { tmp.u8ptr = (volatile uint8_t *)dest_end.u8ptr; // If end of destination is not 64-bit aligned... if (!Test_align((uint32_t)dest_end.u8ptr, sizeof(uint64_t))) { // Fill the end of the flash double-word buffer with the current flash page data. for (i = Get_align((uint32_t)dest_end.u8ptr, sizeof(uint64_t)); i < sizeof(uint64_t); i++) flash_dword.u8[i] = *tmp.u8ptr++; // Write the flash double-word buffer to the page buffer. *dest.u64ptr++ = flash_dword.u64; } // Fill the end of the page buffer with the current flash page data. for (; !Test_align((uint32_t)tmp.u64ptr, AVR32_FLASHC_PAGE_SIZE); tmp.u64ptr++) { *tmp.u64ptr = *tmp.u64ptr; } } } // If the current flash page is in the flash array... if (dest.u8ptr <= AVR32_FLASHC_USER_PAGE) { // Erase the current page if requested and write it from the page buffer. if (erase) { flashc_erase_page(-1, false); error_status |= flashc_error_status; } flashc_write_page(-1); error_status |= flashc_error_status; // If the end of the flash array is reached, go to the User page. if (dest.u8ptr >= flash_array_end.u8ptr) { dest.u8ptr = AVR32_FLASHC_USER_PAGE; } } else { // Erase the User page if requested and write it from the page buffer. if (erase) { flashc_erase_user_page(false); error_status |= flashc_error_status; } flashc_write_user_page(); error_status |= flashc_error_status; } } // Update the FLASHC error status. flashc_error_status = error_status; // Return the initial destination pointer as the standard memset function does. return dst; }
volatile void *flashc_memcpy(volatile void *dst, const void *src, size_t nbytes, bool erase) { uint16_t page_pos; Union64 flash_dword; uint8_t i; bool b_user_page; unsigned int error_status = 0; uint8_t* flash_add; uint8_t* dest_add=(uint8_t*)dst; const uint8_t* src_buf=(const uint8_t*)src; // Copy area must be in flash array or flash user page Assert( (((uint8_t *)dst >= AVR32_FLASH) && (((uint8_t *)dst + nbytes) <= (AVR32_FLASH + flashc_get_flash_size()))) || (((uint8_t *)dst >= AVR32_FLASHC_USER_PAGE) && (((uint8_t *)dst + nbytes) <= (AVR32_FLASHC_USER_PAGE + AVR32_FLASHC_USER_PAGE_SIZE))) ); b_user_page = (volatile uint8_t *)dst >= AVR32_FLASHC_USER_PAGE; flash_add = (uint8_t*)((uint32_t)dest_add - ((uint32_t)dest_add % AVR32_FLASHC_PAGE_SIZE)); while (nbytes) { // Clear the page buffer in order to prepare data for a flash page write. flashc_clear_page_buffer(); error_status |= flashc_error_status; // Loop in the page for (page_pos=0; page_pos<AVR32_FLASHC_PAGE_SIZE; page_pos+=sizeof(uint64_t) ) { // Read the flash double-word buffer flash_dword.u64 = *(volatile uint64_t*)flash_add; // Update double-word if necessary for (i = 0; i < sizeof(uint64_t); i++) { if (nbytes && (flash_add == dest_add)) { // Update page with data source flash_dword.u8[i] = *src_buf++; dest_add++; nbytes--; } flash_add++; } // Write the flash double-word buffer to the page buffer. *(volatile uint64_t*)((uint32_t)flash_add - sizeof(uint64_t))= flash_dword.u64; } // Erase the current page if requested and write it from the page buffer. if (erase) { (b_user_page)? flashc_erase_user_page(false) : flashc_erase_page(-1, false); error_status |= flashc_error_status; } // Write the page (b_user_page)? flashc_write_user_page() : flashc_write_page(-1); error_status |= flashc_error_status; } // Update the FLASHC error status. flashc_error_status = error_status; // Return the initial destination pointer as the standard memcpy function does. return dst; }
volatile void* flashc_memcpy (volatile void* dst, const void* src, size_t nbytes, Bool erase) { // Use aggregated pointers to have several alignments available for a same address. UnionCVPtr flash_array_end; UnionVPtr dest; UnionCPtr source; StructCVPtr dest_end; UnionCVPtr flash_page_source_end; Bool incomplete_flash_page_end; Union64 flash_dword; Bool flash_dword_pending = FALSE; UnionVPtr tmp; unsigned int error_status = 0; unsigned int i, j; // Reformat arguments. flash_array_end.u8ptr = AVR32_FLASH + flashc_get_flash_size (); dest.u8ptr = dst; source.u8ptr = src; dest_end.u8ptr = dest.u8ptr + nbytes; // If destination is outside flash, go to next flash page if any. if (dest.u8ptr < AVR32_FLASH) { source.u8ptr += AVR32_FLASH - dest.u8ptr; dest.u8ptr = AVR32_FLASH; } else if (flash_array_end.u8ptr <= dest.u8ptr && dest.u8ptr < AVR32_FLASHC_USER_PAGE) { source.u8ptr += AVR32_FLASHC_USER_PAGE - dest.u8ptr; dest.u8ptr = AVR32_FLASHC_USER_PAGE; } // If end of destination is outside flash, move it to the end of the previous flash page if any. if (dest_end.u8ptr > AVR32_FLASHC_USER_PAGE + AVR32_FLASHC_USER_PAGE_SIZE) { dest_end.u8ptr = AVR32_FLASHC_USER_PAGE + AVR32_FLASHC_USER_PAGE_SIZE; } else if (AVR32_FLASHC_USER_PAGE >= dest_end.u8ptr && dest_end.u8ptr > flash_array_end.u8ptr) { dest_end.u8ptr = flash_array_end.u8ptr; } // Align each end of destination pointer with its natural boundary. dest_end.u16ptr = (U16 *) Align_down ((U32) dest_end.u8ptr, sizeof (U16)); dest_end.u32ptr = (U32 *) Align_down ((U32) dest_end.u16ptr, sizeof (U32)); dest_end.u64ptr = (U64 *) Align_down ((U32) dest_end.u32ptr, sizeof (U64)); // While end of destination is not reached... while (dest.u8ptr < dest_end.u8ptr) { // Clear the page buffer in order to prepare data for a flash page write. flashc_clear_page_buffer (); error_status |= flashc_error_status; // Determine where the source data will end in the current flash page. flash_page_source_end.u64ptr = (U64 *) min ((U32) dest_end.u64ptr, Align_down ((U32) dest.u8ptr, AVR32_FLASHC_PAGE_SIZE) + AVR32_FLASHC_PAGE_SIZE); // Determine if the current destination page has an incomplete end. incomplete_flash_page_end = (Align_down ((U32) dest.u8ptr, AVR32_FLASHC_PAGE_SIZE) >= Align_down ((U32) dest_end.u8ptr, AVR32_FLASHC_PAGE_SIZE)); // If destination does not point to the beginning of the current flash page... if (!Test_align ((U32) dest.u8ptr, AVR32_FLASHC_PAGE_SIZE)) { // Fill the beginning of the page buffer with the current flash page data. // This is required by the hardware, even if page erase is not requested, // in order to be able to write successfully to erased parts of flash // pages that have already been written to. for (tmp.u8ptr = (U8 *) Align_down ((U32) dest.u8ptr, AVR32_FLASHC_PAGE_SIZE); tmp.u64ptr < (U64 *) Align_down ((U32) dest.u8ptr, sizeof (U64)); tmp.u64ptr++) *tmp.u64ptr = *tmp.u64ptr; // If destination is not 64-bit aligned... if (!Test_align ((U32) dest.u8ptr, sizeof (U64))) { // Fill the beginning of the flash double-word buffer with the current // flash page data. // This is required by the hardware, even if page erase is not // requested, in order to be able to write successfully to erased parts // of flash pages that have already been written to. for (i = 0; i < Get_align ((U32) dest.u8ptr, sizeof (U64)); i++) flash_dword.u8[i] = *tmp.u8ptr++; // Fill the end of the flash double-word buffer with the source data. for (; i < sizeof (U64); i++) flash_dword.u8[i] = *source.u8ptr++; // Align the destination pointer with its 64-bit boundary. dest.u64ptr = (U64 *) Align_down ((U32) dest.u8ptr, sizeof (U64)); // If the current destination double-word is not the last one... if (dest.u64ptr < dest_end.u64ptr) { // Write the flash double-word buffer to the page buffer. *dest.u64ptr++ = flash_dword.u64; } // If the current destination double-word is the last one, the flash // double-word buffer must be kept for later. else flash_dword_pending = TRUE; } } // Read the source data with the maximal possible alignment and write it to // the page buffer with 64-bit alignment. switch (Get_align ((U32) source.u8ptr, sizeof (U32))) { case 0: for (i = flash_page_source_end.u64ptr - dest.u64ptr; i; i--) *dest.u64ptr++ = *source.u64ptr++; break; case sizeof (U16): for (i = flash_page_source_end.u64ptr - dest.u64ptr; i; i--) { for (j = 0; j < sizeof (U64) / sizeof (U16); j++) flash_dword.u16[j] = *source.u16ptr++; *dest.u64ptr++ = flash_dword.u64; } break; default: for (i = flash_page_source_end.u64ptr - dest.u64ptr; i; i--) { for (j = 0; j < sizeof (U64); j++) flash_dword.u8[j] = *source.u8ptr++; *dest.u64ptr++ = flash_dword.u64; } } // If the current destination page has an incomplete end... if (incomplete_flash_page_end) { // If the flash double-word buffer is in use, do not initialize it. if (flash_dword_pending) i = Get_align ((U32) dest_end.u8ptr, sizeof (U64)); // If the flash double-word buffer is free... else { // Fill the beginning of the flash double-word buffer with the source data. for (i = 0; i < Get_align ((U32) dest_end.u8ptr, sizeof (U64)); i++) flash_dword.u8[i] = *source.u8ptr++; } // This is required by the hardware, even if page erase is not requested, // in order to be able to write successfully to erased parts of flash // pages that have already been written to. { tmp.u8ptr = (volatile U8 *) dest_end.u8ptr; // If end of destination is not 64-bit aligned... if (!Test_align ((U32) dest_end.u8ptr, sizeof (U64))) { // Fill the end of the flash double-word buffer with the current flash page data. for (; i < sizeof (U64); i++) flash_dword.u8[i] = *tmp.u8ptr++; // Write the flash double-word buffer to the page buffer. *dest.u64ptr++ = flash_dword.u64; } // Fill the end of the page buffer with the current flash page data. for (; !Test_align ((U32) tmp.u64ptr, AVR32_FLASHC_PAGE_SIZE); tmp.u64ptr++) *tmp.u64ptr = *tmp.u64ptr; } } // If the current flash page is in the flash array... if (dest.u8ptr <= AVR32_FLASHC_USER_PAGE) { // Erase the current page if requested and write it from the page buffer. if (erase) { flashc_erase_page (-1, FALSE); error_status |= flashc_error_status; } flashc_write_page (-1); error_status |= flashc_error_status; // If the end of the flash array is reached, go to the User page. if (dest.u8ptr >= flash_array_end.u8ptr) { source.u8ptr += AVR32_FLASHC_USER_PAGE - dest.u8ptr; dest.u8ptr = AVR32_FLASHC_USER_PAGE; } } // If the current flash page is the User page... else { // Erase the User page if requested and write it from the page buffer. if (erase) { flashc_erase_user_page (FALSE); error_status |= flashc_error_status; } flashc_write_user_page (); error_status |= flashc_error_status; } } // Update the FLASHC error status. flashc_error_status = error_status; // Return the initial destination pointer as the standard memcpy function does. return dst; }