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;
}
