/*!
* \brief Erase and program a page in the flash ROM.
*
* \param page The page number to program, 0..479.
* \param data Pointer to the new page contents.
* \param len Number of bytes to program. If this is less than 256,
* then the remaining bytes will be filled with 0xFF.
*/
static void FlashPage(u_short page, void *data, u_short len)
{
u_short i;
u_short *wp = data;
if (len > 256)
len = 256;
if (page >= 256) {
if (page >= 496)
return;
RAMPZ = 1;
} else
RAMPZ = 0;
page <<= 8;
SpmCommand(page, (1 << PGERS) | (1 << SPMEN));
SpmCommand(0, (1 << RWWSRE) | (1 << SPMEN));
for (i = 0; i < len; i += 2, wp++)
SpmBufferFill(i, *wp);
for (; i < 256; i += 2)
SpmBufferFill(i, 0xFFFF);
SpmCommand(page, (1 << PGWRT) | (1 << SPMEN));
SpmCommand(0, (1 << RWWSRE) | (1 << SPMEN));
}
void flash_write( uint32_t address, uint8_t *data, uint16_t len )
{
uint16_t page;
uint16_t offset;
uint16_t i;
uint32_t page_address;
HAS_CRITICAL_SECTION;
//if( address < ((uint32_t)(&__text_end)) || address > FLASHMEM_SIZE) {
// led_red_toggle();
// Disallow writing to kernel or bootloader
// return;
//}
ENTER_CRITICAL_SECTION();
//! making sure that EEPROM is not busy
eeprom_busy_wait ();
//
// For each different page
//
while( len > 0 ) {
page = (uint16_t)(address >> 8);
offset = (uint16_t)(address & 0x000000ff);
//
// Load data from the page into temperary buffer
//
page_address = address & 0xffffff00;
for( i = 0; i < offset; i+=2 ) {
uint16_t w = pgm_read_word_far( page_address + i );
SpmBufferFill( i, w );
}
//
// Modify the content
//
for( ; len > 0 && i < 256L; i+=2 ) {
uint16_t w = *data++;
w += (*data++) << 8;
len -= 2;
SpmBufferFill( i, w );
}
//
// load the rest of the page
//
for( ; i < 256L; i+=2 ) {
uint16_t w = pgm_read_word_far( page_address + i );
SpmBufferFill( i, w );
}
//
// Write it back
//
if( page >= 256L ) {
RAMPZ = 1;
} else {
RAMPZ = 0;
}
//
// Prepare the page address
//
page <<= 8;
SpmCommand(page, (1 << PGERS) | (1 << SPMEN));
SpmCommand(page, (1 << PGWRT) | (1 << SPMEN));
SpmCommand(0, (1 << RWWSRE) | (1 << SPMEN));
address += (256L - offset);
}
RAMPZ = 0;
LEAVE_CRITICAL_SECTION();
}
