From 684b109e8cd3246c93fdec3e63157bbd11bb359a Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 3 Dec 2016 18:02:20 -0500 Subject: first stab at trying to pull off config data, didn't work. --- src/survive.c | 63 ++++++++++++++++++++++-------- src/survive_data.c | 47 ++++++++++++++++++----- src/survive_internal.h | 7 ++++ src/survive_usb.c | 102 ++++++++++++++++++++++++++++++++++++++++++++++++- 4 files changed, 192 insertions(+), 27 deletions(-) (limited to 'src') 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 #include #include +#include + 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 #include #include +#include #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 #include #include //sleep if I ever use it. - +#include +#include 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; +} + + + + + + -- cgit v1.2.3