roboforum.ru

void config_int(uint16_t address, uint16_t value)
{
   uint16_t page_addr, page_end;
   page_addr = (address & ~(SPM_PAGESIZE - 1));
   page_end = ((address + 2) & ~(SPM_PAGESIZE - 1)) + SPM_PAGESIZE;
   ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
      eeprom_busy_wait();
      while (page_addr < page_end) {
         uint16_t w;
         if (page_addr == address) {
            w = value;
         } else if (page_addr == address - 1) {
            w = pgm_read_word(page_addr);
            w = (w & 0xFF) | (value << 8);
         } else if (page_addr == address + 1) {
            w = pgm_read_word(page_addr);
            w = (w & 0xFF00) | (value >>8);
         } else {
            w = pgm_read_word(page_addr);
         }
         boot_page_fill(page_addr, w);
         if ((page_addr & (SPM_PAGESIZE - 1)) == (SPM_PAGESIZE - 2)) {
            boot_page_erase(page_addr & ~(SPM_PAGESIZE-1));
            boot_spm_busy_wait();
            boot_page_write (page_addr & ~(SPM_PAGESIZE-1));
            boot_spm_busy_wait();
         }
         page_addr += 2;
      }
        boot_rww_enable ();
   }
}