diff options
30 files changed, 775 insertions, 95 deletions
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp index fa3c5f8f..7764253b 100644 --- a/contexthubhal/nanohubhal.cpp +++ b/contexthubhal/nanohubhal.cpp @@ -139,8 +139,11 @@ static void wait_on_dev_lock(pollfd &pfd) { discard_inotify_evt(pfd); while (access(NANOHUB_LOCK_FILE, F_OK) == 0) { ALOGW("Nanohub is locked; blocking read thread"); - int ret = poll(&pfd, 1, 5000); - if (ret > 0) { + int ret = TEMP_FAILURE_RETRY(poll(&pfd, 1, 5000)); + if (ret < 0) { + ALOGE("poll returned with an error: %s", strerror(errno)); + break; + } else if (ret > 0) { discard_inotify_evt(pfd); } } @@ -226,10 +229,12 @@ void* NanoHub::runDeviceRx() setDebugFlags(property_get_int32("persist.nanohub.debug", 0)); while (1) { - int ret = poll(myFds, numPollFds, -1); - if (ret <= 0) { - ALOGD("poll returned with an error: %s", strerror(errno)); + int ret = TEMP_FAILURE_RETRY(poll(myFds, numPollFds, -1)); + if (ret == 0) continue; + else if (ret < 0) { + ALOGE("poll returned with an error: %s", strerror(errno)); + break; } if (hasInotify) { diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c index aae23cfc..55bbdf2f 100644 --- a/firmware/app/chre/common/chre_app.c +++ b/firmware/app/chre/common/chre_app.c @@ -176,7 +176,7 @@ static void chreappProcessSensorData(uint16_t evt, const void *eventData) return; si = eOsSensorFind(SENSOR_TYPE(evt), 0, &sensorHandle); - if (si) { + if (si && eOsSensorGetReqRate(sensorHandle)) { switch (si->numAxis) { case NUM_AXIS_EMBEDDED: processEmbeddedData(eventData, sensorHandle, SENSOR_TYPE(evt)); diff --git a/firmware/lib/builtins/Android.mk b/firmware/lib/builtins/Android.mk index aa8663c6..b43bdb2b 100644 --- a/firmware/lib/builtins/Android.mk +++ b/firmware/lib/builtins/Android.mk @@ -31,6 +31,10 @@ LOCAL_SRC_FILES_cortexm4 := \ umoddi3.c \ aeabi_f2d.c \ aeabi_llsl.c \ + aeabi_llsr.c \ + aeabi_ul2f.c \ + aeabi_l2f.c \ + aeabi_f2ulz.c \ LOCAL_C_INCLUDES = $(LOCAL_PATH) LOCAL_EXPORT_C_INCLUDE_DIRS := \ diff --git a/firmware/lib/builtins/aeabi_f2ulz.c b/firmware/lib/builtins/aeabi_f2ulz.c new file mode 100644 index 00000000..a6413a75 --- /dev/null +++ b/firmware/lib/builtins/aeabi_f2ulz.c @@ -0,0 +1,27 @@ +/* ===-- fixunssfdi.c - Implement __fixunssfdi -----------------------------=== + * + * The LLVM Compiler Infrastructure + * + * This file is dual licensed under the MIT and the University of Illinois Open + * Source Licenses. See LICENSE.TXT for details. + * + * ===----------------------------------------------------------------------=== + */ + +#include "int_lib.h" + +du_int __aeabi_f2ulz(float a); + +/* Support for systems that have hardware floating-point; can set the invalid + * flag as a side-effect of computation. + */ + +du_int +__aeabi_f2ulz(float a) +{ + if (a <= 0.0f) return 0; + float da = a; + su_int high = da / 4294967296.f; /* da / 0x1p32f; */ + su_int low = da - (float)high * 4294967296.f; /* high * 0x1p32f; */ + return ((du_int)high << 32) | low; +} diff --git a/firmware/lib/builtins/aeabi_l2f.c b/firmware/lib/builtins/aeabi_l2f.c new file mode 100644 index 00000000..63d832f0 --- /dev/null +++ b/firmware/lib/builtins/aeabi_l2f.c @@ -0,0 +1,80 @@ +/*===-- floatdisf.c - Implement __floatdisf -------------------------------=== + * + * The LLVM Compiler Infrastructure + * + * This file is dual licensed under the MIT and the University of Illinois Open + * Source Licenses. See LICENSE.TXT for details. + * + *===----------------------------------------------------------------------=== + * + * This file implements __floatdisf for the compiler_rt library. + * + *===----------------------------------------------------------------------=== + */ + +/* Returns: convert a to a float, rounding toward even.*/ + +/* Assumption: float is a IEEE 32 bit floating point type + * di_int is a 64 bit integral type + */ + +/* seee eeee emmm mmmm mmmm mmmm mmmm mmmm */ + +#include "int_lib.h" + +float __aeabi_l2f(di_int a); + +float +__aeabi_l2f(di_int a) +{ + if (a == 0) + return 0.0F; + const unsigned N = sizeof(di_int) * CHAR_BIT; + const di_int s = a >> (N-1); + a = (a ^ s) - s; + int sd = N - __builtin_clzll(a); /* number of significant digits */ + int e = sd - 1; /* exponent */ + if (sd > FLT_MANT_DIG) + { + /* start: 0000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQxxxxxxxxxxxxxxxxxx + * finish: 000000000000000000000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQR + * 12345678901234567890123456 + * 1 = msb 1 bit + * P = bit FLT_MANT_DIG-1 bits to the right of 1 + * Q = bit FLT_MANT_DIG bits to the right of 1 + * R = "or" of all bits to the right of Q + */ + switch (sd) + { + case FLT_MANT_DIG + 1: + a <<= 1; + break; + case FLT_MANT_DIG + 2: + break; + default: + a = ((du_int)a >> (sd - (FLT_MANT_DIG+2))) | + ((a & ((du_int)(-1) >> ((N + FLT_MANT_DIG+2) - sd))) != 0); + }; + /* finish: */ + a |= (a & 4) != 0; /* Or P into R */ + ++a; /* round - this step may add a significant bit */ + a >>= 2; /* dump Q and R */ + /* a is now rounded to FLT_MANT_DIG or FLT_MANT_DIG+1 bits */ + if (a & ((du_int)1 << FLT_MANT_DIG)) + { + a >>= 1; + ++e; + } + /* a is now rounded to FLT_MANT_DIG bits */ + } + else + { + a <<= (FLT_MANT_DIG - sd); + /* a is now rounded to FLT_MANT_DIG bits */ + } + float_bits fb; + fb.u = ((su_int)s & 0x80000000) | /* sign */ + ((e + 127) << 23) | /* exponent */ + ((su_int)a & 0x007FFFFF); /* mantissa */ + return fb.f; +} diff --git a/firmware/lib/builtins/aeabi_llsr.c b/firmware/lib/builtins/aeabi_llsr.c new file mode 100644 index 00000000..2f0ba6b9 --- /dev/null +++ b/firmware/lib/builtins/aeabi_llsr.c @@ -0,0 +1,43 @@ +/* ===-- lshrdi3.c - Implement __lshrdi3 -----------------------------------=== + * + * The LLVM Compiler Infrastructure + * + * This file is dual licensed under the MIT and the University of Illinois Open + * Source Licenses. See LICENSE.TXT for details. + * + * ===----------------------------------------------------------------------=== + * + * This file implements __lshrdi3 for the compiler_rt library. + * + * ===----------------------------------------------------------------------=== + */ + +#include "int_lib.h" + +/* Returns: logical a >> b */ + +/* Precondition: 0 <= b < bits_in_dword */ + +di_int __aeabi_llsr(di_int a, si_int b); + +di_int +__aeabi_llsr(di_int a, si_int b) +{ + const int bits_in_word = (int)(sizeof(si_int) * CHAR_BIT); + udwords input; + udwords result; + input.all = a; + if (b & bits_in_word) /* bits_in_word <= b < bits_in_dword */ + { + result.s.high = 0; + result.s.low = input.s.high >> (b - bits_in_word); + } + else /* 0 <= b < bits_in_word */ + { + if (b == 0) + return a; + result.s.high = input.s.high >> b; + result.s.low = (input.s.high << (bits_in_word - b)) | (input.s.low >> b); + } + return result.all; +} diff --git a/firmware/lib/builtins/aeabi_ul2f.c b/firmware/lib/builtins/aeabi_ul2f.c new file mode 100644 index 00000000..80734db5 --- /dev/null +++ b/firmware/lib/builtins/aeabi_ul2f.c @@ -0,0 +1,77 @@ +/*===-- floatundisf.c - Implement __floatundisf ---------------------------=== + * + * The LLVM Compiler Infrastructure + * + * This file is dual licensed under the MIT and the University of Illinois Open + * Source Licenses. See LICENSE.TXT for details. + * + * ===----------------------------------------------------------------------=== + * + * This file implements __floatundisf for the compiler_rt library. + * + *===----------------------------------------------------------------------=== + */ + +/* Returns: convert a to a float, rounding toward even. */ + +/* Assumption: float is a IEEE 32 bit floating point type + * du_int is a 64 bit integral type + */ + +/* seee eeee emmm mmmm mmmm mmmm mmmm mmmm */ + +#include "int_lib.h" + +float __aeabi_ul2f(du_int a); + +float +__aeabi_ul2f(du_int a) +{ + if (a == 0) + return 0.0F; + const unsigned N = sizeof(du_int) * CHAR_BIT; + int sd = N - __builtin_clzll(a); /* number of significant digits */ + int e = sd - 1; /* 8 exponent */ + if (sd > FLT_MANT_DIG) + { + /* start: 0000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQxxxxxxxxxxxxxxxxxx + * finish: 000000000000000000000000000000000000001xxxxxxxxxxxxxxxxxxxxxxPQR + * 12345678901234567890123456 + * 1 = msb 1 bit + * P = bit FLT_MANT_DIG-1 bits to the right of 1 + * Q = bit FLT_MANT_DIG bits to the right of 1 + * R = "or" of all bits to the right of Q + */ + switch (sd) + { + case FLT_MANT_DIG + 1: + a <<= 1; + break; + case FLT_MANT_DIG + 2: + break; + default: + a = (a >> (sd - (FLT_MANT_DIG+2))) | + ((a & ((du_int)(-1) >> ((N + FLT_MANT_DIG+2) - sd))) != 0); + }; + /* finish: */ + a |= (a & 4) != 0; /* Or P into R */ + ++a; /* round - this step may add a significant bit */ + a >>= 2; /* dump Q and R */ + /* a is now rounded to FLT_MANT_DIG or FLT_MANT_DIG+1 bits */ + if (a & ((du_int)1 << FLT_MANT_DIG)) + { + a >>= 1; + ++e; + } + /* a is now rounded to FLT_MANT_DIG bits */ + } + else + { + a <<= (FLT_MANT_DIG - sd); + /* a is now rounded to FLT_MANT_DIG bits */ + } + float_bits fb; + fb.u = ((e + 127) << 23) | /* exponent */ + ((su_int)a & 0x007FFFFF); /* mantissa */ + return fb.f; +} diff --git a/firmware/lib/builtins/int_lib.h b/firmware/lib/builtins/int_lib.h index 3d968a8e..fddef7f1 100644 --- a/firmware/lib/builtins/int_lib.h +++ b/firmware/lib/builtins/int_lib.h @@ -16,7 +16,8 @@ #ifndef INT_LIB_H #define INT_LIB_H -#define CHAR_BIT 8 +#define FLT_MANT_DIG __FLT_MANT_DIG__ +#define CHAR_BIT 8 typedef unsigned su_int; typedef int si_int; @@ -44,6 +45,12 @@ typedef union } s; } udwords; +typedef union +{ + su_int u; + float f; +} float_bits; + /* Assumption: Signed integral is 2's complement. */ /* Assumption: Right shift of signed negative is arithmetic shift. */ diff --git a/firmware/lib/lib.mk b/firmware/lib/lib.mk index 41d84ee6..b26fc9f1 100644 --- a/firmware/lib/lib.mk +++ b/firmware/lib/lib.mk @@ -84,4 +84,8 @@ SRCS += $(BUILTINS_PATH)/udivmoddi4.c SRCS += $(BUILTINS_PATH)/umoddi3.c SRCS += $(BUILTINS_PATH)/aeabi_f2d.c SRCS += $(BUILTINS_PATH)/aeabi_llsl.c +SRCS += $(BUILTINS_PATH)/aeabi_llsr.c +SRCS += $(BUILTINS_PATH)/aeabi_ul2f.c +SRCS += $(BUILTINS_PATH)/aeabi_l2f.c +SRCS += $(BUILTINS_PATH)/aeabi_f2ulz.c CFLAGS += -I$(BUILTINS_PATH) diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c index 6dd32d64..b8e68251 100644 --- a/firmware/os/core/nanohub_chre.c +++ b/firmware/os/core/nanohub_chre.c @@ -102,7 +102,7 @@ static void osChreApiGetTime(uintptr_t *retValP, va_list args) { uint64_t *timeNanos = va_arg(args, uint64_t *); if (timeNanos) - *timeNanos = timGetTime(); + *timeNanos = sensorGetTime(); } static inline uint32_t osChreTimerSet(uint64_t duration, const void* cookie, bool oneShot) diff --git a/firmware/os/core/osApi.c b/firmware/os/core/osApi.c index 1716f09b..8361d9ae 100644 --- a/firmware/os/core/osApi.c +++ b/firmware/os/core/osApi.c @@ -178,7 +178,7 @@ static void osExpApiSensorTrigger(uintptr_t *retValP, va_list args) *retValP = sensorTriggerOndemand(0, sensorHandle); } -static void osExpApiSensorGetRate(uintptr_t *retValP, va_list args) +static void osExpApiSensorGetCurRate(uintptr_t *retValP, va_list args) { uint32_t sensorHandle = va_arg(args, uint32_t); @@ -191,6 +191,13 @@ static void osExpApiSensorGetTime(uintptr_t *retValP, va_list args) *timeNanos = sensorGetTime(); } +static void osExpApiSensorGetReqRate(uintptr_t *retValP, va_list args) +{ + uint32_t sensorHandle = va_arg(args, uint32_t); + + *retValP = sensorGetReqRate(sensorHandle); +} + static void osExpApiTimGetTime(uintptr_t *retValP, va_list args) { uint64_t *timeNanos = va_arg(args, uint64_t *); @@ -482,9 +489,9 @@ void osApiExport(struct SlabAllocator *mainSlubAllocator) static const struct SyscallTable osMainEvtqTable = { .numEntries = SYSCALL_OS_MAIN_EVTQ_LAST, .entry = { - [SYSCALL_OS_MAIN_EVTQ_SUBCRIBE] = { .func = osExpApiEvtqSubscribe, }, - [SYSCALL_OS_MAIN_EVTQ_UNSUBCRIBE] = { .func = osExpApiEvtqUnsubscribe, }, - [SYSCALL_OS_MAIN_EVTQ_ENQUEUE] = { .func = osExpApiEvtqEnqueue, }, + [SYSCALL_OS_MAIN_EVTQ_SUBCRIBE] = { .func = osExpApiEvtqSubscribe, }, + [SYSCALL_OS_MAIN_EVTQ_UNSUBCRIBE] = { .func = osExpApiEvtqUnsubscribe, }, + [SYSCALL_OS_MAIN_EVTQ_ENQUEUE] = { .func = osExpApiEvtqEnqueue, }, [SYSCALL_OS_MAIN_EVTQ_ENQUEUE_PRIVATE] = { .func = osExpApiEvtqEnqueuePrivate, }, [SYSCALL_OS_MAIN_EVTQ_RETAIN_EVT] = { .func = osExpApiEvtqRetainEvt, }, [SYSCALL_OS_MAIN_EVTQ_FREE_RETAINED] = { .func = osExpApiEvtqFreeRetained, }, @@ -501,17 +508,18 @@ void osApiExport(struct SlabAllocator *mainSlubAllocator) static const struct SyscallTable osMainSensorsTable = { .numEntries = SYSCALL_OS_MAIN_SENSOR_LAST, .entry = { - [SYSCALL_OS_MAIN_SENSOR_SIGNAL] = { .func = osExpApiSensorSignal, }, - [SYSCALL_OS_MAIN_SENSOR_REG] = { .func = osExpApiSensorReg, }, - [SYSCALL_OS_MAIN_SENSOR_UNREG] = { .func = osExpApiSensorUnreg, }, + [SYSCALL_OS_MAIN_SENSOR_SIGNAL] = { .func = osExpApiSensorSignal, }, + [SYSCALL_OS_MAIN_SENSOR_REG] = { .func = osExpApiSensorReg, }, + [SYSCALL_OS_MAIN_SENSOR_UNREG] = { .func = osExpApiSensorUnreg, }, [SYSCALL_OS_MAIN_SENSOR_REG_INIT_COMP] = { .func = osExpApiSensorRegInitComp }, - [SYSCALL_OS_MAIN_SENSOR_FIND] = { .func = osExpApiSensorFind, }, - [SYSCALL_OS_MAIN_SENSOR_REQUEST] = { .func = osExpApiSensorReq, }, - [SYSCALL_OS_MAIN_SENSOR_RATE_CHG] = { .func = osExpApiSensorRateChg, }, - [SYSCALL_OS_MAIN_SENSOR_RELEASE] = { .func = osExpApiSensorRel, }, - [SYSCALL_OS_MAIN_SENSOR_TRIGGER] = { .func = osExpApiSensorTrigger, }, - [SYSCALL_OS_MAIN_SENSOR_GET_RATE] = { .func = osExpApiSensorGetRate, }, - [SYSCALL_OS_MAIN_SENSOR_GET_TIME] = { .func = osExpApiSensorGetTime, }, + [SYSCALL_OS_MAIN_SENSOR_FIND] = { .func = osExpApiSensorFind, }, + [SYSCALL_OS_MAIN_SENSOR_REQUEST] = { .func = osExpApiSensorReq, }, + [SYSCALL_OS_MAIN_SENSOR_RATE_CHG] = { .func = osExpApiSensorRateChg, }, + [SYSCALL_OS_MAIN_SENSOR_RELEASE] = { .func = osExpApiSensorRel, }, + [SYSCALL_OS_MAIN_SENSOR_TRIGGER] = { .func = osExpApiSensorTrigger, }, + [SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE] = { .func = osExpApiSensorGetCurRate, }, + [SYSCALL_OS_MAIN_SENSOR_GET_TIME] = { .func = osExpApiSensorGetTime, }, + [SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE] = { .func = osExpApiSensorGetReqRate, }, }, }; @@ -519,9 +527,9 @@ void osApiExport(struct SlabAllocator *mainSlubAllocator) static const struct SyscallTable osMainTimerTable = { .numEntries = SYSCALL_OS_MAIN_TIME_LAST, .entry = { - [SYSCALL_OS_MAIN_TIME_GET_TIME] = { .func = osExpApiTimGetTime, }, - [SYSCALL_OS_MAIN_TIME_SET_TIMER] = { .func = osExpApiTimSetTimer, }, - [SYSCALL_OS_MAIN_TIME_CANCEL_TIMER] = { .func = osExpApiTimCancelTimer, }, + [SYSCALL_OS_MAIN_TIME_GET_TIME] = { .func = osExpApiTimGetTime, }, + [SYSCALL_OS_MAIN_TIME_SET_TIMER] = { .func = osExpApiTimSetTimer, }, + [SYSCALL_OS_MAIN_TIME_CANCEL_TIMER] = { .func = osExpApiTimCancelTimer, }, }, }; @@ -529,17 +537,17 @@ void osApiExport(struct SlabAllocator *mainSlubAllocator) .numEntries = SYSCALL_OS_MAIN_HEAP_LAST, .entry = { [SYSCALL_OS_MAIN_HEAP_ALLOC] = { .func = osExpApiHeapAlloc }, - [SYSCALL_OS_MAIN_HEAP_FREE] = { .func = osExpApiHeapFree }, + [SYSCALL_OS_MAIN_HEAP_FREE] = { .func = osExpApiHeapFree }, }, }; static const struct SyscallTable osMainSlabTable = { .numEntries = SYSCALL_OS_MAIN_SLAB_LAST, .entry = { - [SYSCALL_OS_MAIN_SLAB_NEW] = { .func = osExpApiSlabNew }, + [SYSCALL_OS_MAIN_SLAB_NEW] = { .func = osExpApiSlabNew }, [SYSCALL_OS_MAIN_SLAB_DESTROY] = { .func = osExpApiSlabDestroy }, - [SYSCALL_OS_MAIN_SLAB_ALLOC] = { .func = osExpApiSlabAlloc }, - [SYSCALL_OS_MAIN_SLAB_FREE] = { .func = osExpApiSlabFree }, + [SYSCALL_OS_MAIN_SLAB_ALLOC] = { .func = osExpApiSlabAlloc }, + [SYSCALL_OS_MAIN_SLAB_FREE] = { .func = osExpApiSlabFree }, }, }; diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c index f27a9e94..61c3d23a 100644 --- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c +++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c @@ -75,6 +75,10 @@ #include <stdlib.h> #include <string.h> +#define VERBOSE_PRINT(fmt, ...) do { \ + osLog(LOG_VERBOSE, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \ + } while (0); + #define INFO_PRINT(fmt, ...) do { \ osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \ } while (0); @@ -85,13 +89,13 @@ #define DEBUG_PRINT(fmt, ...) do { \ if (DBG_ENABLE) { \ - INFO_PRINT(fmt, ##__VA_ARGS__); \ + osLog(LOG_DEBUG, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \ } \ } while (0); #define DEBUG_PRINT_IF(cond, fmt, ...) do { \ if ((cond) && DBG_ENABLE) { \ - INFO_PRINT(fmt, ##__VA_ARGS__); \ + osLog(LOG_DEBUG, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \ } \ } while (0); @@ -1313,7 +1317,7 @@ static bool accPower(bool on, void *cookie) { TDECL(); - INFO_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); + VERBOSE_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { if (on) { // set ACC power mode to NORMAL @@ -1336,7 +1340,7 @@ static bool accPower(bool on, void *cookie) static bool gyrPower(bool on, void *cookie) { TDECL(); - INFO_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); + VERBOSE_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { if (on) { @@ -1369,7 +1373,7 @@ static bool gyrPower(bool on, void *cookie) static bool magPower(bool on, void *cookie) { TDECL(); - INFO_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); + VERBOSE_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE())); if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { if (on) { // set MAG power mode to NORMAL @@ -1665,7 +1669,7 @@ static bool gyrSetRate(uint32_t rate, uint64_t latency, void *cookie) TDECL(); int odr, osr = 0; int osr_mode = 2; // normal - INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", + VERBOSE_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency, getStateName(GET_STATE())); if (trySwitchState(SENSOR_CONFIG_CHANGING)) { @@ -1732,7 +1736,7 @@ static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie) if (rate == SENSOR_RATE_ONCHANGE) rate = SENSOR_HZ(100); - INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", + VERBOSE_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency, getStateName(GET_STATE())); if (trySwitchState(SENSOR_CONFIG_CHANGING)) { @@ -3922,7 +3926,7 @@ static bool startTask(uint32_t task_id) // XXX: this consumes too much memeory, need to optimize T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20); if (!T(mDataSlab)) { - INFO_PRINT("slabAllocatorNew() failed\n"); + ERROR_PRINT("slabAllocatorNew() failed\n"); return false; } T(mWbufCnt) = 0; diff --git a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c index 8f2f16a3..3fb6ee89 100644 --- a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c +++ b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c @@ -143,6 +143,10 @@ enum { osLog(LOG_INFO, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \ } while (0); +#define ERROR_PRINT(fmt, ...) do { \ + osLog(LOG_ERROR, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \ + } while (0); + #define DEBUG_PRINT(fmt, ...) do { \ if (enable_debug) { \ osLog(LOG_INFO, "[Rohm RPR-0521] " fmt, ##__VA_ARGS__); \ @@ -298,7 +302,7 @@ static void i2cCallback(void *cookie, size_t tx, size_t rx, int err) osEnqueuePrivateEvt(EVT_SENSOR_I2C, cookie, NULL, mTask.tid); if (err != 0) - INFO_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); + ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); } static void alsTimerCallback(uint32_t timerId, void *cookie) @@ -385,7 +389,7 @@ static void setMode(bool alsOn, bool proxOn, uint8_t state) static bool sensorPowerAls(bool on, void *cookie) { - DEBUG_PRINT("sensorPowerAls: %d\n", on); + VERBOSE_PRINT("sensorPowerAls: %d\n", on); if (on && !mTask.alsTimerHandle) { mTask.alsTimerHandle = timTimerSet(ROHM_RPR0521_ALS_TIMER_DELAY, 0, 50, alsTimerCallback, NULL, false); @@ -411,7 +415,7 @@ static bool sensorRateAls(uint32_t rate, uint64_t latency, void *cookie) if (rate == SENSOR_RATE_ONCHANGE) rate = ROHM_RPR0521_DEFAULT_RATE; - DEBUG_PRINT("sensorRateAls: rate=%ld Hz latency=%lld ns\n", rate/1024, latency); + VERBOSE_PRINT("sensorRateAls: rate=%ld Hz latency=%lld ns\n", rate/1024, latency); return sensorSignalInternalEvt(mTask.alsHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); } @@ -434,7 +438,7 @@ static bool sendLastSampleAls(void *cookie, uint32_t tid) { static bool sensorPowerProx(bool on, void *cookie) { - DEBUG_PRINT("sensorPowerProx: %d\n", on); + VERBOSE_PRINT("sensorPowerProx: %d\n", on); if (on) { extiClearPendingGpio(mTask.pin); @@ -461,7 +465,7 @@ static bool sensorRateProx(uint32_t rate, uint64_t latency, void *cookie) if (rate == SENSOR_RATE_ONCHANGE) rate = ROHM_RPR0521_DEFAULT_RATE; - DEBUG_PRINT("sensorRateProx: rate=%ld Hz latency=%lld ns\n", rate/1024, latency); + VERBOSE_PRINT("sensorRateProx: rate=%ld Hz latency=%lld ns\n", rate/1024, latency); return sensorSignalInternalEvt(mTask.proxHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); } @@ -475,8 +479,6 @@ static bool sensorCfgDataProx(void *data, void *cookie) { struct I2cTransfer *xfer; - DEBUG_PRINT("sensorCfgDataProx"); - int32_t offset = *(int32_t*)data; INFO_PRINT("Received cfg data: %d\n", (int)offset); diff --git a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c index df1afcef..62f364a4 100644 --- a/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c +++ b/firmware/os/drivers/synaptics_s3708/synaptics_s3708.c @@ -76,10 +76,11 @@ #define ENABLE_DEBUG 0 +#define VERBOSE_PRINT(fmt, ...) osLog(LOG_VERBOSE, "[DoubleTouch] " fmt, ##__VA_ARGS__) #define INFO_PRINT(fmt, ...) osLog(LOG_INFO, "[DoubleTouch] " fmt, ##__VA_ARGS__) #define ERROR_PRINT(fmt, ...) osLog(LOG_ERROR, "[DoubleTouch] " fmt, ##__VA_ARGS__) #if ENABLE_DEBUG -#define DEBUG_PRINT(fmt, ...) INFO_PRINT(fmt, ##__VA_ARGS__) +#define DEBUG_PRINT(fmt, ...) osLog(LOG_DEBUG, "[DoubleTouch] " fmt, ##__VA_ARGS__) #else #define DEBUG_PRINT(fmt, ...) ((void)0) #endif @@ -303,7 +304,7 @@ static void setGesturePower(bool enable, bool skipI2c) bool ret; size_t i; - INFO_PRINT("gesture: %d", enable); + VERBOSE_PRINT("gesture: %d", enable); // Cancel any pending I2C transactions by changing the callback state for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) { @@ -360,7 +361,7 @@ static bool callbackPower(bool on, void *cookie) { uint32_t enabledSeconds, proxEnabledSeconds, proxFarSeconds; - INFO_PRINT("power: %d", on); + VERBOSE_PRINT("power: %d", on); if (on) { mTask.stats.enabledTimestamp = sensorGetTime(); @@ -371,7 +372,7 @@ static bool callbackPower(bool on, void *cookie) enabledSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalEnabledTime, 1000000000); proxEnabledSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalProxEnabledTime, 1000000000); proxFarSeconds = U64_DIV_BY_U64_CONSTANT(mTask.stats.totalProxFarTime, 1000000000); - INFO_PRINT("STATS: enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 + VERBOSE_PRINT("STATS: enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 ", prox enabled %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 ", prox far %02" PRIu32 ":%02" PRIu32 ":%02" PRIu32 ", prox *->f %" PRIu32 diff --git a/firmware/os/drivers/vsync/vsync.c b/firmware/os/drivers/vsync/vsync.c index 68147b06..9d76759a 100644 --- a/firmware/os/drivers/vsync/vsync.c +++ b/firmware/os/drivers/vsync/vsync.c @@ -49,6 +49,10 @@ #error "VSYNC_IRQ is not defined; please define in variant.h" #endif +#define VERBOSE_PRINT(fmt, ...) do { \ + osLog(LOG_VERBOSE, "%s " fmt, "[VSYNC]", ##__VA_ARGS__); \ + } while (0); + #define INFO_PRINT(fmt, ...) do { \ osLog(LOG_INFO, "%s " fmt, "[VSYNC]", ##__VA_ARGS__); \ } while (0); @@ -149,7 +153,7 @@ static const struct SensorInfo mSensorInfo = static bool vsyncPower(bool on, void *cookie) { - INFO_PRINT("power %d\n", on); + VERBOSE_PRINT("power %d\n", on); if (on) { extiClearPendingGpio(mTask.pin); @@ -171,13 +175,13 @@ static bool vsyncFirmwareUpload(void *cookie) static bool vsyncSetRate(uint32_t rate, uint64_t latency, void *cookie) { - INFO_PRINT("setRate\n"); + VERBOSE_PRINT("setRate\n"); return sensorSignalInternalEvt(mTask.sensorHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); } static bool vsyncFlush(void *cookie) { - INFO_PRINT("flush\n"); + VERBOSE_PRINT("flush\n"); return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_VSYNC), SENSOR_DATA_EVENT_FLUSH, NULL); } diff --git a/firmware/os/drivers/window_orientation/window_orientation.c b/firmware/os/drivers/window_orientation/window_orientation.c index 067649a0..c81d8a61 100644 --- a/firmware/os/drivers/window_orientation/window_orientation.c +++ b/firmware/os/drivers/window_orientation/window_orientation.c @@ -34,6 +34,10 @@ #define LOG_TAG "[WO]" +#define LOGV(fmt, ...) do { \ + osLog(LOG_VERBOSE, LOG_TAG " " fmt, ##__VA_ARGS__); \ + } while (0); + #define LOGW(fmt, ...) do { \ osLog(LOG_WARN, LOG_TAG " " fmt, ##__VA_ARGS__); \ } while (0); @@ -618,7 +622,7 @@ static void windowOrientationHandleEvent(uint32_t evtType, const void* evtData) rotation_changed = add_samples(ev); if (rotation_changed) { - LOGI("rotation changed to: ******* %d *******\n", + LOGV("rotation changed to: ******* %d *******\n", (int)mTask.proposed_rotation); // send a single int32 here so no memory alloc/free needed. diff --git a/firmware/os/inc/osApi.h b/firmware/os/inc/osApi.h index 287281c7..77cff48f 100644 --- a/firmware/os/inc/osApi.h +++ b/firmware/os/inc/osApi.h @@ -93,9 +93,10 @@ #define SYSCALL_OS_MAIN_SENSOR_RATE_CHG 6 // (uint32_t clientId, uint32_t sensorHandle, uint32_t newRate) -> bool success #define SYSCALL_OS_MAIN_SENSOR_RELEASE 7 // (uint32_t clientId, uint32_t sensorHandle) -> bool success #define SYSCALL_OS_MAIN_SENSOR_TRIGGER 8 // (uint32_t clientId, uint32_t sensorHandle) -> bool success -#define SYSCALL_OS_MAIN_SENSOR_GET_RATE 9 // (uint32_t sensorHandle) -> uint32_t rate +#define SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE 9 // (uint32_t sensorHandle) -> uint32_t curRate #define SYSCALL_OS_MAIN_SENSOR_GET_TIME 10 // (uint64_t *timeNanos) -> void -#define SYSCALL_OS_MAIN_SENSOR_LAST 11 // always last. holes are allowed, but not immediately before this +#define SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE 11 // (uint32_t sensorHandle) -> uint32_t reqRate +#define SYSCALL_OS_MAIN_SENSOR_LAST 12 // always last. holes are allowed, but not immediately before this //level 3 indices in the OS.main.timer table #define SYSCALL_OS_MAIN_TIME_GET_TIME 0 // (uint64_t *timeNanos) -> void diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h index 7b178899..7979d762 100644 --- a/firmware/os/inc/seos.h +++ b/firmware/os/inc/seos.h @@ -270,10 +270,11 @@ bool osAppIsChre(uint16_t tid); /* Logging */ enum LogLevel { - LOG_ERROR = 'E', - LOG_WARN = 'W', - LOG_INFO = 'I', - LOG_DEBUG = 'D', + LOG_ERROR = 'E', + LOG_WARN = 'W', + LOG_INFO = 'I', + LOG_DEBUG = 'D', + LOG_VERBOSE = 'V', }; void osLogv(char clevel, uint32_t flags, const char *str, va_list vl); diff --git a/firmware/os/inc/syscallDo.h b/firmware/os/inc/syscallDo.h index 68bccee0..4903a085 100644 --- a/firmware/os/inc/syscallDo.h +++ b/firmware/os/inc/syscallDo.h @@ -152,7 +152,7 @@ static inline bool eOsSensorTriggerOndemand(uint32_t clientId, uint32_t sensorHa static inline uint32_t eOsSensorGetCurRate(uint32_t sensorHandle) { - return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_RATE), sensorHandle); + return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_CUR_RATE), sensorHandle); } static inline uint64_t eOsSensorGetTime(void) @@ -162,6 +162,11 @@ static inline uint64_t eOsSensorGetTime(void) return timeNanos; } +static inline uint32_t eOsSensorGetReqRate(uint32_t sensorHandle) +{ + return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_OS, SYSCALL_OS_MAIN, SYSCALL_OS_MAIN_SENSOR, SYSCALL_OS_MAIN_SENSOR_GET_REQ_RATE), sensorHandle); +} + static inline uint64_t eOsTimGetTime(void) { uint64_t timeNanos; diff --git a/firmware/os/platform/stm32/platform.c b/firmware/os/platform/stm32/platform.c index befafe9f..313f1bdb 100644 --- a/firmware/os/platform/stm32/platform.c +++ b/firmware/os/platform/stm32/platform.c @@ -110,7 +110,7 @@ static struct usart mDbgUart; #ifdef DEBUG_LOG_EVT #ifndef EARLY_LOG_BUF_SIZE -#define EARLY_LOG_BUF_SIZE 1024 +#define EARLY_LOG_BUF_SIZE 2048 #endif #define HOSTINTF_HEADER_SIZE 4 uint8_t *mEarlyLogBuffer; diff --git a/lefty/Android.mk b/lefty/Android.mk new file mode 100644 index 00000000..5e8edf75 --- /dev/null +++ b/lefty/Android.mk @@ -0,0 +1,95 @@ +# Copyright (C) 2017 The Android Open Source Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +LOCAL_PATH := $(call my-dir) + +ifeq ($(TARGET_USES_NANOHUB_SENSORHAL), true) + +COMMON_CFLAGS := -Wall -Werror -Wextra + +################################################################################ +ifeq ($(NANOHUB_SENSORHAL_LEFTY_IMPL_ENABLED), true) + +include $(CLEAR_VARS) + +LOCAL_MODULE := vendor.google_clockwork.lefty@1.0-impl.nanohub + +LOCAL_MODULE_RELATIVE_PATH := hw +LOCAL_MODULE_TAGS := optional +LOCAL_MODULE_OWNER := google +LOCAL_PROPRIETARY_MODULE := true + +LOCAL_CFLAGS += $(COMMON_CFLAGS) + +LOCAL_C_INCLUDES += \ + device/google/contexthub/firmware/os/inc \ + device/google/contexthub/sensorhal \ + device/google/contexthub/util/common \ + +LOCAL_SRC_FILES := \ + Lefty.cpp \ + +LOCAL_SHARED_LIBRARIES := \ + vendor.google_clockwork.lefty@1.0 \ + libbase \ + libcutils \ + libhidlbase \ + libhidltransport \ + libhubconnection \ + liblog \ + libutils \ + +include $(BUILD_SHARED_LIBRARY) + +endif +################################################################################ +ifeq ($(NANOHUB_SENSORHAL_LEFTY_SERVICE_ENABLED), true) + +include $(CLEAR_VARS) + +LOCAL_MODULE := liblefty_service_nanohub + +LOCAL_MODULE_TAGS := optional +LOCAL_MODULE_OWNER := google +LOCAL_PROPRIETARY_MODULE := true + +LOCAL_CFLAGS += $(COMMON_CFLAGS) + +LOCAL_C_INCLUDES += \ + device/google/contexthub/firmware/os/inc \ + device/google/contexthub/sensorhal \ + device/google/contexthub/util/common \ + +LOCAL_EXPORT_C_INCLUDE_DIRS := $(LOCAL_PATH) + +LOCAL_SRC_FILES := \ + Lefty.cpp \ + lefty_service.cpp \ + +LOCAL_SHARED_LIBRARIES := \ + vendor.google_clockwork.lefty@1.0 \ + libbase \ + libcutils \ + libhidlbase \ + libhidltransport \ + libhubconnection \ + liblog \ + libutils \ + +include $(BUILD_SHARED_LIBRARY) + +endif +################################################################################ + +endif diff --git a/lefty/Lefty.cpp b/lefty/Lefty.cpp new file mode 100644 index 00000000..b04ac4f9 --- /dev/null +++ b/lefty/Lefty.cpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#define LOG_TAG "Lefty" +#include <utils/Log.h> + +#include "Lefty.h" + +namespace vendor { +namespace google_clockwork { +namespace lefty { +namespace V1_0 { +namespace implementation { + +Lefty::Lefty() : mHubConnection(HubConnection::getInstance()) { + ALOGI("Created a Lefty interface instance"); +} + +// Methods from ::vendor::google_clockwork::lefty::V1_0::ILefty follow. +Return<void> Lefty::setLeftyMode(bool enabled) { + if (mHubConnection->initCheck() == ::android::OK + && mHubConnection->getAliveCheck() == ::android::OK) { + mHubConnection->setLeftyMode(enabled); + } + return Void(); +} + + +ILefty* HIDL_FETCH_ILefty(const char* /* name */) { + return new Lefty(); +} + +} // namespace implementation +} // namespace V1_0 +} // namespace lefty +} // namespace google_clockwork +} // namespace vendor diff --git a/lefty/Lefty.h b/lefty/Lefty.h new file mode 100644 index 00000000..397d2154 --- /dev/null +++ b/lefty/Lefty.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H +#define VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H + +#include "hubconnection.h" +#undef LIKELY +#undef UNLIKELY +#include <vendor/google_clockwork/lefty/1.0/ILefty.h> + +namespace vendor { +namespace google_clockwork { +namespace lefty { +namespace V1_0 { +namespace implementation { + +using ::vendor::google_clockwork::lefty::V1_0::ILefty; +using ::android::hardware::Return; +using ::android::hardware::Void; +using ::android::sp; +using ::android::HubConnection; + +struct Lefty : public ILefty { + Lefty(); + + // Methods from ::vendor::google_clockwork::lefty::V1_0::ILefty follow. + Return<void> setLeftyMode(bool enabled) override; + +private: + sp<HubConnection> mHubConnection; +}; + +extern "C" ILefty* HIDL_FETCH_ILefty(const char* name); + +} // namespace implementation +} // namespace V1_0 +} // namespace lefty +} // namespace google_clockwork +} // namespace vendor + +#endif // VENDOR_GOOGLE_CLOCKWORK_LEFTY_V1_0_LEFTY_H diff --git a/lefty/lefty_service.cpp b/lefty/lefty_service.cpp new file mode 100644 index 00000000..f9e3f946 --- /dev/null +++ b/lefty/lefty_service.cpp @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "Lefty.h" +#include "lefty_service.h" + +#include <android-base/logging.h> +#include <hidl/LegacySupport.h> +#include <utils/StrongPointer.h> +#include <vendor/google_clockwork/lefty/1.0/ILefty.h> + +using ::android::sp; +using ::vendor::google_clockwork::lefty::V1_0::ILefty; +using ::vendor::google_clockwork::lefty::V1_0::implementation::Lefty; + +void register_lefty_service() { + // Kids, don't do this at home. Here, registerAsService is called without + // configureRpcThreadpool/joinRpcThreadpool, because it is called from + // open_sensors() function, called from HIDL_FETCH_ISensors, called from + // ISensor::getService, called from registerPassthroughServiceImplementation + // which is surrounded with configureRpcThreadpool/joinRpcThreadpool. + sp<ILefty> lefty = new Lefty(); + CHECK_EQ(lefty->registerAsService(), android::NO_ERROR) + << "Failed to register Lefty HAL"; +} diff --git a/lefty/lefty_service.h b/lefty/lefty_service.h new file mode 100644 index 00000000..f6ffb74a --- /dev/null +++ b/lefty/lefty_service.h @@ -0,0 +1,17 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +extern "C" void register_lefty_service(); diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk index 1f35410d..6a625db1 100644 --- a/sensorhal/Android.mk +++ b/sensorhal/Android.mk @@ -88,6 +88,11 @@ LOCAL_CFLAGS += -DDYNAMIC_SENSOR_EXT_ENABLED LOCAL_SHARED_LIBRARIES += libdynamic_sensor_ext endif +ifeq ($(NANOHUB_SENSORHAL_LEFTY_SERVICE_ENABLED), true) +LOCAL_CFLAGS += -DLEFTY_SERVICE_ENABLED +LOCAL_SHARED_LIBRARIES += liblefty_service_nanohub +endif + include $(BUILD_SHARED_LIBRARY) ################################################################################ diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp index 236a2359..42366c47 100644 --- a/sensorhal/hubconnection.cpp +++ b/sensorhal/hubconnection.cpp @@ -52,6 +52,9 @@ #define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF)) #define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl" #define APP_ID_APP_BMI160 2 +#define APP_ID_APP_WRIST_TILT_DETECT 0x1005 +#define APP_ID_APP_GAZE_DETECT 0x1009 +#define APP_ID_APP_UNGAZE_DETECT 0x100a #define SENS_TYPE_TO_EVENT(_sensorType) (EVT_NO_FIRST_SENSOR_EVENT + (_sensorType)) @@ -125,6 +128,10 @@ HubConnection::HubConnection() mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f; memset(&mGyroOtcData, 0, sizeof(mGyroOtcData)); + mLefty.accel = false; + mLefty.gyro = false; + mLefty.hub = false; + memset(&mSensorState, 0x00, sizeof(mSensorState)); mFd = open(NANOHUB_FILE_PATH, O_RDWR); mPollFds[0].fd = mFd; @@ -167,20 +174,32 @@ HubConnection::HubConnection() #endif // DOUBLE_TOUCH_ENABLED mSensorState[COMMS_SENSOR_ACCEL].sensorType = SENS_TYPE_ACCEL; - mSensorState[COMMS_SENSOR_ACCEL].alt = COMMS_SENSOR_ACCEL_UNCALIBRATED; + mSensorState[COMMS_SENSOR_ACCEL].alt[0] = COMMS_SENSOR_ACCEL_UNCALIBRATED; + mSensorState[COMMS_SENSOR_ACCEL].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE; mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].sensorType = SENS_TYPE_ACCEL; - mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].primary = SENS_TYPE_ACCEL; - mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt = COMMS_SENSOR_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].primary = COMMS_SENSOR_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[0] = COMMS_SENSOR_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].alt[1] = COMMS_SENSOR_ACCEL_WRIST_AWARE; + mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].sensorType = SENS_TYPE_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].primary = COMMS_SENSOR_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[0] = COMMS_SENSOR_ACCEL; + mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].alt[1] = COMMS_SENSOR_ACCEL_UNCALIBRATED; mSensorState[COMMS_SENSOR_GYRO].sensorType = SENS_TYPE_GYRO; - mSensorState[COMMS_SENSOR_GYRO].alt = COMMS_SENSOR_GYRO_UNCALIBRATED; + mSensorState[COMMS_SENSOR_GYRO].alt[0] = COMMS_SENSOR_GYRO_UNCALIBRATED; + mSensorState[COMMS_SENSOR_GYRO].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE; mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].sensorType = SENS_TYPE_GYRO; mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].primary = COMMS_SENSOR_GYRO; - mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt = COMMS_SENSOR_GYRO; + mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[0] = COMMS_SENSOR_GYRO; + mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].alt[1] = COMMS_SENSOR_GYRO_WRIST_AWARE; + mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].sensorType = SENS_TYPE_GYRO; + mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].primary = COMMS_SENSOR_GYRO; + mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[0] = COMMS_SENSOR_GYRO; + mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].alt[1] = COMMS_SENSOR_GYRO_UNCALIBRATED; mSensorState[COMMS_SENSOR_MAG].sensorType = SENS_TYPE_MAG; - mSensorState[COMMS_SENSOR_MAG].alt = COMMS_SENSOR_MAG_UNCALIBRATED; + mSensorState[COMMS_SENSOR_MAG].alt[0] = COMMS_SENSOR_MAG_UNCALIBRATED; mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].sensorType = SENS_TYPE_MAG; mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].primary = COMMS_SENSOR_MAG; - mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt = COMMS_SENSOR_MAG; + mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].alt[0] = COMMS_SENSOR_MAG; mSensorState[COMMS_SENSOR_LIGHT].sensorType = SENS_TYPE_ALS; mSensorState[COMMS_SENSOR_PROXIMITY].sensorType = SENS_TYPE_PROX; mSensorState[COMMS_SENSOR_PRESSURE].sensorType = SENS_TYPE_BARO; @@ -500,6 +519,62 @@ void HubConnection::saveSensorSettings() const { } } +void HubConnection::setLeftyMode(bool enable) { + struct MsgCmd *cmd; + size_t ret; + + Mutex::Autolock autoLock(mLock); + + if (enable == mLefty.hub) return; + + cmd = (struct MsgCmd *)malloc(sizeof(struct MsgCmd) + sizeof(bool)); + + if (cmd) { + cmd->evtType = EVT_APP_FROM_HOST; + cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_GAZE_DETECT); + cmd->msg.dataLen = sizeof(bool); + memcpy((bool *)(cmd+1), &enable, sizeof(bool)); + + ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + sizeof(bool))); + if (ret == sizeof(*cmd) + sizeof(bool)) + ALOGV("setLeftyMode: lefty (gaze) = %s\n", + (enable ? "true" : "false")); + else + ALOGE("setLeftyMode: failed to send command lefty (gaze) = %s\n", + (enable ? "true" : "false")); + + cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_UNGAZE_DETECT); + + ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + sizeof(bool))); + if (ret == sizeof(*cmd) + sizeof(bool)) + ALOGV("setLeftyMode: lefty (ungaze) = %s\n", + (enable ? "true" : "false")); + else + ALOGE("setLeftyMode: failed to send command lefty (ungaze) = %s\n", + (enable ? "true" : "false")); + + cmd->msg.appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, APP_ID_APP_WRIST_TILT_DETECT); + + ret = TEMP_FAILURE_RETRY(::write(mFd, cmd, sizeof(*cmd) + sizeof(bool))); + if (ret == sizeof(*cmd) + sizeof(bool)) + ALOGV("setLeftyMode: lefty (tilt) = %s\n", + (enable ? "true" : "false")); + else + ALOGE("setLeftyMode: failed to send command lefty (tilt) = %s\n", + (enable ? "true" : "false")); + + free(cmd); + } else { + ALOGE("setLeftyMode: failed to allocate command\n"); + return; + } + + queueFlushInternal(COMMS_SENSOR_ACCEL_WRIST_AWARE, true); + queueFlushInternal(COMMS_SENSOR_GYRO_WRIST_AWARE, true); + + mLefty.hub = enable; +} + sensors_event_t *HubConnection::initEv(sensors_event_t *ev, uint64_t timestamp, uint32_t type, uint32_t sensor) { memset(ev, 0x00, sizeof(sensors_event_t)); @@ -651,7 +726,7 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se { sensors_vec_t *sv; uncalibrated_event_t *ue; - sensors_event_t nev[2]; + sensors_event_t nev[3]; int cnt = 0; switch (sensor) { @@ -678,6 +753,16 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se ue->y_bias = mAccelBias[1]; ue->z_bias = mAccelBias[2]; } + + if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable) { + sv = &initEv(&nev[cnt++], timestamp, + SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE, + COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration; + sv->x = sample->ix * mScaleAccel; + sv->y = (mLefty.accel ? -sample->iy : sample->iy) * mScaleAccel; + sv->z = sample->iz * mScaleAccel; + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + } break; case COMMS_SENSOR_MAG: sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic; @@ -718,7 +803,7 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se sensors_vec_t *sv; uncalibrated_event_t *ue; sensors_event_t *ev; - sensors_event_t nev[2]; + sensors_event_t nev[3]; static const float heading_accuracy = M_PI / 6.0f; float w; int cnt = 0; @@ -747,6 +832,16 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se ue->y_bias = mAccelBias[1]; ue->z_bias = mAccelBias[2]; } + + if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable) { + sv = &initEv(&nev[cnt++], timestamp, + SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE, + COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration; + sv->x = sample->x; + sv->y = (mLefty.accel ? -sample->y : sample->y); + sv->z = sample->z; + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + } break; case COMMS_SENSOR_GYRO: sv = &initEv(&nev[cnt], timestamp, type, sensor)->gyro; @@ -771,6 +866,16 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se ue->y_bias = mGyroBias[1]; ue->z_bias = mGyroBias[2]; } + + if (mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].enable) { + sv = &initEv(&nev[cnt++], timestamp, + SENSOR_TYPE_GYROSCOPE_WRIST_AWARE, + COMMS_SENSOR_GYRO_WRIST_AWARE)->gyro; + sv->x = (mLefty.gyro ? -sample->x : sample->x); + sv->y = sample->y; + sv->z = (mLefty.gyro ? -sample->z : sample->z); + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + } break; case COMMS_SENSOR_ACCEL_BIAS: mAccelBias[0] = sample->x; @@ -955,6 +1060,9 @@ void HubConnection::postOsLog(uint8_t *buf, ssize_t len) case 'D': ALOGD("osLog: %s", &buf[5]); break; + case 'V': + ALOGV("osLog: %s", &buf[5]); + break; default: break; } @@ -1296,13 +1404,18 @@ ssize_t HubConnection::processBuf(uint8_t *buf, size_t len) ev.sensor = 0; ev.meta_data.what = META_DATA_FLUSH_COMPLETE; ev.meta_data.sensor = flush.handle; - --flush.count; - if (flush.count == 0) { + if (flush.internal) { + if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE) + mLefty.accel = !mLefty.accel; + else if (flush.handle == COMMS_SENSOR_GYRO_WRIST_AWARE) + mLefty.gyro = !mLefty.gyro; + } else + write(&ev, 1); + + if (--flush.count == 0) mFlushesPending[primary].pop_front(); - } - write(&ev, 1); ALOGV("flushing %d", ev.meta_data.sensor); } } @@ -1503,31 +1616,28 @@ void HubConnection::setActivityCallback(ActivityEventHandler *eventHandler) void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle) { - uint8_t alt = mSensorState[handle].alt; - memset(cmd, 0x00, sizeof(*cmd)); cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT; cmd->sensorType = mSensorState[handle].sensorType; + cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE; + cmd->rate = mSensorState[handle].rate; + cmd->latency = mSensorState[handle].latency; + + for (int i=0; i<MAX_ALTERNATES; ++i) { + uint8_t alt = mSensorState[handle].alt[i]; + + if (alt == COMMS_SENSOR_INVALID) continue; + if (!mSensorState[alt].enable) continue; - if (alt && mSensorState[alt].enable && mSensorState[handle].enable) { cmd->cmd = CONFIG_CMD_ENABLE; - if (mSensorState[alt].rate > mSensorState[handle].rate) + + if (mSensorState[alt].rate > cmd->rate) { cmd->rate = mSensorState[alt].rate; - else - cmd->rate = mSensorState[handle].rate; - if (mSensorState[alt].latency < mSensorState[handle].latency) + } + if (mSensorState[alt].latency < cmd->latency) { cmd->latency = mSensorState[alt].latency; - else - cmd->latency = mSensorState[handle].latency; - } else if (alt && mSensorState[alt].enable) { - cmd->cmd = mSensorState[alt].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE; - cmd->rate = mSensorState[alt].rate; - cmd->latency = mSensorState[alt].latency; - } else { /* !alt || !mSensorState[alt].enable */ - cmd->cmd = mSensorState[handle].enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE; - cmd->rate = mSensorState[handle].rate; - cmd->latency = mSensorState[handle].latency; + } } // will be a nop if direct report mode is not enabled @@ -1621,12 +1731,16 @@ void HubConnection::queueBatch( void HubConnection::queueFlush(int handle) { + Mutex::Autolock autoLock(mLock); + queueFlushInternal(handle, false); +} + +void HubConnection::queueFlushInternal(int handle, bool internal) +{ struct ConfigCmd cmd; uint32_t primary; int ret; - Mutex::Autolock autoLock(mLock); - if (isValidHandle(handle)) { // If no primary sensor type is specified, // then 'handle' is the primary sensor type. @@ -1635,10 +1749,12 @@ void HubConnection::queueFlush(int handle) std::list<Flush>& flushList = mFlushesPending[primary]; - if (!flushList.empty() && flushList.back().handle == handle) { + if (!flushList.empty() && + flushList.back().internal == internal && + flushList.back().handle == handle) { ++flushList.back().count; } else { - flushList.push_back((struct Flush){handle, 1}); + flushList.push_back((struct Flush){handle, 1, internal}); } initConfigCmd(&cmd, handle); diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h index 29ee4561..3f0a4f9c 100644 --- a/sensorhal/hubconnection.h +++ b/sensorhal/hubconnection.h @@ -50,6 +50,8 @@ #define GYRO_SW_BIAS_TAG "gyro_sw" #define MAG_BIAS_TAG "mag" +#define MAX_ALTERNATES 2 + namespace android { struct HubConnection : public Thread { @@ -95,6 +97,8 @@ struct HubConnection : public Thread { mScaleMag = scaleMag; } + void setLeftyMode(bool enable); + protected: HubConnection(); virtual ~HubConnection(); @@ -150,10 +154,21 @@ private: struct HostHubRawPacket msg; } __attribute__((packed)); + struct LeftyState + { + bool accel; // Process wrist-aware accel samples as lefty mode + bool gyro; // Process wrist-aware gyro samples as lefty mode + bool hub; // Sensor hub is currently operating in lefty mode + }; + struct Flush { int handle; uint8_t count; + + // Used to synchronize the transition in and out of + // lefty mode between nanohub and the AP. + bool internal; }; struct SensorState { @@ -161,7 +176,7 @@ private: rate_q10_t rate; uint8_t sensorType; uint8_t primary; - uint8_t alt; + uint8_t alt[MAX_ALTERNATES]; bool enable; }; @@ -239,6 +254,8 @@ private: float mScaleAccel, mScaleMag; + LeftyState mLefty; + SensorState mSensorState[NUM_COMMS_SENSORS_PLUS_1]; std::list<struct Flush> mFlushesPending[NUM_COMMS_SENSORS_PLUS_1]; @@ -267,6 +284,8 @@ private: void initConfigCmd(struct ConfigCmd *cmd, int handle); + void queueFlushInternal(int handle, bool internal); + void queueDataInternal(int handle, void *data, size_t length); void discardInotifyEvent(); diff --git a/sensorhal/hubdefs.h b/sensorhal/hubdefs.h index 99c8e26b..76ae8e7e 100644 --- a/sensorhal/hubdefs.h +++ b/sensorhal/hubdefs.h @@ -84,6 +84,8 @@ enum comms_sensor_t { COMMS_SENSOR_UNGAZE = 46, COMMS_SENSOR_ACCEL_UNCALIBRATED = 47, COMMS_SENSOR_HUMIDITY = 48, + COMMS_SENSOR_ACCEL_WRIST_AWARE = 49, + COMMS_SENSOR_GYRO_WRIST_AWARE = 50, NUM_COMMS_SENSORS_PLUS_1, @@ -117,6 +119,8 @@ enum { SENSOR_TYPE_DOUBLE_TOUCH = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 4, SENSOR_TYPE_GAZE = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 5, SENSOR_TYPE_UNGAZE = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 6, + SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE=SENSOR_TYPE_DEVICE_PRIVATE_BASE + 7, + SENSOR_TYPE_GYROSCOPE_WRIST_AWARE = SENSOR_TYPE_DEVICE_PRIVATE_BASE + 8, }; } // namespace android diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp index 65b881c5..0ddb263b 100644 --- a/sensorhal/sensors.cpp +++ b/sensorhal/sensors.cpp @@ -35,6 +35,10 @@ #include <SensorEventCallback.h> #endif +#ifdef LEFTY_SERVICE_ENABLED +#include "lefty_service.h" +#endif + using namespace android; //////////////////////////////////////////////////////////////////////////////// @@ -407,6 +411,9 @@ static int open_sensors( gHubAlive = ctx->getHubAlive(); *dev = &ctx->device.common; +#ifdef LEFTY_SERVICE_ENABLED + register_lefty_service(); +#endif return 0; } |