first stab at trying to pull off config data, didn't work.

4 files changed, 192 insertions, 27 deletions

diff --git a/src/survive.c b/src/survive.c
index 223cd57..3712537 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -6,36 +6,44 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <zlib.h>
+
 
 struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) )
 {
 	int r = 0;
-	struct SurviveContext * ret = calloc( 1, sizeof( struct SurviveContext  ) );
-
-	ret->headset.sensors = 32;
-	ret->headset.ctx = ret;
-	memcpy( ret->headset.codename, "HED", 4 );
+	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext  ) );
 
-	ret->watchman[0].sensors = 16;
-	ret->watchman[0].ctx = ret;
-	memcpy( ret->watchman[0].codename, "CT0", 4 );
+	ctx->faultfunction = ff;
+	ctx->notefunction = notefunction;
 
-	ret->watchman[1].sensors = 16;
-	ret->watchman[1].ctx = ret;
-	memcpy( ret->watchman[1].codename, "CT1", 4 );
+	ctx->headset.sensors = 32;
+	ctx->headset.ctx = ctx;
+	memcpy( ctx->headset.codename, "HED", 4 );
 
+	ctx->watchman[0].sensors = 16;
+	ctx->watchman[0].ctx = ctx;
+	memcpy( ctx->watchman[0].codename, "CT0", 4 );
 
-	ret->faultfunction = ff;
-	ret->notefunction = notefunction;
+	ctx->watchman[1].sensors = 16;
+	ctx->watchman[1].ctx = ctx;
+	memcpy( ctx->watchman[1].codename, "CT1", 4 );
 
 	//USB must happen last.
-	if( r = survive_usb_init( ret ) )
+	if( r = survive_usb_init( ctx ) )
 	{
 		return 0;
 	}
 
-	//ret->headset->photos = malloc( ret->headset->sensors * sizeof(struct SurvivePhoto) );
-	return ret;
+	//Next, pull out the config stuff.
+/*	char * ct0conf;
+	int len = survive_get_config( &ct0conf, ctx, 1, 0 );
+	printf( "%d\n", len );
+	puts( ct0conf );
+*/
+
+	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );
+	return ctx;
 }
 
 
@@ -49,3 +57,26 @@ int survive_poll( struct SurviveContext * ctx )
 	survive_usb_poll( ctx );
 }
 
+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.
+	memset( &zs, 0, sizeof( zs ) );
+	inflateInit( &zs ); ///Consider checking error
+
+	//XXX: Todo: If we find that this is not useful past the beginning (nix this here and move into the configuration getter)
+    zs.avail_in = inlen;
+    zs.next_in = (z_const Bytef *)input;
+    zs.avail_out = outlen;
+	zs.next_out = output;
+
+    if( inflate( &zs, Z_FINISH) != Z_STREAM_END )
+	{
+		printf( "Zissue\n" );
+        SV_INFO("survive_simple_inflate could not inflate." );
+        return -1;
+	}
+	int len = zs.total_out;
+	inflateEnd( &zs );
+	return len;
+}
+
diff --git a/src/survive_data.c b/src/survive_data.c
index 53c4444..52c7c12 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -35,7 +35,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 	if( le->type != 0xfe || le->length < 50 ) return;
 	//le->timestamp += (le->length/2);
-
 	if( le->length > 900 ) //Pulse longer than 18us? 
 	{
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time;
@@ -79,15 +78,28 @@ 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 1
+	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;
-	int i;
 	qty-=2;
 	int propset = 0;
+	int doimu = 0;
+
 
 	if( (type & 0xf0) == 0xf0 )
 	{
@@ -115,11 +127,11 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 			type &= ~0x02;
 		}
 
-		//XXX TODO: Maybe more data is here?
-		if( type == 0xe0 )
-		{
-			type = 0x00;
-		}
+		//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 )
@@ -135,7 +147,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		}
 	}
 
-	if( ( type & 0xe8 ) == 0xe8 )
+	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;
 		survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
@@ -192,8 +204,13 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 
 
 #if 1
-		printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 );
-		for( i = 0; i < qty; i++ )
+		static int lasttime;
+		//             good        good          maybe?         probably wrong
+		int mytime = (time1<<24)|(time2<<16)|(readdata[4]<<8)|(readdata[5]);
+		int diff = mytime - lasttime;
+		lasttime = mytime;
+		printf( "POST %d: %4d (%02x%02x) (%9d)- ", propset, qty, time1, time2, diff );
+		for( i = 0; i < qty + 4; i++ )
 		{
 			printf( "%02x ", readdata[i] );
 		}
@@ -287,6 +304,13 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 		//printf( "%d -> ", size );
 		for( i = 0; i < 3; i++ )
 		{
+/*
+			for( i = 0; i < 17; i++ )
+			{
+				printf( "%02x ", readdata[i] );
+			}
+			printf( "\n" );
+*/
 			//handle_lightdata( (struct LightpulseStructure *)readdata );
 			int16_t * acceldata = (int16_t*)readdata;
 			readdata += 12;
@@ -301,6 +325,9 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 				survive_imu_process( &ctx->headset, acceldata, timecode, code );
 			}
 		}
+
+		//DONE OK.
+
 		//printf("\n" );
 		break;
 	}
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 8bb421d..8096b8c 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -18,6 +18,7 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <libusb-1.0/libusb.h>
+#include <zlib.h>
 
 #define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); }
 #define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); }
@@ -82,6 +83,7 @@ struct SurviveContext
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 	struct SurviveUSBInterface uiface[MAX_INTERFACES];
 
+
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
 	int32_t last_photo_time;
@@ -100,6 +102,7 @@ struct SurviveContext
 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 );
 
 //Accept Data from backend.
 void survive_data_cb( struct SurviveUSBInterface * si );
@@ -109,6 +112,10 @@ void survive_light_process( struct SurviveObject * so, int sensor_id, int acode,
 void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 
 
+//Util
+int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
+
+
 #endif
 
 
diff --git a/src/survive_usb.c b/src/survive_usb.c
index 55e51a1..e7472a4 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -15,7 +15,8 @@
 #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
@@ -108,6 +109,31 @@ static inline int update_feature_report(libusb_device_handle* dev, uint16_t inte
 }
 
 
+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 < 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;
+    for (unsigned i = 0; i < 100; i++)
+	{
+        ret = getupdate_feature_report(device, interface, buf, len);
+		printf( "HUD: %d/%d %02x %02x %02x\n", ret, len, buf[0], buf[1], buf[2] );
+		if( ret != -1 || errno != EPIPE ) return ret;
+		usleep( 1000 );
+	}
+
+	return -1;
+}
+
+
 int survive_usb_init( struct SurviveContext * ctx )
 {
 	int r = libusb_init( &ctx->usbctx );
@@ -272,3 +298,77 @@ int survive_usb_poll( struct SurviveContext * ctx )
 	return r;
 }
 
+
+int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface )
+{
+	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];
+
+	cfgbuff[0] = 0x10;
+	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+	{
+		SV_ERROR( "Could not get survive config data for device %d:%d", devno, iface );
+		return -1;
+	}
+
+	cfgbuff[0] = 0x11;
+	do
+	{
+		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+		{
+			SV_ERROR( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
+			return -2;
+		}
+
+		size = cfgbuff[1];
+		printf( "Tag: " );
+		for( i = 0; i < 64; i++ )
+			printf( "%02x ", cfgbuff[i] );
+		printf( "ret: %d %d\n", ret, size );
+
+		if( !size ) break;
+
+		if( size > 62 )
+		{
+			SV_ERROR( "Too much data on packet from config for device %d:%d (count: %d)", devno, iface, count );
+			return -3;
+		}
+
+		if( count + size >= sizeof( compressed_data ) )
+		{
+			SV_ERROR( "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_ERROR( "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_ERROR( "Error: data for config descriptor %d:%d is bad.", devno, iface );
+		return -5;
+	}
+
+	config = malloc( len + 1 );
+	memcpy( config, uncompressed_data, len );
+	return len;
+}
+
+
+
+
+
+