zephyr hacky i2c test

i2c_lsm9ds0_test.patch

diff --git a/samples/drivers/i2c_lsm9ds0/src/main.c b/samples/drivers/i2c_lsm9ds0/src/main.c
index 5622e13..43a5670 100644
--- a/samples/drivers/i2c_lsm9ds0/src/main.c
+++ b/samples/drivers/i2c_lsm9ds0/src/main.c
@@ -15,60 +15,126 @@
  */
 
 #include <zephyr.h>
+#include <device.h>
+
+#include <nanokernel.h>
 
 #include <misc/printk.h>
 
-#include <device.h>
 #include <i2c.h>
 
-#define GYRO_I2C_ADDR 0x6A
-#define WHO_AM_I_REG 0x0F
+#define I2C_ADDR 0x3E
+#define I2C_REG 0x00
 
-static void read_who_am_i(struct device *dev)
+static int i2c_read_byte(struct device *dev, unsigned addr, unsigned reg, uint8_t *data)
 {
-	uint8_t data;
+	struct i2c_msg msg[2];
+	uint8_t write_data = reg;
 
-	data = WHO_AM_I_REG;
-	if (i2c_write(dev, &data, sizeof(data), GYRO_I2C_ADDR) != 0) {
-		printk("Error on i2c_write()\n");
-		return;
-	}
+	msg[0].flags = I2C_MSG_WRITE | I2C_MSG_RESTART;
+	msg[0].buf = &write_data;
+	msg[0].len = 1;
 
-	data = 0;
-	if (i2c_read(dev, &data, sizeof(data), GYRO_I2C_ADDR) != 0) {
-		printk("Error on i2c_read()\n");
-		return;
+	msg[1].flags = I2C_MSG_READ | I2C_MSG_STOP;
+	msg[1].buf = data;
+	msg[1].len = 1;
+
+	if (i2c_transfer(dev, msg, 2, addr) != 0) {
+		printk("Error on i2c_transfer()\n");
+		return -1;
 	}
+	
+	return 0;
+}
+
+static int i2c_write_byte(struct device *dev, unsigned addr, unsigned reg, uint8_t data)
+{
+	struct i2c_msg msg;
+	uint8_t write_data[2] = {reg, data};
+
+	msg.flags = I2C_MSG_WRITE | I2C_MSG_STOP;
+	msg.buf = write_data;
+	msg.len = 2;
 
-	if (data == 0xd4)
-		printk("Register Who am I read successfully\n");
-	else
-		printk("Register Who am I read FAILED\n");
+	if (i2c_transfer(dev, &msg, 1, addr) != 0) {
+		printk("Error on i2c_transfer()\n");
+		return -1;
+	}
+	
+	return 0;
 }
 
 static void read_who_am_i_by_transfer(struct device *dev)
 {
-	struct i2c_msg msg[2];
-	uint8_t write_data = WHO_AM_I_REG;
 	uint8_t read_data = 0;
+	unsigned i = 0;
 
-	msg[0].flags = I2C_MSG_WRITE | I2C_MSG_RESTART;
-	msg[0].buf = &write_data;
-	msg[0].len = sizeof(write_data);
+	if (i2c_write_byte(dev, I2C_ADDR, 0x7d, 0x12) < 0)
+		return;
 
-	msg[1].flags = I2C_MSG_READ | I2C_MSG_STOP;
-	msg[1].buf = &read_data;
-	msg[1].len = sizeof(read_data);
+	if (i2c_read_byte(dev, I2C_ADDR, 0x7d, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
 
-	if (i2c_transfer(dev, msg, 2, GYRO_I2C_ADDR) != 0) {
-		printk("Error on i2c_transfer()\n");
+	if (i2c_write_byte(dev, I2C_ADDR, 0x7d, 0x34) < 0)
 		return;
-	}
 
-	if (read_data == 0xd4)
-		printk("Register Who am I read successfully\n");
-	else
-		printk("Register Who am I read FAILED\n");
+	if (i2c_read_byte(dev, I2C_ADDR, 0x7d, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x1f, 0x01) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x1f, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x0d, 0x00) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x0d, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x11, 0x55) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x11, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x10, 0xAA) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x10, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x0f, 0) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x0f, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	if (i2c_write_byte(dev, I2C_ADDR, 0x0e, 0) < 0)
+		return;
+
+	if (i2c_read_byte(dev, I2C_ADDR, 0x0e, &read_data) < 0)
+		return;
+	printk("read 0x%x\n", read_data);
+
+	while (1) {
+		i = (i + 1) % 16;
+		task_sleep(USEC(100000));
+
+		if (i2c_write_byte(dev, I2C_ADDR, 0x11, i < 8 ? 1 << i : 0) < 0)
+			return;
+
+		if (i2c_write_byte(dev, I2C_ADDR, 0x10, i >= 8 ? 1 << (i-8) : 0) < 0)
+			return;
+	}
 }
 
 void main(void)
@@ -94,6 +160,5 @@ void main(void)
 		return;
 	}
 
-	read_who_am_i(dev);
 	read_who_am_i_by_transfer(dev);
 }