1 files changed, 12 insertions, 29 deletions

diff --git a/rf_test/rfm.c b/rf_test/rfm.c
index c48add4..c55a3b0 100644
--- a/rf_test/rfm.c
+++ b/rf_test/rfm.c
@@ -24,47 +24,30 @@ static inline void spi_init(void)
 	SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR0);
 }
 
-static inline uint8_t spi_read(uint8_t addr)
+static inline void send_cmd(uint8_t addr, uint8_t val)
 {
 	SS_PORT |= (1 << SS_PIN);
 
-	SPDR = addr;
+check_val:
+	SPDR = addr | READ_MASK;
 	while (!(SPSR & (1 << SPIF)))
 		;
 
-	addr = SPDR;
-
-	SS_PORT &= ~(1 << SS_PIN);
-
-	return addr;
-}
+	if (SPDR != val) {
+		SPDR = addr | WRITE_MASK;
+		while (!(SPSR & (1 << SPIF)))
+			;
 
-static inline void spi_write(uint8_t addr, uint8_t data)
-{
-	SS_PORT |= (1 << SS_PIN);
-
-	SPDR = addr;
-	while (!(SPSR & (1 << SPIF)))
-		;
+		SPDR = val;
+		while (!(SPSR & (1 << SPIF)))
+			;
 
-	SPDR = data;
-	while (!(SPSR & (1 << SPIF)))
-		;
+		goto check_val;
+	}
 
 	SS_PORT &= ~(1 << SS_PIN);
 }
 
-static inline void send_cmd(uint8_t addr, uint8_t val)
-{
-	uint8_t ra, wa;
-
-	ra = addr | READ_MASK;
-	wa = addr | WRITE_MASK; 
-
-	while (spi_read(ra) != val)
-		spi_write(wa, val);
-}
-
 void rfm_init(uint8_t addr)
 {
 	spi_init();