uint32_t flashmem_write( const void *from, uint32_t toaddr, uint32_t size )
{
uint32_t temp, rest, ssize = size;
unsigned i;
char tmpdata[ INTERNAL_FLASH_WRITE_UNIT_SIZE ];
const uint8_t *pfrom = ( const uint8_t* )from;
const uint32_t blksize = INTERNAL_FLASH_WRITE_UNIT_SIZE;
const uint32_t blkmask = INTERNAL_FLASH_WRITE_UNIT_SIZE - 1;
// Align the start
if( toaddr & blkmask )
{
rest = toaddr & blkmask;
temp = toaddr & ~blkmask; // this is the actual aligned address
// c_memcpy( tmpdata, ( const void* )temp, blksize );
flashmem_read_internal( tmpdata, temp, blksize );
for( i = rest; size && ( i < blksize ); i ++, size --, pfrom ++ )
tmpdata[ i ] = *pfrom;
flashmem_write_internal( tmpdata, temp, blksize );
if( size == 0 )
return ssize;
toaddr = temp + blksize;
}
// The start address is now a multiple of blksize
// Compute how many bytes we can write as multiples of blksize
rest = size & blkmask;
temp = size & ~blkmask;
// Program the blocks now
if( temp )
{
flashmem_write_internal( pfrom, toaddr, temp );
toaddr += temp;
pfrom += temp;
}
// And the final part of a block if needed
if( rest )
{
// c_memcpy( tmpdata, ( const void* )toaddr, blksize );
flashmem_read_internal( tmpdata, toaddr, blksize );
for( i = 0; size && ( i < rest ); i ++, size --, pfrom ++ )
tmpdata[ i ] = *pfrom;
flashmem_write_internal( tmpdata, toaddr, blksize );
}
return ssize;
}
uint32_t flashmem_read( void *to, uint32_t fromaddr, uint32_t size )
{
uint32_t temp, rest, ssize = size;
unsigned i;
char tmpdata[ INTERNAL_FLASH_READ_UNIT_SIZE ];
uint8_t *pto = ( uint8_t* )to;
const uint32_t blksize = INTERNAL_FLASH_READ_UNIT_SIZE;
const uint32_t blkmask = INTERNAL_FLASH_READ_UNIT_SIZE - 1;
// Align the start
if( fromaddr & blkmask )
{
rest = fromaddr & blkmask;
temp = fromaddr & ~blkmask; // this is the actual aligned address
flashmem_read_internal( tmpdata, temp, blksize );
for( i = rest; size && ( i < blksize ); i ++, size --, pto ++ )
*pto = tmpdata[ i ];
if( size == 0 )
return ssize;
fromaddr = temp + blksize;
}
// The start address is now a multiple of blksize
// Compute how many bytes we can read as multiples of blksize
rest = size & blkmask;
temp = size & ~blkmask;
// Program the blocks now
if( temp )
{
flashmem_read_internal( pto, fromaddr, temp );
fromaddr += temp;
pto += temp;
}
// And the final part of a block if needed
if( rest )
{
flashmem_read_internal( tmpdata, fromaddr, blksize );
for( i = 0; size && ( i < rest ); i ++, size --, pto ++ )
*pto = tmpdata[ i ];
}
return ssize;
}
