use HSRUN functions in eeprom

teensy3/eeprom.c

 		if (stat) FTFL_FSTAT = stat;
 		// FlexRAM is configured as traditional RAM
 		// We need to reconfigure for EEPROM usage
+		kinetis_hsrun_disable();
 		FTFL_FCCOB0 = 0x80; // PGMPART = Program Partition Command
 		FTFL_FCCOB3 = 0;
 		FTFL_FCCOB4 = EEESPLIT | EEESIZE;
 		// do_flash_cmd() must execute from RAM.  Luckily the C syntax is simple...
 		(*((void (*)(volatile uint8_t *))((uint32_t)do_flash_cmd | 1)))(&FTFL_FSTAT);
 		__enable_irq();
+		kinetis_hsrun_enable();
 		status = FTFL_FSTAT;
 		if (status & 0x70) {
 			FTFL_FSTAT = (status & 0x70);
 	}
 }
 
-#if F_CPU > 120000000 && defined(__MK66FX1M0__)
-static volatile uint16_t c_intrestore = 0;
-
-void c_enable_irq( void );
-void c_disable_irq( void );
-static __inline__ uint32_t __get_primask(void) \
-{ uint32_t primask = 0; \
-  __asm__ volatile ("MRS %[result], PRIMASK\n\t":[result]"=r"(primask)::); \
-  return primask; } // returns 0 if interrupts enabled, 1 if disabled
-void c_enable_irq( void ){
-	if ( c_intrestore ) {
-		c_intrestore =0;
-		__enable_irq( );
-	}
-}
-void c_disable_irq( void ){
-	if ( !__get_primask() ) { // Returns 0 if they are enabled, or non-zero if disabled 
-		__disable_irq( );
-		c_intrestore = 1;		
-	}
-}
-static volatile uint8_t restore_hsrun = 0;
-static void hsrun_off(void)
-{
-	if (SMC_PMSTAT == SMC_PMSTAT_HSRUN) {
-		c_disable_irq( ); // Turn off interrupts for the DURATION !!!!
-		SMC_PMCTRL = SMC_PMCTRL_RUNM(0); // exit HSRUN mode
-		while (SMC_PMSTAT == SMC_PMSTAT_HSRUN) delayMicroseconds(2); // wait for !HSRUN
-		delayMicroseconds(100);
-		restore_hsrun = 1;
-	}
-}
-
-static void hsrun_on(void)
-{
-	if (restore_hsrun) {
-	    SMC_PMCTRL = SMC_PMCTRL_RUNM(3); // enter HSRUN mode
-	    while (SMC_PMSTAT != SMC_PMSTAT_HSRUN) delayMicroseconds(2);; // wait for HSRUN
-		restore_hsrun = 0;
-	    c_enable_irq( ); // Restore interrupts only when HSRUN restored	}
-	}
-}
-#else
-#define hsrun_off()
-#define hsrun_on()
-#endif
-
-
 void eeprom_write_byte(uint8_t *addr, uint8_t value)
 {
 	uint32_t offset = (uint32_t)addr;
 
 	if (offset >= EEPROM_SIZE) return;
 	if (!(FTFL_FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
-	hsrun_off();
 	if (FlexRAM[offset] != value) {
+		kinetis_hsrun_disable();
 		uint8_t stat = FTFL_FSTAT & 0x70;
 		if (stat) FTFL_FSTAT = stat;
 		FlexRAM[offset] = value;
 		flexram_wait();
+		kinetis_hsrun_enable();
 	}
-	hsrun_on();
 }
 
 void eeprom_write_word(uint16_t *addr, uint16_t value)
 
 	if (offset >= EEPROM_SIZE-1) return;
 	if (!(FTFL_FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
-	hsrun_off();
 #ifdef HANDLE_UNALIGNED_WRITES
 	if ((offset & 1) == 0) {
 #endif
 		if (*(uint16_t *)(&FlexRAM[offset]) != value) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			*(uint16_t *)(&FlexRAM[offset]) = value;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 #ifdef HANDLE_UNALIGNED_WRITES
 	} else {
 		if (FlexRAM[offset] != value) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			FlexRAM[offset] = value;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 		if (FlexRAM[offset + 1] != (value >> 8)) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			FlexRAM[offset + 1] = value >> 8;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 	}
 #endif
-	hsrun_on();
 }
 
 void eeprom_write_dword(uint32_t *addr, uint32_t value)
 
 	if (offset >= EEPROM_SIZE-3) return;
 	if (!(FTFL_FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
-	hsrun_off();
 #ifdef HANDLE_UNALIGNED_WRITES
 	switch (offset & 3) {
 	case 0:
 #endif
 		if (*(uint32_t *)(&FlexRAM[offset]) != value) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			*(uint32_t *)(&FlexRAM[offset]) = value;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
-		hsrun_on();
 		return;
 #ifdef HANDLE_UNALIGNED_WRITES
 	case 2:
 		if (*(uint16_t *)(&FlexRAM[offset]) != value) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			*(uint16_t *)(&FlexRAM[offset]) = value;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 		if (*(uint16_t *)(&FlexRAM[offset + 2]) != (value >> 16)) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			*(uint16_t *)(&FlexRAM[offset + 2]) = value >> 16;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
-		hsrun_on();
 		return;
 	default:
 		if (FlexRAM[offset] != value) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			FlexRAM[offset] = value;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 		if (*(uint16_t *)(&FlexRAM[offset + 1]) != (value >> 8)) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			*(uint16_t *)(&FlexRAM[offset + 1]) = value >> 8;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 		if (FlexRAM[offset + 3] != (value >> 24)) {
+			kinetis_hsrun_disable();
 			uint8_t stat = FTFL_FSTAT & 0x70;
 			if (stat) FTFL_FSTAT = stat;
 			FlexRAM[offset + 3] = value >> 24;
 			flexram_wait();
+			kinetis_hsrun_enable();
 		}
 	}
 #endif
-	hsrun_on();
 }
 
 void eeprom_write_block(const void *buf, void *addr, uint32_t len)
 
 	if (offset >= EEPROM_SIZE) return;
 	if (!(FTFL_FCNFG & FTFL_FCNFG_EEERDY)) eeprom_initialize();
-	hsrun_off();
 	if (len >= EEPROM_SIZE) len = EEPROM_SIZE;
 	if (offset + len >= EEPROM_SIZE) len = EEPROM_SIZE - offset;
 	while (len > 0) {
 			val32 |= (*src++ << 16);
 			val32 |= (*src++ << 24);
 			if (*(uint32_t *)(&FlexRAM[offset]) != val32) {
+				kinetis_hsrun_disable();
 				uint8_t stat = FTFL_FSTAT & 0x70;
 				if (stat) FTFL_FSTAT = stat;
 				*(uint32_t *)(&FlexRAM[offset]) = val32;
 				flexram_wait();
+				kinetis_hsrun_enable();
 			}
 			offset += 4;
 			len -= 4;
 			val16 = *src++;
 			val16 |= (*src++ << 8);
 			if (*(uint16_t *)(&FlexRAM[offset]) != val16) {
+				kinetis_hsrun_disable();
 				uint8_t stat = FTFL_FSTAT & 0x70;
 				if (stat) FTFL_FSTAT = stat;
 				*(uint16_t *)(&FlexRAM[offset]) = val16;
 				flexram_wait();
+				kinetis_hsrun_enable();
 			}
 			offset += 2;
 			len -= 2;
 			// write 8 bits
 			uint8_t val8 = *src++;
 			if (FlexRAM[offset] != val8) {
+				kinetis_hsrun_disable();
 				uint8_t stat = FTFL_FSTAT & 0x70;
 				if (stat) FTFL_FSTAT = stat;
 				FlexRAM[offset] = val8;
 				flexram_wait();
+				kinetis_hsrun_enable();
 			}
 			offset++;
 			len--;
 		}
 	}
-	hsrun_on();
 }
 
 /*