More hacking:

* initial Wii Motion+ support
* initial timer support

Makefile

@@ -3,7 +3,7 @@
 NAME=quad
 
 SSRCS=crt0.s
-CSRCS=main.c i2c.c
+CSRCS=main.c i2c.c wmp.c timer.c
 
 CFLAGS=-march=armv4t -msoft-float -O1
 

i2c.c

@@ -29,8 +29,13 @@ void init_i2c(void)
 	IWREG(I2SCLL) = (25 * 100);
 	IWREG(I2SCLH) = (12 * 100);
 #else
+# if 0
 	IWREG(I2SCLL) = 1475; /* ~5kHz */
 	IWREG(I2SCLH) = 1475;
+# else
+	IWREG(I2SCLL) = 73; /* ~100kHz */
+	IWREG(I2SCLH) = 73;
+# endif
 #endif
 }
 
@@ -45,11 +50,6 @@ int i2c_wait(void)
 	return stat;
 }
 
-void i2c_go(void)
-{
-	IREG(I2CONCLR) = SIFLAG;
-}
-
 int i2c_conreg(void)
 {
 	return IREG(I2CONSET);
@@ -64,14 +64,12 @@ bool i2c_send_start(void)
 {
 	IREG(I2CONCLR) = STOFLAG | STAFLAG;
 	IREG(I2CONSET) = STAFLAG;
-/*	i2c_go(); */
 	switch (i2c_wait()) {
 	case 0x08:
 	case 0x10:
-/*		IREG(I2CONCLR) = STAFLAG; */
 		return TRUE;
 	default:
-/*		IREG(I2CONCLR) = STAFLAG; */
+		i2c_send_stop();
 		return FALSE;
 	}
 }
@@ -84,12 +82,30 @@ bool i2c_send_address(int addr, bool write)
 bool i2c_send_data(int data)
 {
 	IREG(I2DAT) = data;
-	IREG(I2CONCLR) = STAFLAG | SIFLAG;
+	IREG(I2CONCLR) = STAFLAG | STOFLAG | SIFLAG;
 	switch (i2c_wait()) {
 	case 0x18:
 	case 0x28:
+	case 0x40:
 		return TRUE;
 	default:
+		i2c_send_stop();
+		return FALSE;
+	}
+}
+
+bool i2c_receive_data(int *data, bool last)
+{
+	if (!last)
+		IREG(I2CONSET) = AAFLAG;
+	IREG(I2CONCLR) = STAFLAG | STOFLAG | SIFLAG | (last?AAFLAG:0);
+	switch (i2c_wait()) {
+	case 0x50:
+	case 0x58:
+		*data = IREG(I2DAT);
+		return TRUE;
+	default:
+		i2c_send_stop();
 		return FALSE;
 	}
 }
@@ -98,7 +114,7 @@ void i2c_send_stop(void)
 {
 	IREG(I2CONCLR) = STAFLAG | AAFLAG;
 	IREG(I2CONSET) = STOFLAG;
-	i2c_go();
+	IREG(I2CONCLR) = SIFLAG;
 	/* Don't think we need to wait here. Could be wrong. */
 }
 

i2c.h

@@ -1,3 +1,6 @@
+#ifndef __I2C_H
+#define __I2C_H
+
 #include "types.h"
 void init_i2c(void);
 extern int i2cstat;
@@ -8,4 +11,7 @@ int i2c_statreg(void);
 bool i2c_send_start(void);
 bool i2c_send_address(int addr, bool write);
 bool i2c_send_data(int data);
+bool i2c_receive_data(int *data, bool last);
 void i2c_send_stop(void);
+
+#endif /* __I2C_H */

main.c

@@ -1,5 +1,7 @@
 
+#include "wmp.h"
 #include "i2c.h"
+#include "timer.h"
 
 #define UARTBASE 0xE000C000
 
@@ -23,20 +25,6 @@
 #define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */
 #define U0DR ((UREG(LSR) & (1<<0))) /* UART0 data ready */
 
-
-#define I2CBASE  0xE001C000
-
-#define I2CONSET 0x00
-#define I2STAT   0x04
-#define I2DAT    0x08
-#define I2ADR    0x0c
-#define I2SCLH   0x10
-#define I2SCLL   0x14
-#define I2CONCLR 0x18
-
-#define IREG(x) (((volatile unsigned char *)I2CBASE)[x])
-
-
 #define PINSEL0 (*((volatile unsigned char *) 0xE002C000))
 
 void init_uart(void)
@@ -109,10 +97,142 @@ void reply(char *str)
 
 unsigned int count = 0;
 
+void minmax_sample(void)
+{
+	int count;
+	int fast_roll_min, fast_roll_max;
+	int fast_pitch_min, fast_pitch_max;
+	int fast_yaw_min, fast_yaw_max;
+	int slow_roll_min, slow_roll_max;
+	int slow_pitch_min, slow_pitch_max;
+	int slow_yaw_min, slow_yaw_max;
+
+	putstr("Sampling min/max values\r\n");
+	if (!wmp_sample()) {
+		putstr("\r\nRead error\r\n");
+		return;
+	}
+
+	fast_roll_min = fast_roll_max = wmp_roll;
+	fast_pitch_min = fast_pitch_max = wmp_pitch;
+	fast_yaw_min = fast_yaw_max = wmp_yaw;
+
+	slow_roll_min = slow_roll_max = wmp_roll;
+	slow_pitch_min = slow_pitch_max = wmp_pitch;
+	slow_yaw_min = slow_yaw_max = wmp_yaw;
+
+	count = 0;
+
+	while (1) {
+		if (!wmp_sample()) {
+			putstr("\r\nRead error\r\n");
+			return;
+		}
+		if (wmp_roll_fast) {
+			if (wmp_roll < fast_roll_min)
+				fast_roll_min = wmp_roll;
+			if (wmp_roll > fast_roll_max)
+				fast_roll_max = wmp_roll;
+		} else {
+			if (wmp_roll < slow_roll_min)
+				slow_roll_min = wmp_roll;
+			if (wmp_roll > slow_roll_max)
+				slow_roll_max = wmp_roll;
+		}
+		if (wmp_pitch_fast) {
+			if (wmp_pitch < fast_pitch_min)
+				fast_pitch_min = wmp_pitch;
+			if (wmp_pitch > fast_pitch_max)
+				fast_pitch_max = wmp_pitch;
+		} else {
+			if (wmp_pitch < slow_pitch_min)
+				slow_pitch_min = wmp_pitch;
+			if (wmp_pitch > slow_pitch_max)
+				slow_pitch_max = wmp_pitch;
+		}
+		if (wmp_yaw_fast) {
+			if (wmp_yaw < fast_yaw_min)
+				fast_yaw_min = wmp_yaw;
+			if (wmp_yaw > fast_yaw_max)
+				fast_yaw_max = wmp_yaw;
+		} else {
+			if (wmp_yaw < slow_yaw_min)
+				slow_yaw_min = wmp_yaw;
+			if (wmp_yaw > slow_yaw_max)
+				slow_yaw_max = wmp_yaw;
+		}
+		count++;
+		if (count > 1000) {
+			putstr("(");
+			puthex(slow_roll_min);
+			putstr(", ");
+			puthex(slow_pitch_min);
+			putstr(", ");
+			puthex(slow_yaw_min);
+			putstr(") (");
+			puthex(slow_roll_max);
+			putstr(", ");
+			puthex(slow_pitch_max);
+			putstr(", ");
+			puthex(slow_yaw_max);
+			putstr(") (");
+			puthex(fast_roll_min);
+			putstr(", ");
+			puthex(fast_pitch_min);
+			putstr(", ");
+			puthex(fast_yaw_min);
+			putstr(") (");
+			puthex(fast_roll_max);
+			putstr(", ");
+			puthex(fast_pitch_max);
+			putstr(", ");
+			puthex(fast_yaw_max);
+			putstr(")                   \r");
+			count = 0;
+		}
+		timer_delay_ms(2);
+	}
+}
+
+void average_sample(void)
+{
+	int i;
+	int roll_total;
+	int pitch_total;
+	int yaw_total;
+
+	putstr("Sampling average values\r\n");
+
+	roll_total = 0;
+	pitch_total = 0;
+	yaw_total = 0;
+
+	for (i = 0; i < 0x1000; i++) {
+		if (!wmp_sample()) {
+			putstr("\r\nRead error\r\n");
+			return;
+		}
+		roll_total += wmp_roll;
+		pitch_total += wmp_pitch;
+		yaw_total += wmp_yaw;
+		timer_delay_ms(2);
+	}
+	putstr("(");
+	puthex(roll_total);
+	putstr(", ");
+	puthex(pitch_total);
+	putstr(", ");
+	puthex(yaw_total);
+	putstr(")\r\n");
+}
+
 int main(void) {
+	int i;
+	int data;
 	init_uart();
 	init_i2c();
 	init_pins();
+	init_timer();
 	putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
 
 	while (1) {
@@ -140,17 +260,15 @@ int main(void) {
 		case '?':
 			reply("Help is not available. Try a psychiatrist.");
 			break;
-		case 'R':
-			putstr("I2C register is: ");
-			puthex(i2c_conreg());
-			reply(".");
-			break;
 		case 'T':
 			putstr("I2C status was: ");
 			puthex(i2cstat);
 			putstr(" I2C status is: ");
 			puthex(i2c_statreg());
 			reply(".");
+			putstr("I2C register is: ");
+			puthex(i2c_conreg());
+			reply(".");
 			break;
 		case 'S':
 			putstr("Sending START... ");
@@ -159,30 +277,60 @@ int main(void) {
 			else
 				reply("FAIL");
 			break;
-		case 'D':
-			putstr("Sending address... ");
-			if (i2c_send_address(0x53, TRUE))
-				reply("OK");
-			else
-				reply("FAIL");
-			break;
-		case 'B':
-			putstr("Sending bytes... ");
-			if (i2c_send_data(0xfe))
-				reply("OK");
-			else
-				reply("FAIL");
-
-			if (i2c_send_data(0x04))
-				reply("OK");
-			else
-				reply("FAIL");
-			break;
 		case 'O':
 			putstr("Sending STOP... ");
 			i2c_send_stop();
 			reply("sent");
 			break;
+		case 'I':
+			putstr("Initialising WMP... ");
+			if (wmp_init())
+				reply("done");
+			else
+				reply("FAIL");
+			break;
+		case 'M':
+			putstr("Reading from WMP... ");
+			if (wmp_sample()) {
+				putstr("(");
+				puthex(wmp_roll);
+				putstr(", ");
+				puthex(wmp_pitch);
+				putstr(", ");
+				puthex(wmp_yaw);
+				reply(").");
+			} else
+				reply("FAIL");
+			break;
+		case 'L':
+			minmax_sample();
+			break;
+		case 'V':
+			average_sample();
+			break;
+		case 'D':
+			putstr("Reading calibration data... ");
+			if (wmp_read_calibration_data()) {
+				putstr("\r\n");
+				for (i = 0; i < 0x10 ; i++) {
+					puthex(wmp_calibration_data[i]);
+					putstr(" ");
+				}
+				putstr("\r\n");
+				for (i = 0x10; i < 0x20 ; i++) {
+					puthex(wmp_calibration_data[i]);
+					putstr(" ");
+				}
+				putstr("\r\n");
+			} else {
+				reply("FAIL");
+			}
+			break;
+		case 'N':
+			putstr("The time is ");
+			puthex(timer_read());
+			reply(".");
+			break;
 		default:
 			reply("Unrecognised command.");
 			break;

timer.c

@@ -0,0 +1,53 @@
+#include "timer.h"
+
+#define TIMER0BASE  0xE0004000
+#define TIMER1BASE  0xE0008000
+
+#define IR    0x00
+#define TCR   0x04
+#define TC    0x08
+#define PR    0x0c
+#define PC    0x10
+#define MCR   0x14
+#define MR0   0x18
+#define MR1   0x1C
+#define MR2   0x20
+#define MR3   0x24
+#define CCR   0x28
+#define CR0   0x2C
+#define CR1   0x30
+#define CR2   0x34
+#define CR3   0x38
+#define EMR   0x3C
+#define CTCR  0x70
+#define PWM   0x74
+
+#define TREG(x) (((volatile unsigned char *)TIMER0BASE)[x])
+#define TWREG(x) (((volatile unsigned int *)TIMER0BASE)[(x)/sizeof(unsigned int)])
+
+#define TCR_ENABLE (1<<0)
+#define TCR_RESET  (1<<1)
+
+void init_timer(void)
+{
+	TREG(TCR) = TCR_ENABLE | TCR_RESET;
+
+	TREG(CTCR) = 0; /* Use PCLK */
+	TREG(TC) = 0;
+	TREG(PR) = TIMER_PRESCALE ;
+	TREG(PC) = 0;
+
+	TREG(TCR) = TCR_ENABLE;
+}
+
+unsigned int timer_read(void)
+{
+	return TWREG(TC);
+}
+
+void timer_delay_clocks(unsigned int clocks)
+{
+	signed int time = TWREG(TC) + clocks;
+	while (((signed int) (time-TWREG(TC))) > 0);
+}
+

timer.h

@@ -0,0 +1,18 @@
+#ifndef __TIMER_H
+#define __TIMER_H
+
+#define TIMER_PCLK 14745600
+#define TIMER_PRESCALE 0
+
+#define TIMER_SECOND (TIMER_PCLK/(TIMER_PRESCALE+1))
+#define TIMER_MS (TIMER_SECOND/1000)
+#define TIMER_US (TIMER_SECOND/1000000)
+
+void init_timer(void);
+unsigned int timer_read(void);
+void timer_delay_clocks(unsigned int clocks);
+
+#define timer_delay_us(x) timer_delay_clocks((x)*TIMER_US)
+#define timer_delay_ms(x) timer_delay_clocks((x)*TIMER_MS)
+
+#endif /* __TIMER_H */

types.h

@@ -1,3 +1,8 @@
+#ifndef __TYPES_H
+#define __TYPES_H
+
 typedef int bool;
 #define TRUE 1
 #define FALSE 0
+
+#endif /* __TYPES_H */

wmp.c

@@ -0,0 +1,100 @@
+
+#include "wmp.h"
+#include "i2c.h"
+
+bool wmp_init(void)
+{
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0xfe))
+		return FALSE;
+	if (!i2c_send_data(0x04))
+		return FALSE;
+	i2c_send_stop();
+}
+
+unsigned char wmp_calibration_data[0x20];
+
+bool wmp_read_calibration_data(void)
+{
+	int i;
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0x20))
+		return FALSE;
+	i2c_send_stop();
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, FALSE))
+		return FALSE;
+	for (i = 0; i < 0x20; i++) {
+		unsigned int data;
+		if (!i2c_receive_data(&data, (i == 0x1f)))
+			return FALSE;
+		wmp_calibration_data[i] = data;
+	}
+	i2c_send_stop();
+	return TRUE;
+}
+
+unsigned int wmp_yaw;
+unsigned int wmp_pitch;
+unsigned int wmp_roll;
+
+bool wmp_yaw_fast;
+bool wmp_pitch_fast;
+bool wmp_roll_fast;
+
+/* There's considerable debate about these values, and they may vary
+ * between different models of the Wii Motion Plus. It would be nice
+ * to be able to use the calibration data stored on the device itself
+ * but we don't know the format yet.
+ */
+#define SLOW_YAW_STEP	(1000/20)
+#define SLOW_PITCH_STEP (1000/20)
+#define SLOW_ROLL_STEP  (1000/20)
+#define FAST_YAW_STEP	(1000/4)
+#define FAST_PITCH_STEP (1000/4)
+#define FAST_ROLL_STEP  (1000/4)
+
+bool wmp_sample(void)
+{
+	int i;
+	unsigned int b[6];
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x52, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0x00))
+		return FALSE;
+	i2c_send_stop();
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x52, FALSE))
+		return FALSE;
+	for (i = 0; i < 6; i++) {
+		if (!i2c_receive_data(&(b[i]), (i == 5)))
+			return FALSE;
+	}
+	i2c_send_stop();
+
+	wmp_yaw   = ((b[3]>>2)<<8) + b[0];
+	wmp_pitch = ((b[4]>>2)<<8) + b[1];
+	wmp_roll  = ((b[5]>>2)<<8) + b[2];
+
+	/* XXX We don't take into account the fast/slow mode flag here */
+	wmp_yaw_fast = !(b[3] & 0x2);
+	wmp_pitch_fast = !(b[3] & 0x1);
+	wmp_roll_fast = !(b[4] & 0x2);
+
+	return TRUE;
+}
+

wmp.h

@@ -0,0 +1,20 @@
+#ifndef __WMP_H
+#define __WMP_H
+
+#include "types.h"
+
+extern unsigned int wmp_yaw;
+extern unsigned int wmp_pitch;
+extern unsigned int wmp_roll;
+
+extern unsigned char wmp_calibration_data[];
+
+extern bool wmp_yaw_fast;
+extern bool wmp_pitch_fast;
+extern bool wmp_roll_fast;
+
+bool wmp_init(void);
+bool wmp_sample(void);
+bool wmp_read_calibration_data(void);
+
+#endif /* __WMP_H */