1 files changed, 90 insertions, 1 deletions
diff --git a/main.cpp b/main.cpp
index 22ff85a..6bdabd0 100644
--- a/main.cpp
+++ b/main.cpp
@@ -13,14 +13,103 @@ void* vectors[4] __attribute__((section(".vectors"))) = {
 
 volatile unsigned int cnt;
 
+void i2c_write_reg(uint8_t addr, uint8_t reg, uint8_t data) {
+	I2C2.CR1 |= 0x100;
+	while(!(I2C2.SR1 & 0x01));
+	
+	I2C2.DR = (addr << 1) | 0;
+	while (!(I2C2.SR1 & 0x02));
+	I2C2.SR2;
+	
+	I2C2.DR = reg;
+	while (!(I2C2.SR1 & 0x80));
+	
+	I2C2.DR = data;
+	while (!(I2C2.SR1 & 0x04));
+	
+	I2C2.CR1 |= 0x200;
+}
+
+void i2c_read_reg(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* buf) {
+	I2C2.CR1 |= 0x100;
+	while(!(I2C2.SR1 & 0x01));
+	
+	I2C2.DR = (addr << 1) | 0;
+	while (!(I2C2.SR1 & 0x02));
+	I2C2.SR2;
+	
+	I2C2.DR = reg;
+	while (!(I2C2.SR1 & 0x80));
+	
+	I2C2.CR1 |= 0x100;
+	while(!(I2C2.SR1 & 0x01));
+	
+	I2C2.DR = (addr << 1) | 1;
+	while (!(I2C2.SR1 & 0x02));
+	I2C2.SR2;
+	
+	I2C2.CR1 |= 0x400; // Set ACK.
+	
+	while(len) {
+		if(len == 3) {
+			while (!(I2C2.SR1 & 0x04)); // Wait for BTF
+			
+			I2C2.CR1 &= ~0x400; // Clear ACK.
+			*buf++ = I2C2.DR;
+			len--;
+			
+			I2C2.CR1 |= 0x200; // Set STOP.
+			*buf++ = I2C2.DR;
+			len--;
+			
+		} else {
+			while (!(I2C2.SR1 & 0x40)); // Wait for RXNE
+			
+			*buf++ = I2C2.DR;
+			len--;
+		}
+	}
+}
+
 int main() {
-	RCC.APB2ENR |= 0x5;
+	RCC.APB2ENR |= 0xd;
 	
 	GPIOA.CRL = 0x44344444;
 	GPIOA.ODR = 1 << 5;
 	
+	GPIOB.CRH = 0x4444ff44;
+	
+	cnt = 0;
+	while(cnt++ < (1 << 20));
+	
+	// 100 kHz.
+	RCC.APB1ENR = (1 << 22);
+	while(cnt++ < (1 << 20));
+	
+	I2C2.CR1 = 0x8000;
+	I2C2.CR1 = 0;
+	
+	I2C2.CR2 = 36;
+	I2C2.TRISE = 37;
+	I2C2.CCR = 180;
+	I2C2.CR1 = 1;
+	
+	i2c_write_reg(0x68, 0x3e, 0x03);
+	i2c_write_reg(0x68, 0x16, 0x18 | 0x02);
+	
+	uint8_t buf[6];
+	
 	while(1) {
 		cnt++;
+		if(cnt & (1 << 20)) {
+			GPIOA.ODR = 1 << 5;
+		} else {
+			GPIOA.ODR = 0;
+		}
+		
+		if(!(cnt & ((1 << 20) - 1))) {
+			i2c_read_reg(0x68, 0x1d, 6, buf);
+		}
 	}
 }