1 files changed, 4 insertions, 13 deletions

diff --git a/main.c b/main.c
index de3d7ff..01d426c 100644
--- a/main.c
+++ b/main.c
@@ -38,9 +38,9 @@ static inline uint8_t read_reg(uint8_t reg)
 	return SPDR;
 }
 
-static inline void write_reg(uint8_t reg, uint8_t val)
+static inline void write_reg_check(uint8_t reg, uint8_t val, uint8_t ref)
 {
-	while (read_reg(reg) != val) {
+	while (read_reg(reg) != ref) {
 		SPI_PORT &= ~(1 << SPI_SS);
 		SPDR = (reg & 0x1F) | 0x20;
 		while (!(SPSR & (1 << SPIF)))
@@ -52,18 +52,9 @@ static inline void write_reg(uint8_t reg, uint8_t val)
 	}
 }
 
-static inline void write_reg_check(uint8_t reg, uint8_t val, uint8_t ref)
+static inline void write_reg(uint8_t reg, uint8_t val)
 {
-	while (read_reg(reg) != ref) {
-		SPI_PORT &= ~(1 << SPI_SS);
-		SPDR = (reg & 0x1F) | 0x20;
-		while (!(SPSR & (1 << SPIF)))
-			;
-		SPDR = val;
-		while (!(SPSR & (1 << SPIF)))
-			;
-		SPI_PORT |= (1 << SPI_SS);
-	}
+	write_reg_check(reg, val, val);
 }
 
 static inline void print_config(void)