3 files changed, 44 insertions, 44 deletions

diff --git a/src/drivers/pc80/rtc/mc146818rtc.c b/src/drivers/pc80/rtc/mc146818rtc.c
index 2029b1e51f..1d2fc97a74 100644
--- a/src/drivers/pc80/rtc/mc146818rtc.c
+++ b/src/drivers/pc80/rtc/mc146818rtc.c
@@ -81,6 +81,12 @@ static void cmos_set_checksum(int range_start, int range_end, int cks_loc)
 	cmos_write(((sum >> 0) & 0x0ff), cks_loc + 1);
 }
 
+/* See if the CMOS error condition has been flagged */
+int cmos_error(void)
+{
+	return (cmos_read(RTC_VALID) & RTC_VRT) == 0;
+}
+
 #define RTC_CONTROL_DEFAULT (RTC_24H)
 #define RTC_FREQ_SELECT_DEFAULT (RTC_REF_CLCK_32KHZ | RTC_RATE_1024HZ)
 
@@ -90,7 +96,6 @@ static bool __cmos_init(bool invalid)
 	bool checksum_invalid = false;
 	bool clear_cmos;
 	size_t i;
-	uint8_t x;
 
 	/*
 	 * Avoid clearing pending interrupts and resetting the RTC control
@@ -104,8 +109,7 @@ static bool __cmos_init(bool invalid)
 	printk(BIOS_DEBUG, "RTC Init\n");
 
 	/* See if there has been a CMOS power problem. */
-	x = cmos_read(RTC_VALID);
-	cmos_invalid = !(x & RTC_VRT);
+	cmos_invalid = cmos_error();
 
 	if (CONFIG(USE_OPTION_TABLE)) {
 		/* See if there is a CMOS checksum error */
@@ -118,7 +122,7 @@ static bool __cmos_init(bool invalid)
 	}
 
 	if (cmos_invalid || invalid)
-		cmos_write(cmos_read(RTC_CONTROL) | RTC_SET, RTC_CONTROL);
+		cmos_disable_rtc();
 
 	if (invalid || cmos_invalid || checksum_invalid) {
 		if (clear_cmos) {
@@ -407,6 +411,17 @@ enum cb_err set_option(const char *name, void *value)
 }
 
 /*
+ * Upon return the caller is guaranteed 244 microseconds to complete any
+ * RTC operations. wait_uip may be called a single time prior to multiple
+ * accesses, but sequences requiring more time should call wait_uip again.
+ */
+static void wait_uip(void)
+{
+	while (cmos_read(RTC_REG_A) & RTC_UIP)
+		;
+}
+
+/*
  * If the CMOS is cleared, the rtc_reg has the invalid date. That
  * hurts some OSes. Even if we don't set USE_OPTION_TABLE, we need
  * to make sure the date is valid.
diff --git a/src/drivers/pc80/rtc/mc146818rtc_boot.c b/src/drivers/pc80/rtc/mc146818rtc_boot.c
index 3000946413..a52e2220f1 100644
--- a/src/drivers/pc80/rtc/mc146818rtc_boot.c
+++ b/src/drivers/pc80/rtc/mc146818rtc_boot.c
@@ -19,14 +19,6 @@
 #include <option_table.h>
 #endif
 
-int cmos_error(void)
-{
-	unsigned char reg_d;
-	/* See if the cmos error condition has been flagged */
-	reg_d = cmos_read(RTC_REG_D);
-	return (reg_d & RTC_VRT) == 0;
-}
-
 int cmos_chksum_valid(void)
 {
 #if CONFIG(USE_OPTION_TABLE)
@@ -59,10 +51,10 @@ void sanitize_cmos(void)
 					CBFS_COMPONENT_CMOS_DEFAULT, &length);
 		if (cmos_default) {
 			size_t i;
-			cmos_disable_rtc();
+			u8 control_state = cmos_disable_rtc();
 			for (i = 14; i < MIN(128, length); i++)
 				cmos_write_inner(cmos_default[i], i);
-			cmos_enable_rtc();
+			cmos_restore_rtc(control_state);
 		}
 	}
 }
@@ -72,22 +64,22 @@ void sanitize_cmos(void)
 #error "CONFIG_MAX_REBOOT_CNT too high"
 #endif
 
-static inline int boot_count(uint8_t rtc_byte)
+static int boot_count(uint8_t rtc_byte)
 {
 	return rtc_byte >> 4;
 }
 
-static inline uint8_t increment_boot_count(uint8_t rtc_byte)
+static uint8_t increment_boot_count(uint8_t rtc_byte)
 {
 	return rtc_byte + (1 << 4);
 }
 
-static inline uint8_t boot_set_fallback(uint8_t rtc_byte)
+static uint8_t boot_set_fallback(uint8_t rtc_byte)
 {
 	return rtc_byte & ~RTC_BOOT_NORMAL;
 }
 
-static inline int boot_use_normal(uint8_t rtc_byte)
+static int boot_use_normal(uint8_t rtc_byte)
 {
 	return rtc_byte & RTC_BOOT_NORMAL;
 }
diff --git a/src/include/pc80/mc146818rtc.h b/src/include/pc80/mc146818rtc.h
index a8221c7259..e2f44941d3 100644
--- a/src/include/pc80/mc146818rtc.h
+++ b/src/include/pc80/mc146818rtc.h
@@ -114,16 +114,6 @@ static inline unsigned char cmos_read(unsigned char addr)
 	return inb(RTC_BASE_PORT + offs + 1);
 }
 
-/* Upon return the caller is guaranteed 244 microseconds to complete any
- * RTC operations.  wait_uip may be called a single time prior to multiple
- * accesses, but sequences requiring more time should call wait_uip again.
- */
-static inline void wait_uip(void)
-{
-	while (cmos_read(RTC_REG_A) & RTC_UIP)
-		;
-}
-
 static inline void cmos_write_inner(unsigned char val, unsigned char addr)
 {
 	int offs = 0;
@@ -135,31 +125,34 @@ static inline void cmos_write_inner(unsigned char val, unsigned char addr)
 	outb(val, RTC_BASE_PORT + offs + 1);
 }
 
-static inline void cmos_write(unsigned char val, unsigned char addr)
+static inline u8 cmos_disable_rtc(void)
 {
 	u8 control_state = cmos_read(RTC_CONTROL);
-	/* There are various places where RTC bits might be hiding,
-	 * eg. the Century / AltCentury byte. So to be safe, disable
-	 * RTC before changing any value.
-	 */
-	if ((addr != RTC_CONTROL) && !(control_state & RTC_SET))
+	if (!(control_state & RTC_SET))
 		cmos_write_inner(control_state | RTC_SET, RTC_CONTROL);
-	cmos_write_inner(val, addr);
-	/* reset to prior configuration */
-	if ((addr != RTC_CONTROL) && !(control_state & RTC_SET))
-		cmos_write_inner(control_state, RTC_CONTROL);
+	return control_state;
 }
 
-static inline void cmos_disable_rtc(void)
+static inline void cmos_restore_rtc(u8 control_state)
 {
-	u8 control_state = cmos_read(RTC_CONTROL);
-	cmos_write(control_state | RTC_SET, RTC_CONTROL);
+	if (!(control_state & RTC_SET))
+		cmos_write_inner(control_state, RTC_CONTROL);
 }
 
-static inline void cmos_enable_rtc(void)
+static inline void cmos_write(unsigned char val, unsigned char addr)
 {
-	u8 control_state = cmos_read(RTC_CONTROL);
-	cmos_write(control_state & ~RTC_SET, RTC_CONTROL);
+	u8 control_state;
+
+	/*
+	 * There are various places where RTC bits might be hiding,
+	 * eg. the Century / AltCentury byte. So to be safe, disable
+	 * RTC before changing any value.
+	 */
+	if (addr != RTC_CONTROL)
+		control_state = cmos_disable_rtc();
+	cmos_write_inner(val, addr);
+	if (addr != RTC_CONTROL)
+		cmos_restore_rtc(control_state);
 }
 
 static inline u32 cmos_read32(u8 offset)