//Programming Loop
{
int ReservedBlock = 0;
int StartBlock = ReservedBlock + 1;
int blocknum = StartBlock;
int block_start;
int i, j;
while (blocknum * erase_sz < fw->size){
block_start = blocknum * erase_sz;
for (i = 0; i < erase_sz && fw->data[block_start + i] == 0 ; i++){ }
if (i < erase_sz){ // found a non-zero byte
flash_start();
sslbp_write_long(LBPFLASHOFFSETLOC, block_start);
sslbp_write_byte(LBPFLASHCOMMITLOC, FLASHERASE_CMD);
if (sslbp_read_cookie() != LBPCOOKIE){
HM2_ERR("Synch failed during block erase: aborting\n");
goto fail0;
}
flash_stop();
HM2_PRINT("Erased block %i\n", blocknum);
flash_start();
for (i = 0; i < erase_sz ; i += write_sz){
sslbp_write_long(LBPFLASHOFFSETLOC, block_start + i);
for (j = 0 ; j < write_sz ; j += 8){
u32 data0, data1;
memcpy(&data0, &(fw->data[j]), 4);
memcpy(&data1, &(fw->data[j + 4]), 4);
sslbp_write_double(j, data0, data1);
}
sslbp_write_byte(LBPFLASHCOMMITLOC, FLASHWRITE_CMD);
if (sslbp_read_cookie() != LBPCOOKIE){
HM2_ERR("Synch failed during block write: aborting\n");
goto fail0;
}
}
flash_stop();
HM2_PRINT("Wrote block %i\n", blocknum);
}
else // Looks like an all-zeros block
{
HM2_PRINT("Skipped Block %i\n", blocknum);
}
blocknum++;
}
}