i2c: Add multi write

3 files changed, 34 insertions, 60 deletions

diff --git a/gyroscope.c b/gyroscope.c
index 0f932c2..23fa197 100644
--- a/gyroscope.c
+++ b/gyroscope.c
@@ -41,23 +41,16 @@ void gyroscopeInit () {
 
 /* XXX: make nonblocking */
 void gyroscopeStart () {
-	/* XXX: implement multi-write */
-	/* disable power-down-mode */
-	if (!twWrite (L3GD20, L3GD20_CTRLREG1, 0b00001111)) {
-		printf ("cannot start write\n");
-	}
-	sleepwhile (twr.status == TWST_WAIT);
-	printf ("final twi status was %i\n", twr.status);
-
-	/* enable drdy interrupt */
-	if (!twWrite (L3GD20, L3GD20_CTRLREG3, 0b00001000)) {
-		printf ("cannot start write\n");
-	}
-	sleepwhile (twr.status == TWST_WAIT);
-	printf ("final twi status was %i\n", twr.status);
+	/* configuration:
+	 * disable power-down-mode
+	 * defaults
+	 * enable drdy interrupt
+	 * select 500dps
+	 */
+	uint8_t data[] = {0b00001111, 0b0, 0b00001000, 0b00010000};
 
-	/* select 500dps */
-	if (!twWrite (L3GD20, L3GD20_CTRLREG4, 0b00010000)) {
+	if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
+			sizeof (data)/sizeof (*data))) {
 		printf ("cannot start write\n");
 	}
 	sleepwhile (twr.status == TWST_WAIT);
@@ -68,7 +61,7 @@ bool gyroscopeRead () {
 	if (drdy) {
 		drdy = false;
 
-		if (!twReadMulti (L3GD20, 0x28, (uint8_t *) val, 6)) {
+		if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) {
 			printf ("cannot start read\n");
 		}
 
diff --git a/i2c.c b/i2c.c
index 4368158..2a342b1 100644
--- a/i2c.c
+++ b/i2c.c
@@ -52,39 +52,17 @@ void twInit () {
 
 /*	high-level write
  */
-bool twWrite (const uint8_t address, const uint8_t subaddress,
-		const uint8_t data) {
+bool twRequest (const twMode mode, const uint8_t address,
+		const uint8_t subaddress, uint8_t * const data, const uint8_t count) {
 	/* do not start if request is pending */
 	if (twr.status == TWST_WAIT) {
 		return false;
 	}
 
-	twr.mode = TWM_WRITE;
+	twr.mode = mode;
 	twr.address = address;
 	twr.subaddress = subaddress;
 	twr.data = data;
-	twr.step = 0;
-	twr.status = TWST_WAIT;
-	/* wait for stop finish */
-	while (TW_STATUS != 0xf8);
-	twStartRaw ();
-
-	return true;
-}
-
-/*	high-level read for multiple bytes/addresses
- */
-bool twReadMulti (const uint8_t address, const uint8_t subaddress,
-		uint8_t * const retData, const uint8_t count) {
-	/* do not start if request is pending */
-	if (twr.status == TWST_WAIT) {
-		return false;
-	}
-
-	twr.mode = TWM_READ_MULTI;
-	twr.address = address;
-	twr.subaddress = subaddress;
-	twr.retData = retData;
 	twr.count = count;
 	twr.i = 0;
 	twr.step = 0;
@@ -104,6 +82,7 @@ static void twIntWrite () {
 			if (TW_STATUS == TW_START) {
 				twWriteRaw (twr.address | TW_WRITE);
 				twFlushRaw ();
+				++twr.step;
 			} else {
 				twr.status = TWST_ERR;
 			}
@@ -111,8 +90,10 @@ static void twIntWrite () {
 
 		case 1:
 			if (TW_STATUS == TW_MT_SLA_ACK) {
-				twWriteRaw (twr.subaddress);
+				/* write subaddress, enable auto-increment */
+				twWriteRaw ((1 << 7) | twr.subaddress);
 				twFlushRaw ();
+				++twr.step;
 			} else {
 				twr.status = TWST_ERR;
 			}
@@ -120,8 +101,12 @@ static void twIntWrite () {
 
 		case 2:
 			if (TW_STATUS == TW_MT_DATA_ACK) {
-				twWriteRaw (twr.data);
+				twWriteRaw (twr.data[twr.i]);
+				++twr.i;
 				twFlushRaw ();
+				if (twr.i >= twr.count) {
+					++twr.step;
+				}
 			} else {
 				twr.status = TWST_ERR;
 			}
@@ -140,12 +125,11 @@ static void twIntWrite () {
 			printf ("nope\n");
 			break;
 	}
-	++twr.step;
 }
 
 /*	handle interrupt, read request
  */
-static void twIntReadMulti () {
+static void twIntRead () {
 	switch (twr.step) {
 		case 0:
 			if (TW_STATUS == TW_START) {
@@ -201,7 +185,7 @@ static void twIntReadMulti () {
 
 		case 5:
 			if (TW_STATUS == TW_MR_DATA_ACK) {
-				twr.retData[twr.i] = TWDR;
+				twr.data[twr.i] = TWDR;
 				++twr.i;
 				if (twr.i < twr.count-1) {
 					/* read another byte, not the last one */
@@ -220,7 +204,7 @@ static void twIntReadMulti () {
 		case 6:
 			if (TW_STATUS == TW_MR_DATA_NACK) {
 				/* receive final byte, send stop */
-				twr.retData[twr.i] = TWDR;
+				twr.data[twr.i] = TWDR;
 				twStopRaw ();
 				twr.status = TWST_OK;
 			} else {
@@ -229,7 +213,7 @@ static void twIntReadMulti () {
 			break;
 
 		default:
-			printf ("twIntReadMulti: nope\n");
+			printf ("twIntRead: nope\n");
 			break;
 	}
 }
@@ -240,8 +224,8 @@ ISR(TWI_vect) {
 			twIntWrite ();
 			break;
 
-		case TWM_READ_MULTI:
-			twIntReadMulti ();
+		case TWM_READ:
+			twIntRead ();
 			break;
 
 		default:
diff --git a/i2c.h b/i2c.h
index abcf5f3..eb68d8b 100644
--- a/i2c.h
+++ b/i2c.h
@@ -4,7 +4,7 @@
 typedef enum {
 	TWM_INVALID = 0,
 	TWM_WRITE,
-	TWM_READ_MULTI,
+	TWM_READ,
 } twMode;
 
 typedef enum {
@@ -19,11 +19,10 @@ typedef struct {
 	twMode mode;
 	uint8_t address;
 	uint8_t subaddress;
-	uint8_t data;
 	uint8_t step;
-	/* read data store */
-	uint8_t *retData;
-	/* number of bytes to be read */
+	/* pointer to read/write data */
+	uint8_t *data;
+	/* number of bytes to be read/written */
 	uint8_t count;
 	/* current byte */
 	uint8_t i;
@@ -40,9 +39,7 @@ extern volatile twReq twr;
 #include <stdbool.h>
 
 void twInit ();
-bool twWrite (const uint8_t address, const uint8_t subaddress,
-		const uint8_t data);
-bool twReadMulti (const uint8_t address, const uint8_t subaddress,
-		uint8_t * const retData, const uint8_t count);
+bool twRequest (const twMode mode, const uint8_t address,
+		const uint8_t subaddress, uint8_t * const data, const uint8_t count);
 
 #endif /* TW_H */