Huge shift: Put HTC vive into its own file, to free up the rest of the system for libsurvive.

15 files changed, 1275 insertions, 1021 deletions

diff --git a/Makefile b/Makefile
index fb74e75..cf237f2 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 all : lib data_recorder test calibrate
 
-CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE
+CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
 
 
@@ -20,7 +20,7 @@ calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFuncti
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o $(DEBUGSTUFF) $(CALS)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o $(DEBUGSTUFF) $(CALS)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/calibrate.c b/calibrate.c
index 04ea9a8..f50bad3 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -18,11 +18,11 @@ void HandleKey( int keycode, int bDown )
 
 	if( keycode == 'O' || keycode == 'o' )
 	{
-		survive_usb_send_magic(ctx,1);
+		survive_send_magic(ctx,1,0,0);
 	}
 	if( keycode == 'F' || keycode == 'f' )
 	{
-		survive_usb_send_magic(ctx,0);
+		survive_send_magic(ctx,0,0,0);
 	}
 }
 
@@ -40,6 +40,7 @@ int buffertimeto[32*3];
 
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+//	if( timeinsweep < 0 ) return;
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
 
 	if( acode == -1 ) return;
@@ -95,6 +96,10 @@ void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint
 void * GuiThread( void * v )
 {
 	short screenx, screeny;
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+
 	while(1)
 	{
 		CNFGHandleInput();
@@ -148,9 +153,6 @@ int main()
 
 	survive_cal_install( ctx );
 
-	CNFGBGColor = 0x000000;
-	CNFGDialogColor = 0x444444;
-	CNFGSetup( "Survive GUI Debug", 640, 480 );
 	OGCreateThread( GuiThread, 0 );
 	
 
@@ -164,5 +166,6 @@ int main()
 	{
 		//Do stuff.
 	}
+	printf( "Returned\n" );
 }
 
diff --git a/data_recorder.c b/data_recorder.c
index ced82c4..516ac6c 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -17,11 +17,11 @@ void HandleKey( int keycode, int bDown )
 
 	if( keycode == 'O' || keycode == 'o' )
 	{
-		survive_usb_send_magic(ctx,1);
+		survive_send_magic(ctx,1,0,0);
 	}
 	if( keycode == 'F' || keycode == 'f' )
 	{
-		survive_usb_send_magic(ctx,0);
+		survive_send_magic(ctx,0,0,0);
 	}
 }
 
diff --git a/include/survive.h b/include/survive.h
index 1b9c29c..44f9926 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -24,7 +24,8 @@ struct SurviveObject
 {
 	struct SurviveContext * ctx;
 
-	char    codename[4];  //3 letters, null-terminated.  Currently HMD, WM0, WM1.
+	char    codename[4];    //3 letters, null-terminated.  Currently HMD, WM0, WM1.
+	char    drivername[4];  //3 letters for driver.  Currently "HTC"
 	int16_t buttonmask;
 	int16_t axis1;
 
@@ -85,8 +86,7 @@ struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, cons
 //Utilitiy functions.
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
 
-//TODO: Need to make this do haptic responses for hands. 
-int survive_usb_send_magic( struct SurviveContext * ctx, int on );
+int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen );
 
 //Install the calibrator.
 void survive_cal_install( struct SurviveContext * ctx );
diff --git a/src/survive.c b/src/survive.c
index aab2ae6..b472ccc 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -3,22 +3,12 @@
 
 #include <survive.h>
 #include "survive_internal.h"
+#include "survive_driverman.h"
 #include <stdio.h>
 #include <stdlib.h>
-#include <jsmn.h>
 #include <string.h>
 #include <zlib.h>
 
-
-static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
- if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
-    strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
-		return 0;
-	}
-	return -1;
-}
-
-
 static void survivefault( struct SurviveContext * ctx, const char * fault )
 {
 	fprintf( stderr, "Error: %s\n", fault );
@@ -30,113 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
-{
-	int k;
-	int pts = t[i+1].size;
-	jsmntok_t * tk;
-
-	so->nr_locations = 0;
-	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
-
-	for( k = 0; k < pts; k++ )
-	{
-		tk = &t[i+2+k*4];
-
-		FLT vals[3];
-		int m;
-		for( m = 0; m < 3; m++ )
-		{
-			char ctt[128];
-
-			tk++;
-			int elemlen = tk->end - tk->start;
-
-			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
-			{
-				SV_ERROR( "Parse error in JSON\n" );
-				return 1;
-			}
-
-			memcpy( ctt, ct0conf + tk->start, elemlen );
-			ctt[elemlen] = 0;
-			FLT f = atof( ctt );
-			int id = so->nr_locations*3+m;
-			(*floats_out)[id] = f;
-		}
-		so->nr_locations++;
-	}
-	return 0;
-}
-
-static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, int devno, int iface, int extra_magic )
-{
-	char * ct0conf = 0;
-	int len = survive_get_config( &ct0conf, ctx, devno, iface, extra_magic );
-
-#if 0
-	char fname[100];
-	sprintf( fname, "%s_config.json", so->codename );
-	FILE * f = fopen( fname, "w" );
-	fwrite( ct0conf, strlen(ct0conf), 1, f );
-	fclose( f );
-#endif
-
-	if( len > 0 )
-	{
-
-		//From JSMN example.
-		jsmn_parser p;
-		jsmntok_t t[4096];
-		jsmn_init(&p);
-		int i;
-		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
-		if (r < 0) {
-			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
-			return -1;
-		}
-		if (r < 1 || t[0].type != JSMN_OBJECT) {
-			SV_INFO("Object expected in HMD configuration\n");
-			return -2;
-		}
-
-		for (i = 1; i < r; i++) {
-			jsmntok_t * tk = &t[i];
-
-			char ctxo[100];
-			int ilen = tk->end - tk->start;
-			if( ilen > 99 ) ilen = 99;
-			memcpy(ctxo, ct0conf + tk->start, ilen);
-			ctxo[ilen] = 0;
-
-//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
-//				printf( "%.*s\n", ilen, ct0conf + tk->start );
-
-			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i  ) )
-				{
-					break;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
-				{
-					break;
-				}
-			}
-		}
-	}
-	else
-	{
-		//TODO: Cleanup any remaining USB stuff.
-		return 1;
-	}
-	return 0;
-}
 
 struct SurviveContext * survive_init()
 {
 	int r = 0;
+	int i = 0;
 	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
 
 	ctx->faultfunction = survivefault;
@@ -146,66 +34,16 @@ struct SurviveContext * survive_init()
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 
-	ctx->headset.ctx = ctx;
-	memcpy( ctx->headset.codename, "HMD", 4 );
-	ctx->watchman[0].ctx = ctx;
-	memcpy( ctx->watchman[0].codename, "WM0", 4 );
-	ctx->watchman[1].ctx = ctx;
-	memcpy( ctx->watchman[1].codename, "WM1", 4 );
-
-	//USB must happen last.
-	if( r = survive_usb_init( ctx ) )
-	{
-		//TODO: Cleanup any libUSB stuff sitting around.
-		goto fail_gracefully;
-	}
-
-	//Next, pull out the config stuff.
-	if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully;
-	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
-	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
-
-	ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000;
-	ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000;
-	ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200;
-	ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000;
-	ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800;
-
-	ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000;
-	ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000;
-
-	ctx->headset.timecenter_ticks     = ctx->headset.timebase_hz / 240;
-	ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240;
-	ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240;
-/*
-	int i;
-	int locs = ctx->headset.nr_locations;
-	printf( "Locs: %d\n", locs );
-	if (ctx->headset.sensor_locations )
+	const char * DriverName;
+	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
 	{
-		printf( "POSITIONS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", ctx->headset.sensor_locations[i+0], ctx->headset.sensor_locations[i+1], ctx->headset.sensor_locations[i+2] );
-		}
+		DeviceDriver dd = GetDriver( DriverName );
+		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
+		r = dd( ctx );
+		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
-	if( ctx->headset.sensor_normals )
-	{
-		printf( "NORMALS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", ctx->headset.sensor_normals[i+0], ctx->headset.sensor_normals[i+1], ctx->headset.sensor_normals[i+2] );
-		}
-	}
-*/
-
-	
 
 	return ctx;
-fail_gracefully:
-	survive_usb_close( ctx );
-	free( ctx );
-	return 0;
 }
 
 void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
@@ -249,18 +87,76 @@ void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func
 		ctx->angleproc = survive_default_angle_process;
 }
 
+int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj )
+{
+	int oldct = ctx->objs_ct;
+	ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) );
+	ctx->objs[oldct] = obj;
+	ctx->objs_ct = oldct+1;
+}
+
+void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
+{
+	int oldct = ctx->driver_ct;
+	ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) );
+	ctx->driverpolls = realloc( ctx->driverpolls, sizeof( DeviceDriverCb * ) * (oldct+1) );
+	ctx->drivercloses = realloc( ctx->drivercloses, sizeof( DeviceDriverCb * ) * (oldct+1) );
+	ctx->drivermagics = realloc( ctx->drivermagics, sizeof( DeviceDriverMagicCb * ) * (oldct+1) );
+	ctx->drivers[oldct] = payload;
+	ctx->driverpolls[oldct] = poll;
+	ctx->drivercloses[oldct] = close;
+	ctx->drivermagics[oldct] = magic;
+	ctx->driver_ct = oldct+1;
+}
 
+int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen )
+{
+	int oldct = ctx->driver_ct;
+	int i;
+	for( i = 0; i < oldct; i++ )
+	{
+		ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen );
+	}	
+}
 
 void survive_close( struct SurviveContext * ctx )
 {
-	survive_usb_close( ctx );
+	const char * DriverName;
+	int r = 0;
+	while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) )
+	{
+		DeviceDriver dd = GetDriver( DriverName );
+		SV_INFO( "De-registering driver %s (%p)", DriverName, dd );
+		r = dd( ctx );
+		SV_INFO( "Driver %s reports status %d", DriverName, r );
+	}
+
+	int oldct = ctx->driver_ct;
+	int i;
+	for( i = 0; i < oldct; i++ )
+	{
+		ctx->driverpolls[i]( ctx, ctx->drivers[i] );
+	}
+
+	//TODO: Free everything except for self.
+	//XXX Will leak memory.
 }
 
 int survive_poll( struct SurviveContext * ctx )
 {
-	survive_usb_poll( ctx );
+	int oldct = ctx->driver_ct;
+	int i, r;
+
+	for( i = 0; i < oldct; i++ )
+	{
+		r = ctx->driverpolls[i]( ctx, ctx->drivers[i] );
+		if( r ) return r;
+	}
+
+	return 0;
 }
 
+
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen )
 {
 	z_stream zs; //Zlib stream.  May only be used by configuration at beginning and by USB thread periodically.
@@ -285,9 +181,12 @@ int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int
 
 struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name )
 {
-	if( strcmp( name, "HMD" ) == 0 ) return &ctx->headset;
-	if( strcmp( name, "WM0" ) == 0 ) return &ctx->watchman[0];
-	if( strcmp( name, "WM1" ) == 0 ) return &ctx->watchman[1];
+	int i;
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		if( strcmp( ctx->objs[i]->codename, name ) == 0 )
+			return ctx->objs[i];
+	}
 	return 0;
 }
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 760692c..2ed1986 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -96,6 +96,14 @@ void survive_cal_install( struct SurviveContext * ctx )
 	cd->stage = 1;
 	cd->ctx = ctx;
 
+	cd->hmd = survive_get_so_by_name( ctx, "HMD" );
+	if( !cd->hmd )
+	{
+		SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+		free( cd );
+		return;
+	}
+
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
 	ctx->calptr = cd;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 0a772f1..bb4eb32 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -57,6 +57,8 @@ struct SurviveCalData
 
 	int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
 
+	struct SurviveObject * hmd;
+
 	//Stage:
 	// 0: Idle
 	// 1: Collecting OOTX data.
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
index 19f732f..e1e5fc9 100644
--- a/src/survive_cal_lhfind.c
+++ b/src/survive_cal_lhfind.c
@@ -156,8 +156,9 @@ static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * Lightho
 	FLT LastUsToTarget[3];
 	FLT mux = .9;
 	quatsetnone( LighthouseQuat );
-	FLT * hmd_points  = cd->ctx->headset.sensor_locations;
-	FLT * hmd_normals = cd->ctx->headset.sensor_normals;
+	struct SurviveObject * hmd = cd->hmd;
+	FLT * hmd_points  = hmd->sensor_locations;
+	FLT * hmd_normals = hmd->sensor_normals;
 
 	int first = 1, second = 0;
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 402282d..e55b640 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -1,35 +1,12 @@
 //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
 //
-//Based off of https://github.com/collabora/OSVR-Vive-Libre
-// Originally Copyright 2016 Philipp Zabel
-// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk>
-// Originally Copyright (C) 2013 Fredrik Hultin
-// Originally Copyright (C) 2013 Jakob Bornecrantz
-//
-//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license.
-//If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
 
-#define POP1  (*(readdata++))
-#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
-#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
-
-
-struct LightcapElement
-{
-	uint8_t sensor_id;
-	uint8_t type;
-	uint16_t length;
-	uint32_t timestamp;
-} __attribute__((packed));
-
-
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
+void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
 	struct SurviveContext * ctx = so->ctx;
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
@@ -200,336 +177,3 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 }
 
 
-
-static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
-{
-	int i;
-
-	uint8_t startread[29];
-	memcpy( startread, readdata, 29 );
-
-#if 0
-	printf( "DAT:     " );
-		for( i = 0; i < 29; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
-	uint8_t time1 = POP1;
-	uint8_t qty = POP1;
-	uint8_t time2 = POP1;
-	uint8_t type = POP1;
-	qty-=2;
-	int propset = 0;
-	int doimu = 0;
-
-
-	if( (type & 0xf0) == 0xf0 )
-	{
-		propset |= 4;
-		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
-		type &= ~0x10;
-
-		if( type & 0x01 )
-		{
-			qty-=1;
-			w->buttonmask = POP1;
-			type &= ~0x01;
-		}
-		if( type & 0x04 ) 
-		{
-			qty-=1;
-			w->axis1 = ( POP1 ) * 128; 
-			type &= ~0x04;
-		}
-		if( type & 0x02 )
-		{
-			qty-=4;
-			w->axis2 = POP2;
-			w->axis3 = POP2;
-			type &= ~0x02;
-		}
-
-		//XXX TODO: Is this correct?  It looks SO WACKY
-		type &= 0x7f;
-		if( type == 0x68 ) doimu = 1;
-		type &= 0x0f;
-		if( type == 0x00 && qty ) { type = POP1; qty--; }
-	}
-
-	if( type == 0xe1 )
-	{
-		propset |= 1;
-		w->charging = readdata[0]>>7;
-		w->charge = POP1&0x7f; qty--;
-		w->ison = 1; 
-		if( qty )
-		{
-			qty--;
-			type = POP1; //IMU usually follows.
-		}
-	}
-
-	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
-	{
-		propset |= 2;
-		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
-		int16_t * k = (int16_t *)readdata+1;
-		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
-		readdata += 13; qty -= 13;
-		type &= ~0xe8;
-		if( qty )
-		{
-			qty--;
-			type = POP1;
-		}
-	}
-
-
-	if( qty )
-	{
-		int j;
-		qty++;
-		readdata--;
-		*readdata = type; //Put 'type' back on stack.
-		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
-
-//#define DEBUG_WATCHMAN
-#ifdef DEBUG_WATCHMAN
-		printf( "_%s ", w->codename);
-		for( i = 0; i < qty; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
-
-		uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0);
-
-		uint32_t times[20];
-		const int nrtime = sizeof(times)/sizeof(uint32_t);
-		int timecount = 0;
-		int leds;
-		int parameters;
-		int fault = 0;
-
-		///Handle uint32_tifying (making sure we keep it incrementing)
-		uint32_t llt = w->last_lighttime;
-		uint32_t imumsb = time1<<24;
-		mytime |= imumsb;
-
-		//Compare mytime to llt
-
-		int diff = mytime - llt;
-		if( diff < -0x1000000 )
-			mytime += 0x1000000;
-		else if( diff > 0x100000 )
-			mytime -= 0x1000000;
-
-		w->last_lighttime = mytime;
-
-		times[timecount++] = mytime;
-#ifdef DEBUG_WATCHMAN
-		printf( "_%s Packet Start Time: %d\n", w->codename, mytime );
-#endif	
-
-		//First, pull off the times, starting with the current time, then all the delta times going backwards.
-		{
-			while( mptr - readdata > (timecount>>1) )
-			{
-				uint32_t arcane_value = 0;
-				//ArcanePop (Pop off values from the back, forward, checking if the MSB is set)
-				do
-				{
-					uint8_t ap = *(mptr--);
-					arcane_value |= (ap&0x7f);
-					if( ap & 0x80 )  break;
-					arcane_value <<= 7;
-				} while(1);
-				times[timecount++] = (mytime -= arcane_value);
-#ifdef DEBUG_WATCHMAN
-				printf( "_%s Time: %d  newtime: %d\n", w->codename, arcane_value, mytime );
-#endif
-			}
-
-			leds = timecount>>1;
-			//Check that the # of sensors at the beginning match the # of parameters we would expect.
-			if( timecount & 1 ) { fault = 1; goto end; }				//Inordinal LED count
-			if( leds != mptr - readdata + 1 ) { fault = 2; goto end; }	//LED Count does not line up with parameters
-		}
-
-
-		struct LightcapElement les[10];
-		int lese = 0; //les's end
-
-		//Second, go through all LEDs and extract the lightevent from them. 
-		{
-			uint8_t marked[nrtime];
-			memset( marked, 0, sizeof( marked ) );
-			int i, parpl = 0;
-			timecount--;
-			int timepl = 0;
-
-			//This works, but usually returns the values in reverse end-time order.
-			for( i = 0; i < leds; i++ )
-			{
-				int led = readdata[i];
-				int adv = led & 0x07;
-				led >>= 3;
-
-				while( marked[timepl] ) timepl++;
-				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
-				uint32_t endtime = times[timepl++];
-				int end = timepl + adv;
-				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
-				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
-				uint32_t starttime = times[end];
-				marked[end] = 1;
-
-				//Insert all lighting things into a sorted list.  This list will be
-				//reverse sorted, but that is to minimize operations.  To read it
-				//in sorted order simply read it back backwards.
-				//Use insertion sort, since we should most of the time, be in order.
-				struct LightcapElement * le = &les[lese++];
-				le->sensor_id = led;
-				le->type = 0xfe;
-
-				if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb.
-				le->length = endtime - starttime;
-				le->timestamp = starttime;
-
-#ifdef DEBUG_WATCHMAN
-				printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime );
-#endif
-				int swap = lese-2;
-				while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
-				{
-					struct LightcapElement l;
-					memcpy( &l, &les[swap], sizeof( l ) );
-					memcpy( &les[swap], &les[swap+1], sizeof( l ) );
-					memcpy( &les[swap+1], &l, sizeof( l ) );
-					swap--;
-				}
-			}
-		}
-
-		int i;
-		for( i = lese-1; i >= 0; i-- )
-		{
-			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
-			handle_lightcap( w, &les[i] );
-		}
-
-		return;
-	end:
-		{
-			struct SurviveContext * ctx = w->ctx;
-			SV_INFO( "Light decoding fault: %d\n", fault );
-		}
-	}
-}
-
-
-void survive_data_cb( struct SurviveUSBInterface * si )
-{
-	int size = si->actual_len;
-	struct SurviveContext * ctx = si->ctx;
-#if 0
-	int i;
-	printf( "%16s: %d: ", si->hname, len );
-	for( i = 0; i < size; i++ )
-	{
-		printf( "%02x ", si->buffer[i] );
-	}
-	printf( "\n" );
-	return;
-#endif 
-
-	int iface = si->which_interface_am_i;
-	uint8_t * readdata = si->buffer;
-
-	int id = POP1;
-//	printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface );
-
-
-	switch( si->which_interface_am_i )
-	{
-	case USB_IF_HMD:
-	{
-		struct SurviveObject * headset = &ctx->headset;
-		readdata+=2;
-		headset->buttonmask = POP1;		//Lens
-		headset->axis2 = POP2;			//Lens Separation
-		readdata+=2;
-		headset->buttonmask |= POP1;	//Button
-		readdata+=3;
-		readdata++;						//Proxchange, No change = 0, Decrease = 1, Increase = 2
-		readdata++;
-		headset->axis3 = POP2;			//Proximity  	<< how close to face are you?  Less than 80 = not on face.
-		headset->axis1 = POP2;			//IPD   		<< what is this?
-		headset->ison = 1;
-		break;
-	}
-	case USB_IF_LIGHTHOUSE:
-	{
-		int i;
-		//printf( "%d -> ", size );
-		for( i = 0; i < 3; i++ )
-		{
-			int16_t * acceldata = (int16_t*)readdata;
-			readdata += 12;
-			uint32_t timecode = POP4;
-			uint8_t code = POP1;
-			//printf( "%d ", code );
-			int8_t cd = code - ctx->headset.oldcode;
-
-			if( cd > 0 )
-			{
-				ctx->headset.oldcode = code;
-				ctx->imuproc( &ctx->headset, acceldata, timecode, code );
-			}
-		}
-
-		//DONE OK.
-		break;
-	}
-	case USB_IF_WATCHMAN1:
-	case USB_IF_WATCHMAN2:
-	{
-		struct SurviveObject * w = &ctx->watchman[si->which_interface_am_i-USB_IF_WATCHMAN1];
-		if( id == 35 )
-		{
-			handle_watchman( w, readdata);
-		}
-		else if( id == 36 )
-		{
-			handle_watchman( w, readdata);
-			handle_watchman( w, readdata+29 );
-		}
-		else if( id == 38 )
-		{
-			w->ison = 0;
-		}
-		else
-		{
-			SV_INFO( "Unknown watchman code %d\n", id );
-		}
-		break;
-	}
-	case USB_IF_LIGHTCAP:
-	{
-		//Done!
-		int i;
-		for( i = 0; i < 7; i++ )
-		{
-			handle_lightcap( &ctx->headset, (struct LightcapElement*)&readdata[i*8] );
-		}
-		break;
-	}
-	}
-}
-
-
diff --git a/src/survive_driverman.c b/src/survive_driverman.c
new file mode 100644
index 0000000..b4684c8
--- /dev/null
+++ b/src/survive_driverman.c
@@ -0,0 +1,50 @@
+#include "survive_driverman.h"
+#include <string.h>
+#include <stdio.h>
+
+static void * Drivers[MAX_DRIVERS];
+static const char * DriverNames[MAX_DRIVERS];
+static int NrDrivers;
+
+void   RegisterDriver( const char * element, void * data )
+{
+	Drivers[NrDrivers] = data;
+	DriverNames[NrDrivers] = element;
+	NrDrivers++;
+}
+
+void * GetDriver( const char * element )
+{
+	int i;
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		if( strcmp( element, DriverNames[i] ) == 0 ) return Drivers[i];
+	}
+	return 0;
+}
+
+const char * GetDriverNameMatching( const char * prefix, int place )
+{
+	int i;
+	int prefixlen = strlen( prefix );
+
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		if( memcmp( prefix, DriverNames[i], prefixlen ) == 0 )
+			if( 0 == (place--) )
+				return DriverNames[i];
+	}
+	return 0;
+}
+
+void   ListDrivers()
+{
+	int i;
+	printf( "Drivers (%d/%d):\n", NrDrivers, MAX_DRIVERS );
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		printf( " %s\n", DriverNames[i] );
+	}
+
+}
+
diff --git a/src/survive_driverman.h b/src/survive_driverman.h
new file mode 100644
index 0000000..fb385da
--- /dev/null
+++ b/src/survive_driverman.h
@@ -0,0 +1,22 @@
+#ifndef SURVIVE_DRIVERMAN_H
+#define SURVIVE_DRIVERMAN_H
+
+//Driver registration
+#define MAX_DRIVERS 32
+
+void   RegisterDriver( const char * name, void * data );
+void * GetDriver( const char * name );
+const char * GetDriverNameMatching( const char * prefix, int place );
+void   ListDrivers();
+
+#define REGISTER_LINKTIME( func ) \
+	void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
+
+struct SurviveContext;
+
+typedef int (*DeviceDriver)( struct SurviveContext * ctx );
+typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
+typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
+
+#endif
+
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 19aa956..3dc471a 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -17,7 +17,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <libusb-1.0/libusb.h>
+#include "survive_driverman.h"
 #include <zlib.h>
 #include <survive.h>
 
@@ -27,39 +27,12 @@
 //XXX TODO This one needs to be rewritten.
 #define SV_KILL()		exit(0)
 
-#define USB_DEV_HMD			0
-#define USB_DEV_LIGHTHOUSE	1
-#define USB_DEV_WATCHMAN1	2
-#define USB_DEV_WATCHMAN2	3
-#define MAX_USB_DEVS				4
-
-
-#define USB_IF_HMD			0
-#define USB_IF_LIGHTHOUSE 	1
-#define USB_IF_WATCHMAN1	2
-#define USB_IF_WATCHMAN2	3
-#define USB_IF_LIGHTCAP		4
-#define MAX_INTERFACES				5
-
 #define INTBUFFSIZE			64
 #define SENSORS_PER_OBJECT	32
 
 struct SurviveContext;
 struct SurviveUSBInterface;
 
-typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
-
-struct SurviveUSBInterface
-{
-	struct libusb_transfer * transfer;
-	struct SurviveContext * ctx;
-	int actual_len;
-	uint8_t buffer[INTBUFFSIZE];
-	usb_callback cb;
-	int which_interface_am_i;	//for indexing into uiface
-	const char * hname;			//human-readable names
-};
-
 //This is defined in survive.h
 struct SurviveObject;
 struct SurviveCalData;
@@ -81,35 +54,43 @@ struct BaseStationData
 
 struct SurviveContext
 {
-	//USB Subsystem
-    struct libusb_context* usbctx;
-	struct libusb_device_handle * udev[MAX_USB_DEVS];
-	struct SurviveUSBInterface uiface[MAX_INTERFACES];
-
 	text_feedback_func faultfunction;
 	text_feedback_func notefunction;
 	light_process_func lightproc;
 	imu_process_func imuproc;
 	angle_process_func angleproc;
 
-
 	//Calibration data:
 	struct BaseStationData bsd[NUM_LIGHTHOUSES];
 
 	struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
 
-	//Data Subsystem.  These should be last, as there may be additional surviveobjects.
-	struct SurviveObject headset;
-	struct SurviveObject watchman[2]; //Currently only two supported watchmen.
+	struct SurviveObject ** objs;
+	int objs_ct;
 
+	void ** drivers;
+	DeviceDriverCb * driverpolls;
+	DeviceDriverCb * drivercloses;
+	DeviceDriverMagicCb * drivermagics;
+	int driver_ct;
 };
 
+int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj );
+
+void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
+
+//For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
+struct LightcapElement
+{
+	uint8_t sensor_id;
+	uint8_t type;
+	uint16_t length;
+	uint32_t timestamp;
+} __attribute__((packed));
+
+//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
+void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le );
 
-//USB Subsystem 
-void survive_usb_close( struct SurviveContext * t );
-int survive_usb_init( struct SurviveContext * t );
-int survive_usb_poll( struct SurviveContext * ctx );
-int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int interface, int send_extra_magic );
 
 //Accept Data from backend.
 void survive_data_cb( struct SurviveUSBInterface * si );
diff --git a/src/survive_usb.c b/src/survive_usb.c
index aec76db..743d805 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -10,435 +10,4 @@
 //If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
-
-#include "survive_internal.h"
-#include <libusb-1.0/libusb.h>
-#include <stdio.h>
-#include <unistd.h> //sleep if I ever use it.
-#include <errno.h>
-#include <string.h>
-
-const short vidpids[] = {
-	0x0bb4, 0x2c87, 0, //The main HTC HMD device
-	0x28de, 0x2000, 0, //Valve lighthouse
-	0x28de, 0x2101, 0, //Valve Watchman
-	0x28de, 0x2101, 1, //Valve Watchman
-}; //length MAX_USB_INTERFACES*2
-
-const char * devnames[] = {
-	"HMD",
-	"Lighthouse",
-	"Watchman 1",
-	"Watchman 2",
-}; //length MAX_USB_INTERFACES
-
-
-static void handle_transfer(struct libusb_transfer* transfer)
-{
-	struct SurviveUSBInterface * iface = transfer->user_data;
-	struct SurviveContext * ctx = iface->ctx;
-
-	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
-	{
-		SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname );
-		SV_KILL();
-		return;
-	}
-
-	iface->actual_len = transfer->actual_length;
-	iface->cb( iface );
-
-	if( libusb_submit_transfer(transfer) )
-	{
-		SV_ERROR( "Error resubmitting transfer for %s", iface->hname );
-		SV_KILL();
-	}
-}
-
-
-
-static int AttachInterface( struct SurviveContext * ctx, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
-{
-	struct SurviveUSBInterface * iface = &ctx->uiface[which_interface_am_i];
-	iface->ctx = ctx;
-	iface->which_interface_am_i = which_interface_am_i;
-	iface->hname = hname;
-	struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0);
-	iface->cb = cb;
-	//printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh );
-
-	if (!iface->transfer)
-	{
-		SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname );
-		return 4;
-	}
-
-	libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0);
-
-	int rc = libusb_submit_transfer( tx );
-	if( rc )
-	{
-		SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc );
-		return 6;
-	}
-
-	return 0;
-}
-
-/*
-static void debug_cb( struct SurviveUSBInterface * si )
-{
-	int i;
-	int len = si->actual_len;
-	printf( "%16s: %d: ", si->hname, len );
-	for( i = 0; i < len; i++ )
-	{
-		printf( "%02x ", si->buffer[i] );
-	}
-	printf( "\n" );
-}*/
-
-//XXX TODO: Redo this subsystem for setting/updating feature reports.
-
-static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
-//	int xfer;
-//	int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000);
-//	printf( "XFER: %d / R: %d\n", xfer, r );
-//	return xfer;
-	return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
-		0x09, 0x300 | data[0], interface, data, datalen, 1000 );
-}
-
-
-static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
-
-	int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
-		0x01, 0x300 | data[0], interface, data, datalen, 1000 );
-	if( ret == -9 ) return -9;
-    if (ret < 0)
-        return -1;
-	return ret;
-}
-
-
-
-static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
-{
-	int ret;
-	uint8_t i = 0;
-    for (i = 0; i < 100; i++)
-	{
-        ret = getupdate_feature_report(device, interface, buf, len);
-		if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
-		usleep( 1000 );
-	}
-
-	return -1;
-}
-
-
-int survive_usb_init( struct SurviveContext * ctx )
-{
-	int r = libusb_init( &ctx->usbctx );
-	if( r )
-	{
-		SV_ERROR( "libusb fault %d\n", r );
-		return r;
-	}
-
-	int i;
-	int16_t j;
-	libusb_device** devs;
-	int ret = libusb_get_device_list(ctx->usbctx, &devs);
-
-	if( ret < 0 )
-	{
-		SV_ERROR( "Couldn't get list of USB devices %d", ret );
-		return ret;
-	}
-
-
-	//Open all interfaces.
-	for( i = 0; i < MAX_USB_DEVS; i++ )
-	{
-		libusb_device * d;
-		int vid = vidpids[i*3+0];
-		int pid = vidpids[i*3+1];
-		int which = vidpids[i*3+2];
-
-		int did;
-		for( did = 0; d = devs[did]; did++ )
-		{
-			struct libusb_device_descriptor desc;
-
-			int ret = libusb_get_device_descriptor( d, &desc);
-			if (ret < 0) {
-				continue;
-			}
-
-			if( desc.idVendor == vid && desc.idProduct == pid)
-			{
-				if( which == 0 ) break;
-				which--;
-			}
-		}
-
-		if( d == 0 )
-		{
-			SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
-			return -99;
-		}
-
-		struct libusb_config_descriptor *conf;
-		ret = libusb_get_config_descriptor(d, 0, &conf);
-		if( ret )
-			continue;
-
-		ret = libusb_open(d, &ctx->udev[i]);
-
-		if( !ctx->udev[i] || ret )
-		{
-			SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid );
-			return -5;
-		}
-
-		libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 );
-		for (j = 0; j < conf->bNumInterfaces; j++ )
-		{
-#if 0
-		    if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) {
-		        ret = libusb_detach_kernel_driver(ctx->udev[i], j);
-		        if (ret != LIBUSB_SUCCESS) {
-		            SV_ERROR("Failed to unclaim interface %d for device %s "
-		                    "from the kernel.", j, devnames[i] );
-		            libusb_free_config_descriptor(conf);
-		            libusb_close(ctx->udev[i]);
-		            continue;
-		        }
-		    }
-#endif
-
-			if( libusb_claim_interface(ctx->udev[i], j) )
-			{
-				SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] );
-				return -9;
-			}
-		}
-
-		SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces );
-	}
-	libusb_free_device_list( devs, 1 );
-
-	if( AttachInterface( ctx, USB_IF_HMD,        ctx->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
-	if( AttachInterface( ctx, USB_IF_LIGHTHOUSE, ctx->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
-	if( AttachInterface( ctx, USB_IF_WATCHMAN1,  ctx->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
-	if( AttachInterface( ctx, USB_IF_WATCHMAN2,  ctx->udev[USB_DEV_WATCHMAN2],  0x81, survive_data_cb, "Watchman 2" ) ) { return -9; }
-	if( AttachInterface( ctx, USB_IF_LIGHTCAP,   ctx->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; }
-
-	SV_INFO( "All devices attached." );
-
-
-	//libUSB initialized.  Continue.
-	return 0;
-}
-
-int survive_usb_send_magic(struct SurviveContext * ctx, int turnon )
-{
-	int r;
-
-
-	if( turnon )
-	{
-		//Magic from vl_magic.h, originally copywritten under LGPL. 
-		// * Copyright (C) 2013 Fredrik Hultin
-		// * Copyright (C) 2013 Jakob Bornecrantz
-#if 0
-		static uint8_t vive_magic_power_on[] = {
-			0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
-			0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
-			0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
-			0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
-			0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
-		};
-#else
-		//From actual steam.
-		static uint8_t vive_magic_power_on[64] = {   0x04, 0x78, 0x29, 0x38,
-			0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-#endif
-		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
-		if( r != sizeof( vive_magic_power_on ) ) return 5;
-
-		static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 };  //[64] wat?  Why did that fix it?
-		r = update_feature_report( ctx->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
-		if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
-
-#if 0
-		for( i = 0; i < 256; i++ )
-		{
-			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
-			r = update_feature_report( ctx->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
-			SV_INFO( "UCR: %d", r );
-			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
-			usleep( 1000 );
-		}
-#endif
-		SV_INFO( "Powered unit on." );
-	}
-	else
-	{
-
-		static uint8_t vive_magic_power_off1[] = {
-			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
-			0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
-			0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
-			0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
-			0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
-		};
-
-		static uint8_t vive_magic_power_off2[] = {
-			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
-			0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
-			0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
-			0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
-			0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
-		};
-
-//		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) );
-//		SV_INFO( "UCR: %d", r );
-//		if( r != sizeof( vive_magic_power_off1 ) ) return 5;
-
-		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) );
-		SV_INFO( "UCR: %d", r );
-		if( r != sizeof( vive_magic_power_off2 ) ) return 5;
-
-	}
-
-}
-
-void survive_usb_close( struct SurviveContext * t )
-{
-	int i;
-	for( i = 0; i < MAX_USB_DEVS; i++ )
-	{
-	    libusb_close( t->udev[i] );
-	}
-    libusb_exit(t->usbctx);	
-}
-
-int survive_usb_poll( struct SurviveContext * ctx )
-{
-	int r = libusb_handle_events( ctx->usbctx );
-	if( r )
-	{
-		SV_ERROR( "Libusb poll failed." );
-	}
-	return r;
-}
-
-
-int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface, int send_extra_magic )
-{
-	int i, ret, count = 0, size = 0;
-	uint8_t cfgbuff[64];
-	uint8_t compressed_data[8192];
-	uint8_t uncompressed_data[65536];
-	struct libusb_device_handle * dev = ctx->udev[devno];
-
-	if( send_extra_magic )
-	{
-		uint8_t cfgbuffwide[65];
-
-		memset( cfgbuffwide, 0, sizeof( cfgbuff ) );
-		cfgbuffwide[0] = 0x01;
-		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
-		usleep(1000);
-
-		int k;
-
-		//Switch mode to pull config?
-		for( k = 0; k < 10; k++ )
-		{
-			uint8_t cfgbuff_send[64] = { 
-				0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69,
-				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18,
-				0x00, 0xe8,	0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a,
-				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69,
-				0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45,
-				0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69,
-				0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a,
-				0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00
-			};
-
-			int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
-			0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 );
-			usleep(1000);
-		}
-
-		cfgbuffwide[0] = 0xff;
-		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
-		usleep(1000);
-	}
-
-	memset( cfgbuff, 0, sizeof( cfgbuff ) );
-	cfgbuff[0] = 0x10;
-	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
-	{
-		SV_INFO( "Could not get survive config data for device %d:%d", devno, iface );
-		return -1;
-	}
-
-
-	cfgbuff[1] = 0xaa;
-	cfgbuff[0] = 0x11;
-	do
-	{
-		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
-		{
-			SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
-			return -2;
-		}
-
-		size = cfgbuff[1];
-
-		if( !size ) break;
-
-		if( size > 62 )
-		{
-			SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
-			return -3;
-		}
-
-		if( count + size >= sizeof( compressed_data ) )
-		{
-			SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
-			return -4;
-		}
-
-        memcpy( &compressed_data[count], cfgbuff + 2, size );
-		count += size;
-	} while( 1 );
-
-	if( count == 0 )
-	{
-		SV_INFO( "Empty configuration for %d:%d", devno, iface );
-		return -5;
-	}
-
-	SV_INFO( "Got config data length %d", count );
-
-	int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
-	if( len <= 0 )
-	{
-		SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface );
-		return -5;
-	}
-
-	*config = malloc( len + 1 );
-	memcpy( *config, uncompressed_data, len );
-	return len;
-}
-
-
+//No code?
diff --git a/src/survive_vive.c b/src/survive_vive.c
new file mode 100644
index 0000000..a402153
--- /dev/null
+++ b/src/survive_vive.c
@@ -0,0 +1,1075 @@
+//Unofficial driver for the official Valve/HTC Vive hardware.
+//
+//Based off of https://github.com/collabora/OSVR-Vive-Libre
+// Originally Copyright 2016 Philipp Zabel
+// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk>
+// Originally Copyright (C) 2013 Fredrik Hultin
+// Originally Copyright (C) 2013 Jakob Bornecrantz
+//
+//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license.
+//If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#include "survive_internal.h"
+#include "survive_driverman.h"
+#include <jsmn.h>
+
+#include "survive_internal.h"
+#include <libusb-1.0/libusb.h>
+#include <stdio.h>
+#include <unistd.h> //sleep if I ever use it.
+#include <errno.h>
+#include <string.h>
+
+struct SurviveViveData;
+
+//USB Subsystem 
+void survive_usb_close( struct SurviveContext * t );
+int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 );
+int survive_usb_poll( struct SurviveContext * ctx );
+int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
+
+
+const short vidpids[] = {
+	0x0bb4, 0x2c87, 0, //The main HTC HMD device
+	0x28de, 0x2000, 0, //Valve lighthouse
+	0x28de, 0x2101, 0, //Valve Watchman
+	0x28de, 0x2101, 1, //Valve Watchman
+}; //length MAX_USB_INTERFACES*2
+
+const char * devnames[] = {
+	"HMD",
+	"Lighthouse",
+	"Watchman 1",
+	"Watchman 2",
+}; //length MAX_USB_INTERFACES
+
+
+#define USB_DEV_HMD			0
+#define USB_DEV_LIGHTHOUSE	1
+#define USB_DEV_WATCHMAN1	2
+#define USB_DEV_WATCHMAN2	3
+#define MAX_USB_DEVS				4
+
+
+#define USB_IF_HMD			0
+#define USB_IF_LIGHTHOUSE 	1
+#define USB_IF_WATCHMAN1	2
+#define USB_IF_WATCHMAN2	3
+#define USB_IF_LIGHTCAP		4
+#define MAX_INTERFACES				5
+
+typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
+
+struct SurviveUSBInterface
+{
+	struct SurviveViveData * sv;
+	struct SurviveContext * ctx;
+
+	struct libusb_transfer * transfer;
+	struct SurviveObject * assoc_obj;
+	int actual_len;
+	uint8_t buffer[INTBUFFSIZE];
+	usb_callback cb;
+	int which_interface_am_i;	//for indexing into uiface
+	const char * hname;			//human-readable names
+};
+
+struct SurviveViveData
+{
+	struct SurviveContext * ctx;
+
+	//XXX TODO: UN-STATICIFY THIS.
+	struct SurviveUSBInterface uiface[MAX_INTERFACES];
+	//USB Subsystem
+	struct libusb_context* usbctx;
+	struct libusb_device_handle * udev[MAX_USB_DEVS];
+
+};
+
+
+
+
+static void handle_transfer(struct libusb_transfer* transfer)
+{
+	struct SurviveUSBInterface * iface = transfer->user_data;
+	struct SurviveContext * ctx = iface->ctx;
+
+	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
+	{
+		SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname );
+		SV_KILL();
+		return;
+	}
+
+	iface->actual_len = transfer->actual_length;
+	iface->cb( iface );
+
+	if( libusb_submit_transfer(transfer) )
+	{
+		SV_ERROR( "Error resubmitting transfer for %s", iface->hname );
+		SV_KILL();
+	}
+}
+
+
+
+static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
+	iface->ctx = ctx;
+	iface->sv = sv;
+	iface->which_interface_am_i = which_interface_am_i;
+	iface->assoc_obj = assocobj;
+	iface->hname = hname;
+	struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0);
+	iface->cb = cb;
+	//printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh );
+
+	if (!iface->transfer)
+	{
+		SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname );
+		return 4;
+	}
+
+	libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0);
+
+	int rc = libusb_submit_transfer( tx );
+	if( rc )
+	{
+		SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc );
+		return 6;
+	}
+
+	return 0;
+}
+
+/*
+static void debug_cb( struct SurviveUSBInterface * si )
+{
+	int i;
+	int len = si->actual_len;
+	printf( "%16s: %d: ", si->hname, len );
+	for( i = 0; i < len; i++ )
+	{
+		printf( "%02x ", si->buffer[i] );
+	}
+	printf( "\n" );
+}*/
+
+//XXX TODO: Redo this subsystem for setting/updating feature reports.
+
+static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
+//	int xfer;
+//	int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000);
+//	printf( "XFER: %d / R: %d\n", xfer, r );
+//	return xfer;
+	return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+		0x09, 0x300 | data[0], interface, data, datalen, 1000 );
+}
+
+
+static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
+
+	int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+		0x01, 0x300 | data[0], interface, data, datalen, 1000 );
+	if( ret == -9 ) return -9;
+    if (ret < 0)
+        return -1;
+	return ret;
+}
+
+
+
+static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
+{
+	int ret;
+	uint8_t i = 0;
+    for (i = 0; i < 100; i++)
+	{
+        ret = getupdate_feature_report(device, interface, buf, len);
+		if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
+		usleep( 1000 );
+	}
+
+	return -1;
+}
+
+
+int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	int r = libusb_init( &sv->usbctx );
+	if( r )
+	{
+		SV_ERROR( "libusb fault %d\n", r );
+		return r;
+	}
+
+	int i;
+	int16_t j;
+	libusb_device** devs;
+	int ret = libusb_get_device_list(sv->usbctx, &devs);
+
+	if( ret < 0 )
+	{
+		SV_ERROR( "Couldn't get list of USB devices %d", ret );
+		return ret;
+	}
+
+
+	//Open all interfaces.
+	for( i = 0; i < MAX_USB_DEVS; i++ )
+	{
+		libusb_device * d;
+		int vid = vidpids[i*3+0];
+		int pid = vidpids[i*3+1];
+		int which = vidpids[i*3+2];
+
+		int did;
+		for( did = 0; d = devs[did]; did++ )
+		{
+			struct libusb_device_descriptor desc;
+
+			int ret = libusb_get_device_descriptor( d, &desc);
+			if (ret < 0) {
+				continue;
+			}
+
+			if( desc.idVendor == vid && desc.idProduct == pid)
+			{
+				if( which == 0 ) break;
+				which--;
+			}
+		}
+
+		if( d == 0 )
+		{
+			SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
+			return -99;
+		}
+
+		struct libusb_config_descriptor *conf;
+		ret = libusb_get_config_descriptor(d, 0, &conf);
+		if( ret )
+			continue;
+
+		ret = libusb_open(d, &sv->udev[i]);
+
+		if( !sv->udev[i] || ret )
+		{
+			SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid );
+			return -5;
+		}
+
+		libusb_set_auto_detach_kernel_driver( sv->udev[i], 1 );
+		for (j = 0; j < conf->bNumInterfaces; j++ )
+		{
+#if 0
+		    if (libusb_kernel_driver_active(sv->udev[i], j) == 1) {
+		        ret = libusb_detach_kernel_driver(sv->udev[i], j);
+		        if (ret != LIBUSB_SUCCESS) {
+		            SV_ERROR("Failed to unclaim interface %d for device %s "
+		                    "from the kernel.", j, devnames[i] );
+		            libusb_free_config_descriptor(conf);
+		            libusb_close(sv->udev[i]);
+		            continue;
+		        }
+		    }
+#endif
+
+			if( libusb_claim_interface(sv->udev[i], j) )
+			{
+				SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] );
+				return -9;
+			}
+		}
+
+		SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces );
+	}
+	libusb_free_device_list( devs, 1 );
+
+	if( AttachInterface( sv, hmd, USB_IF_HMD,        sv->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
+	if( AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
+	if( AttachInterface( sv, wm0, USB_IF_WATCHMAN1,  sv->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
+	if( AttachInterface( sv, wm1, USB_IF_WATCHMAN2,  sv->udev[USB_DEV_WATCHMAN2],  0x81, survive_data_cb, "Watchman 2" ) ) { return -9; }
+	if( AttachInterface( sv, hmd, USB_IF_LIGHTCAP,   sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; }
+
+	SV_INFO( "All devices attached." );
+
+
+	//libUSB initialized.  Continue.
+	return 0;
+}
+
+int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
+{
+	int r;
+	struct SurviveViveData * sv = drv;
+	printf( "*CALLING %p %p\n", ctx, sv );
+
+	//XXX TODO: Handle haptics, etc.
+	int turnon = magic_code;
+
+
+	if( turnon )
+	{
+		//Magic from vl_magic.h, originally copywritten under LGPL. 
+		// * Copyright (C) 2013 Fredrik Hultin
+		// * Copyright (C) 2013 Jakob Bornecrantz
+#if 0
+		static uint8_t vive_magic_power_on[] = {
+			0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
+			0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
+			0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
+			0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
+			0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
+		};
+#else
+		//From actual steam.
+		static uint8_t vive_magic_power_on[64] = {   0x04, 0x78, 0x29, 0x38,
+			0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+#endif
+		r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+		if( r != sizeof( vive_magic_power_on ) ) return 5;
+
+		static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 };  //[64] wat?  Why did that fix it?
+		r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+		if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+#if 0
+		for( i = 0; i < 256; i++ )
+		{
+			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
+			r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+			SV_INFO( "UCR: %d", r );
+			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
+			usleep( 1000 );
+		}
+#endif
+		SV_INFO( "Powered unit on." );
+	}
+	else
+	{
+
+		static uint8_t vive_magic_power_off1[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
+			0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
+			0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
+
+		static uint8_t vive_magic_power_off2[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
+			0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
+			0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
+
+//		r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) );
+//		SV_INFO( "UCR: %d", r );
+//		if( r != sizeof( vive_magic_power_off1 ) ) return 5;
+
+		r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) );
+		SV_INFO( "UCR: %d", r );
+		if( r != sizeof( vive_magic_power_off2 ) ) return 5;
+
+	}
+
+}
+
+void survive_vive_usb_close( struct SurviveViveData * sv )
+{
+	int i;
+	for( i = 0; i < MAX_USB_DEVS; i++ )
+	{
+	    libusb_close( sv->udev[i] );
+	}
+    libusb_exit(sv->usbctx);	
+}
+
+int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
+{
+	struct SurviveViveData * sv = v;
+	int r = libusb_handle_events( sv->usbctx );
+	if( r )
+	{
+		struct SurviveContext * ctx = sv->ctx;
+		SV_ERROR( "Libusb poll failed." );
+	}
+	return r;
+}
+
+
+int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	int i, ret, count = 0, size = 0;
+	uint8_t cfgbuff[64];
+	uint8_t compressed_data[8192];
+	uint8_t uncompressed_data[65536];
+	struct libusb_device_handle * dev = sv->udev[devno];
+
+	if( send_extra_magic )
+	{
+		uint8_t cfgbuffwide[65];
+
+		memset( cfgbuffwide, 0, sizeof( cfgbuff ) );
+		cfgbuffwide[0] = 0x01;
+		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
+		usleep(1000);
+
+		int k;
+
+		//Switch mode to pull config?
+		for( k = 0; k < 10; k++ )
+		{
+			uint8_t cfgbuff_send[64] = { 
+				0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69,
+				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18,
+				0x00, 0xe8,	0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a,
+				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69,
+				0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45,
+				0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69,
+				0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a,
+				0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00
+			};
+
+			int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+			0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 );
+			usleep(1000);
+		}
+
+		cfgbuffwide[0] = 0xff;
+		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
+		usleep(1000);
+	}
+
+	memset( cfgbuff, 0, sizeof( cfgbuff ) );
+	cfgbuff[0] = 0x10;
+	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+	{
+		SV_INFO( "Could not get survive config data for device %d:%d", devno, iface );
+		return -1;
+	}
+
+
+	cfgbuff[1] = 0xaa;
+	cfgbuff[0] = 0x11;
+	do
+	{
+		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+		{
+			SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
+			return -2;
+		}
+
+		size = cfgbuff[1];
+
+		if( !size ) break;
+
+		if( size > 62 )
+		{
+			SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
+			return -3;
+		}
+
+		if( count + size >= sizeof( compressed_data ) )
+		{
+			SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
+			return -4;
+		}
+
+        memcpy( &compressed_data[count], cfgbuff + 2, size );
+		count += size;
+	} while( 1 );
+
+	if( count == 0 )
+	{
+		SV_INFO( "Empty configuration for %d:%d", devno, iface );
+		return -5;
+	}
+
+	SV_INFO( "Got config data length %d", count );
+
+	int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
+	if( len <= 0 )
+	{
+		SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface );
+		return -5;
+	}
+
+	*config = malloc( len + 1 );
+	memcpy( *config, uncompressed_data, len );
+	return len;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+#define POP1  (*(readdata++))
+#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
+#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
+
+static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
+{
+	int i;
+
+	uint8_t startread[29];
+	memcpy( startread, readdata, 29 );
+
+#if 0
+	printf( "DAT:     " );
+		for( i = 0; i < 29; i++ )
+		{
+			printf( "%02x ", readdata[i] );
+		}
+		printf("\n");
+#endif
+
+	uint8_t time1 = POP1;
+	uint8_t qty = POP1;
+	uint8_t time2 = POP1;
+	uint8_t type = POP1;
+	qty-=2;
+	int propset = 0;
+	int doimu = 0;
+
+
+	if( (type & 0xf0) == 0xf0 )
+	{
+		propset |= 4;
+		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
+		type &= ~0x10;
+
+		if( type & 0x01 )
+		{
+			qty-=1;
+			w->buttonmask = POP1;
+			type &= ~0x01;
+		}
+		if( type & 0x04 ) 
+		{
+			qty-=1;
+			w->axis1 = ( POP1 ) * 128; 
+			type &= ~0x04;
+		}
+		if( type & 0x02 )
+		{
+			qty-=4;
+			w->axis2 = POP2;
+			w->axis3 = POP2;
+			type &= ~0x02;
+		}
+
+		//XXX TODO: Is this correct?  It looks SO WACKY
+		type &= 0x7f;
+		if( type == 0x68 ) doimu = 1;
+		type &= 0x0f;
+		if( type == 0x00 && qty ) { type = POP1; qty--; }
+	}
+
+	if( type == 0xe1 )
+	{
+		propset |= 1;
+		w->charging = readdata[0]>>7;
+		w->charge = POP1&0x7f; qty--;
+		w->ison = 1; 
+		if( qty )
+		{
+			qty--;
+			type = POP1; //IMU usually follows.
+		}
+	}
+
+	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
+	{
+		propset |= 2;
+		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+		int16_t * k = (int16_t *)readdata+1;
+		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
+		readdata += 13; qty -= 13;
+		type &= ~0xe8;
+		if( qty )
+		{
+			qty--;
+			type = POP1;
+		}
+	}
+
+
+	if( qty )
+	{
+		int j;
+		qty++;
+		readdata--;
+		*readdata = type; //Put 'type' back on stack.
+		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
+
+//#define DEBUG_WATCHMAN
+#ifdef DEBUG_WATCHMAN
+		printf( "_%s ", w->codename);
+		for( i = 0; i < qty; i++ )
+		{
+			printf( "%02x ", readdata[i] );
+		}
+		printf("\n");
+#endif
+
+
+		uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0);
+
+		uint32_t times[20];
+		const int nrtime = sizeof(times)/sizeof(uint32_t);
+		int timecount = 0;
+		int leds;
+		int parameters;
+		int fault = 0;
+
+		///Handle uint32_tifying (making sure we keep it incrementing)
+		uint32_t llt = w->last_lighttime;
+		uint32_t imumsb = time1<<24;
+		mytime |= imumsb;
+
+		//Compare mytime to llt
+
+		int diff = mytime - llt;
+		if( diff < -0x1000000 )
+			mytime += 0x1000000;
+		else if( diff > 0x100000 )
+			mytime -= 0x1000000;
+
+		w->last_lighttime = mytime;
+
+		times[timecount++] = mytime;
+#ifdef DEBUG_WATCHMAN
+		printf( "_%s Packet Start Time: %d\n", w->codename, mytime );
+#endif	
+
+		//First, pull off the times, starting with the current time, then all the delta times going backwards.
+		{
+			while( mptr - readdata > (timecount>>1) )
+			{
+				uint32_t arcane_value = 0;
+				//ArcanePop (Pop off values from the back, forward, checking if the MSB is set)
+				do
+				{
+					uint8_t ap = *(mptr--);
+					arcane_value |= (ap&0x7f);
+					if( ap & 0x80 )  break;
+					arcane_value <<= 7;
+				} while(1);
+				times[timecount++] = (mytime -= arcane_value);
+#ifdef DEBUG_WATCHMAN
+				printf( "_%s Time: %d  newtime: %d\n", w->codename, arcane_value, mytime );
+#endif
+			}
+
+			leds = timecount>>1;
+			//Check that the # of sensors at the beginning match the # of parameters we would expect.
+			if( timecount & 1 ) { fault = 1; goto end; }				//Inordinal LED count
+			if( leds != mptr - readdata + 1 ) { fault = 2; goto end; }	//LED Count does not line up with parameters
+		}
+
+
+		struct LightcapElement les[10];
+		int lese = 0; //les's end
+
+		//Second, go through all LEDs and extract the lightevent from them. 
+		{
+			uint8_t marked[nrtime];
+			memset( marked, 0, sizeof( marked ) );
+			int i, parpl = 0;
+			timecount--;
+			int timepl = 0;
+
+			//This works, but usually returns the values in reverse end-time order.
+			for( i = 0; i < leds; i++ )
+			{
+				int led = readdata[i];
+				int adv = led & 0x07;
+				led >>= 3;
+
+				while( marked[timepl] ) timepl++;
+				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
+				uint32_t endtime = times[timepl++];
+				int end = timepl + adv;
+				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
+				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
+				uint32_t starttime = times[end];
+				marked[end] = 1;
+
+				//Insert all lighting things into a sorted list.  This list will be
+				//reverse sorted, but that is to minimize operations.  To read it
+				//in sorted order simply read it back backwards.
+				//Use insertion sort, since we should most of the time, be in order.
+				struct LightcapElement * le = &les[lese++];
+				le->sensor_id = led;
+				le->type = 0xfe;
+
+				if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb.
+				le->length = endtime - starttime;
+				le->timestamp = starttime;
+
+#ifdef DEBUG_WATCHMAN
+				printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime );
+#endif
+				int swap = lese-2;
+				while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
+				{
+					struct LightcapElement l;
+					memcpy( &l, &les[swap], sizeof( l ) );
+					memcpy( &les[swap], &les[swap+1], sizeof( l ) );
+					memcpy( &les[swap+1], &l, sizeof( l ) );
+					swap--;
+				}
+			}
+		}
+
+		int i;
+		for( i = lese-1; i >= 0; i-- )
+		{
+			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			handle_lightcap( w, &les[i] );
+		}
+
+		return;
+	end:
+		{
+			struct SurviveContext * ctx = w->ctx;
+			SV_INFO( "Light decoding fault: %d\n", fault );
+		}
+	}
+}
+
+
+void survive_data_cb( struct SurviveUSBInterface * si )
+{
+	int size = si->actual_len;
+	struct SurviveContext * ctx = si->ctx;
+#if 0
+	int i;
+	printf( "%16s: %d: ", si->hname, len );
+	for( i = 0; i < size; i++ )
+	{
+		printf( "%02x ", si->buffer[i] );
+	}
+	printf( "\n" );
+	return;
+#endif 
+
+	int iface = si->which_interface_am_i;
+	struct SurviveObject * obj = si->assoc_obj;
+	uint8_t * readdata = si->buffer;
+
+	int id = POP1;
+//	printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface );
+
+
+	switch( si->which_interface_am_i )
+	{
+	case USB_IF_HMD:
+	{
+		struct SurviveObject * headset = obj;
+		readdata+=2;
+		headset->buttonmask = POP1;		//Lens
+		headset->axis2 = POP2;			//Lens Separation
+		readdata+=2;
+		headset->buttonmask |= POP1;	//Button
+		readdata+=3;
+		readdata++;						//Proxchange, No change = 0, Decrease = 1, Increase = 2
+		readdata++;
+		headset->axis3 = POP2;			//Proximity  	<< how close to face are you?  Less than 80 = not on face.
+		headset->axis1 = POP2;			//IPD   		<< what is this?
+		headset->ison = 1;
+		break;
+	}
+	case USB_IF_LIGHTHOUSE:
+	{
+		int i;
+		//printf( "%d -> ", size );
+		for( i = 0; i < 3; i++ )
+		{
+			int16_t * acceldata = (int16_t*)readdata;
+			readdata += 12;
+			uint32_t timecode = POP4;
+			uint8_t code = POP1;
+			//printf( "%d ", code );
+			int8_t cd = code - obj->oldcode;
+
+			if( cd > 0 )
+			{
+				obj->oldcode = code;
+				ctx->imuproc( obj, acceldata, timecode, code );
+			}
+		}
+
+		//DONE OK.
+		break;
+	}
+	case USB_IF_WATCHMAN1:
+	case USB_IF_WATCHMAN2:
+	{
+		struct SurviveObject * w = obj;
+		if( id == 35 )
+		{
+			handle_watchman( w, readdata);
+		}
+		else if( id == 36 )
+		{
+			handle_watchman( w, readdata);
+			handle_watchman( w, readdata+29 );
+		}
+		else if( id == 38 )
+		{
+			w->ison = 0;
+		}
+		else
+		{
+			SV_INFO( "Unknown watchman code %d\n", id );
+		}
+		break;
+	}
+	case USB_IF_LIGHTCAP:
+	{
+		//Done!
+		int i;
+		for( i = 0; i < 7; i++ )
+		{
+			handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] );
+		}
+		break;
+	}
+	}
+}
+
+
+
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+
+
+static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
+ if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
+    strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
+		return 0;
+	}
+	return -1;
+}
+
+
+static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
+{
+	int k;
+	int pts = t[i+1].size;
+	jsmntok_t * tk;
+
+	so->nr_locations = 0;
+	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
+
+	for( k = 0; k < pts; k++ )
+	{
+		tk = &t[i+2+k*4];
+
+		FLT vals[3];
+		int m;
+		for( m = 0; m < 3; m++ )
+		{
+			char ctt[128];
+
+			tk++;
+			int elemlen = tk->end - tk->start;
+
+			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
+			{
+				SV_ERROR( "Parse error in JSON\n" );
+				return 1;
+			}
+
+			memcpy( ctt, ct0conf + tk->start, elemlen );
+			ctt[elemlen] = 0;
+			FLT f = atof( ctt );
+			int id = so->nr_locations*3+m;
+			(*floats_out)[id] = f;
+		}
+		so->nr_locations++;
+	}
+	return 0;
+}
+
+static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	char * ct0conf = 0;
+	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
+
+#if 0
+	char fname[100];
+	sprintf( fname, "%s_config.json", so->codename );
+	FILE * f = fopen( fname, "w" );
+	fwrite( ct0conf, strlen(ct0conf), 1, f );
+	fclose( f );
+#endif
+
+	if( len > 0 )
+	{
+
+		//From JSMN example.
+		jsmn_parser p;
+		jsmntok_t t[4096];
+		jsmn_init(&p);
+		int i;
+		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
+		if (r < 0) {
+			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+			return -1;
+		}
+		if (r < 1 || t[0].type != JSMN_OBJECT) {
+			SV_INFO("Object expected in HMD configuration\n");
+			return -2;
+		}
+
+		for (i = 1; i < r; i++) {
+			jsmntok_t * tk = &t[i];
+
+			char ctxo[100];
+			int ilen = tk->end - tk->start;
+			if( ilen > 99 ) ilen = 99;
+			memcpy(ctxo, ct0conf + tk->start, ilen);
+			ctxo[ilen] = 0;
+
+//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
+//				printf( "%.*s\n", ilen, ct0conf + tk->start );
+
+			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
+				if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i  ) )
+				{
+					break;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+				if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
+				{
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		//TODO: Cleanup any remaining USB stuff.
+		return 1;
+	}
+	return 0;
+}
+
+
+int survive_vive_close( struct SurviveContext * ctx, void * driver )
+{
+	struct SurviveViveData * sv = driver;
+ 
+	survive_vive_usb_close( sv );
+}
+
+
+int DriverRegHTCVive( struct SurviveContext * ctx )
+{
+	int i, r;
+	struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) );
+
+	sv->ctx = ctx;
+
+	hmd->ctx = ctx;
+	memcpy( hmd->codename, "HMD", 4 );
+	memcpy( hmd->drivername, "HTC", 4 );
+	wm0->ctx = ctx;
+	memcpy( wm0->codename, "WM0", 4 );
+	memcpy( wm0->drivername, "HTC", 4 );
+	wm1->ctx = ctx;
+	memcpy( wm1->codename, "WM1", 4 );
+	memcpy( wm1->drivername, "HTC", 4 );
+
+	//USB must happen last.
+	if( r = survive_usb_init( sv, hmd, wm0, wm1 ) )
+	{
+		//TODO: Cleanup any libUSB stuff sitting around.
+		goto fail_gracefully;
+	}
+
+	//Next, pull out the config stuff.
+	if( LoadConfig( sv, hmd, 1, 0, 0 ) ) goto fail_gracefully;
+	if( LoadConfig( sv, wm0, 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
+	if( LoadConfig( sv, wm1, 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
+
+	hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000;
+	hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000;
+	hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200;
+	hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000;
+	hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800;
+
+	hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000;
+	hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000;
+
+	hmd->timecenter_ticks = hmd->timebase_hz / 240;
+	wm0->timecenter_ticks = wm0->timebase_hz / 240;
+	wm1->timecenter_ticks = wm1->timebase_hz / 240;
+/*
+	int i;
+	int locs = hmd->nr_locations;
+	printf( "Locs: %d\n", locs );
+	if (hmd->sensor_locations )
+	{
+		printf( "POSITIONS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
+		{
+			printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] );
+		}
+	}
+	if( hmd->sensor_normals )
+	{
+		printf( "NORMALS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
+		{
+			printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] );
+		}
+	}
+*/
+
+	//Add the drivers.
+
+	survive_add_object( ctx, hmd );
+	survive_add_object( ctx, wm0 );
+	survive_add_object( ctx, wm1 );
+	survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+
+	return 0;
+fail_gracefully:
+	free( hmd );
+	free( wm0 );
+	free( wm1 );
+	survive_vive_usb_close( sv );
+	free( sv );
+	return -1;
+}
+
+REGISTER_LINKTIME( DriverRegHTCVive );
+
diff --git a/test.c b/test.c
index 17fa7f3..d5845d3 100644
--- a/test.c
+++ b/test.c
@@ -59,7 +59,7 @@ int main()
 		double Now = OGGetAbsoluteTime();
 		if( Now > (Start+1) && !magicon )
 		{
-			survive_usb_send_magic(ctx,1);
+			survive_send_magic(ctx,1,0,0);
 			magicon = 1;
 		}
 		//Do stuff.