From 55cedfc6a6b035d6eb54457782818fef61cae500 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 25 Feb 2017 23:52:48 -0500 Subject: Huge shift: Put HTC vive into its own file, to free up the rest of the system for libsurvive. --- Makefile | 4 +- calibrate.c | 13 +- data_recorder.c | 4 +- include/survive.h | 6 +- src/survive.c | 249 ++++------- src/survive_cal.c | 8 + src/survive_cal.h | 2 + src/survive_cal_lhfind.c | 5 +- src/survive_data.c | 358 +-------------- src/survive_driverman.c | 50 +++ src/survive_driverman.h | 22 + src/survive_internal.h | 65 +-- src/survive_usb.c | 433 +------------------ src/survive_vive.c | 1075 ++++++++++++++++++++++++++++++++++++++++++++++ test.c | 2 +- 15 files changed, 1275 insertions(+), 1021 deletions(-) create mode 100644 src/survive_driverman.c create mode 100644 src/survive_driverman.h create mode 100644 src/survive_vive.c 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 #include "survive_internal.h" +#include "survive_driverman.h" #include #include -#include #include #include - -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 -// 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 #include -#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 +#include + +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 #include #include -#include +#include "survive_driverman.h" #include #include @@ -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 -#include -#include //sleep if I ever use it. -#include -#include - -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 +// 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 + +#include "survive_internal.h" +#include +#include +#include //sleep if I ever use it. +#include +#include + +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. -- cgit v1.2.3