flashc.c

new freertos source code V5.0.3

    // 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));

    // 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((U32)dest.u8ptr, AVR32_FLASHC_PAGE_SIZE))
    {
      // If page erase is requested...
      if (erase)
      {
        // Fill the beginning of the page buffer with the current flash page data.
        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)))
      {
        // If page erase is requested...
        if (erase)
        {
          // Fill the beginning of the flash double-word buffer with the current flash page data.
          for (i = 0; i < Get_align((U32)dest.u8ptr, sizeof(U64)); i++)
            flash_dword.u8[i] = *tmp.u8ptr++;
        }
        // If page erase is not requested...
        else
        {
          // Erase the beginning of the flash double-word buffer.
          for (i = 0; i < Get_align((U32)dest.u8ptr, sizeof(U64)); i++)
            flash_dword.u8[i] = 0xFF;
        }

        // 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 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)
    {
      // If page erase is requested...
      if (erase)
      {
        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 = Get_align((U32)dest_end.u8ptr, sizeof(U64)); 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 page erase is not requested but end of destination is not 64-bit aligned...
      else if (!Test_align((U32)dest_end.u8ptr, sizeof(U64)))
      {
        // Erase the end of the flash double-word buffer.
        for (i = Get_align((U32)dest_end.u8ptr, sizeof(U64)); i < sizeof(U64); i++)
          flash_dword.u8[i] = 0xFF;

        // Write the flash double-word buffer to the page buffer.
        *dest.u64ptr++ = flash_dword.u64;
      }
    }

    // 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;
    }
    // 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 memset 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))
    {
      // If page erase is requested...
      if (erase)
      {
        // Fill the beginning of the page buffer with the current flash page data.
        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)))
      {
        // If page erase is requested...
        if (erase)
        {
          // Fill the beginning of the flash double-word buffer with the current flash page data.
          for (i = 0; i < Get_align((U32)dest.u8ptr, sizeof(U64)); i++)
            flash_dword.u8[i] = *tmp.u8ptr++;
        }
        // If page erase is not requested...
        else
        {
          // Erase the beginning of the flash double-word buffer.
          for (i = 0; i < Get_align((U32)dest.u8ptr, sizeof(U64)); i++)
            flash_dword.u8[i] = 0xFF;
        }

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

      // If page erase is requested...
      if (erase)
      {
        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 page erase is not requested but end of destination is not 64-bit aligned...
      else if (!Test_align((U32)dest_end.u8ptr, sizeof(U64)))
      {
        // Erase the end of the flash double-word buffer.
        for (; i < sizeof(U64); i++)
          flash_dword.u8[i] = 0xFF;

        // Write the flash double-word buffer to the page buffer.
        *dest.u64ptr++ = flash_dword.u64;
      }
    }

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


//! @}