nanohub: chre: add support for osChreSensorConfigure
Bug: 32319611
Test: subscribe to sensor using chre calls and verify samples
are received by the chre event handler in the expected chre format.
Change-Id: I54b328e79d17412585113e1ac1f852b33c655b34
Signed-off-by: Ben Fennema <fennema@google.com>
diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c
index d12941a..f09a238 100644
--- a/firmware/app/chre/common/chre_app.c
+++ b/firmware/app/chre/common/chre_app.c
@@ -19,8 +19,14 @@
#include <timer.h>
#include <toolchain.h>
#include <crt_priv.h>
+#include <string.h>
#include <chre.h>
+#include <sensors.h>
+#include <syscallDo.h>
+#include <hostIntf.h>
+
+#define SENSOR_TYPE(x) ((x) & 0xFF)
/*
* Common CHRE App support code
@@ -38,6 +44,146 @@
__crt_exit();
}
+static void initDataHeader(struct chreSensorDataHeader *header, uint64_t timestamp, uint32_t sensorHandle) {
+ header->baseTimestamp = timestamp;
+ header->sensorHandle = sensorHandle;
+ header->readingCount = 1;
+ header->reserved[0] = header->reserved[1] = 0;
+}
+
+static void processTripleAxisData(const struct TripleAxisDataEvent *src, uint32_t sensorHandle, uint8_t sensorType)
+{
+ int i;
+ struct chreSensorThreeAxisData three;
+
+ initDataHeader(&three.header, src->referenceTime, sensorHandle);
+ three.readings[0].timestampDelta = 0;
+
+ for (i=0; i<src->samples[0].firstSample.numSamples; i++) {
+ if (i > 0)
+ three.header.baseTimestamp += src->samples[i].deltaTime;
+ three.readings[0].x = src->samples[i].x;
+ three.readings[0].y = src->samples[i].y;
+ three.readings[0].z = src->samples[i].z;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &three);
+ }
+}
+
+static void processSingleAxisData(const struct SingleAxisDataEvent *src, uint32_t sensorHandle, uint8_t sensorType)
+{
+ int i;
+
+ switch (sensorType) {
+ case CHRE_SENSOR_TYPE_INSTANT_MOTION_DETECT:
+ case CHRE_SENSOR_TYPE_STATIONARY_DETECT: {
+ struct chreSensorOccurrenceData occ;
+
+ initDataHeader(&occ.header, src->referenceTime, sensorHandle);
+ occ.readings[0].timestampDelta = 0;
+
+ for (i=0; i<src->samples[0].firstSample.numSamples; i++) {
+ if (i > 0)
+ occ.header.baseTimestamp += src->samples[i].deltaTime;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &occ);
+ }
+ break;
+ }
+ case CHRE_SENSOR_TYPE_LIGHT:
+ case CHRE_SENSOR_TYPE_PRESSURE: {
+ struct chreSensorFloatData flt;
+
+ initDataHeader(&flt.header, src->referenceTime, sensorHandle);
+ flt.readings[0].timestampDelta = 0;
+
+ for (i=0; i<src->samples[0].firstSample.numSamples; i++) {
+ if (i > 0)
+ flt.header.baseTimestamp += src->samples[i].deltaTime;
+ flt.readings[0].value = src->samples[i].fdata;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &flt);
+ }
+ break;
+ }
+ case CHRE_SENSOR_TYPE_PROXIMITY: {
+ struct chreSensorByteData byte;
+
+ initDataHeader(&byte.header, src->referenceTime, sensorHandle);
+ byte.readings[0].timestampDelta = 0;
+
+ for (i=0; i<src->samples[0].firstSample.numSamples; i++) {
+ if (i > 0)
+ byte.header.baseTimestamp += src->samples[i].deltaTime;
+ byte.readings[0].isNear = src->samples[i].fdata == 0.0f;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &byte);
+ }
+ break;
+ }
+ }
+}
+
+static void processEmbeddedData(const void *src, uint32_t sensorHandle, uint8_t sensorType)
+{
+ union EmbeddedDataPoint data = (union EmbeddedDataPoint)((void *)src);
+
+ switch (sensorType) {
+ case CHRE_SENSOR_TYPE_INSTANT_MOTION_DETECT:
+ case CHRE_SENSOR_TYPE_STATIONARY_DETECT: {
+ struct chreSensorOccurrenceData occ;
+
+ initDataHeader(&occ.header, eOsSensorGetTime(), sensorHandle);
+ occ.readings[0].timestampDelta = 0;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &occ);
+ break;
+ }
+ case CHRE_SENSOR_TYPE_LIGHT:
+ case CHRE_SENSOR_TYPE_PRESSURE: {
+ struct chreSensorFloatData flt;
+
+ initDataHeader(&flt.header, eOsSensorGetTime(), sensorHandle);
+ flt.readings[0].timestampDelta = 0;
+ flt.readings[0].value = data.fdata;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &flt);
+ break;
+ }
+ case CHRE_SENSOR_TYPE_PROXIMITY: {
+ struct chreSensorByteData byte;
+
+ initDataHeader(&byte.header, eOsSensorGetTime(), sensorHandle);
+ byte.readings[0].timestampDelta = 0;
+ byte.readings[0].isNear = data.fdata == 0.0f;
+
+ nanoappHandleEvent(CHRE_INSTANCE_ID, CHRE_EVENT_SENSOR_DATA_EVENT_BASE | sensorType, &byte);
+ break;
+ }
+ }
+}
+
+static void chreappProcessSensorData(uint16_t evt, const void *eventData)
+{
+ const struct SensorInfo *si;
+ uint32_t sensorHandle;
+
+ si = eOsSensorFind(SENSOR_TYPE(evt), 0, &sensorHandle);
+ if (si) {
+ switch (si->numAxis) {
+ case NUM_AXIS_EMBEDDED:
+ processEmbeddedData(eventData, sensorHandle, SENSOR_TYPE(evt));
+ break;
+ case NUM_AXIS_ONE:
+ processSingleAxisData(eventData, sensorHandle, SENSOR_TYPE(evt));
+ break;
+ case NUM_AXIS_THREE:
+ processTripleAxisData(eventData, sensorHandle, SENSOR_TYPE(evt));
+ break;
+ }
+ }
+}
+
static void chreappHandle(uint32_t eventTypeAndTid, const void *eventData)
{
uint16_t evt = eventTypeAndTid;
@@ -74,6 +220,10 @@
// ignore any other system events; OS may send them to any app
if (evt < EVT_NO_FIRST_USER_EVENT)
return;
+ else if (evt > EVT_NO_FIRST_SENSOR_EVENT && evt < EVT_NO_SENSOR_CONFIG_EVENT) {
+ chreappProcessSensorData(evt, data);
+ return;
+ }
}
nanoappHandleEvent(srcTid, evt, data);
}
diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c
index 7bb42c8..5a7cb7d 100644
--- a/firmware/os/core/nanohub_chre.c
+++ b/firmware/os/core/nanohub_chre.c
@@ -21,6 +21,7 @@
#include <string.h>
#include <cpu.h>
+#include <cpu/cpuMath.h>
#include <heap.h>
#include <sensors.h>
#include <sensors_priv.h>
@@ -260,10 +261,25 @@
if (!s || !status)
return false;
- // TODO
- status->interval = 0;
- status->latency = 0;
- status->enabled = 0;
+ if (s->currentRate == SENSOR_RATE_OFF) {
+ status->enabled = 0;
+ status->interval = 0;
+ status->latency = 0;
+ } else {
+ status->enabled = true;
+ if ((s->currentRate == SENSOR_RATE_ONDEMAND ||
+ s->currentRate == SENSOR_RATE_ONCHANGE ||
+ s->currentRate == SENSOR_RATE_ONESHOT))
+ status->interval = CHRE_SENSOR_INTERVAL_DEFAULT;
+ else
+ status->interval = (UINT32_C(1024000000) / s->currentRate) * UINT64_C(1000);
+
+ if (s->currentLatency == SENSOR_LATENCY_NODATA)
+ status->latency = CHRE_SENSOR_INTERVAL_DEFAULT;
+ else
+ status->latency = s->currentLatency;
+ }
+
return true;
}
@@ -278,9 +294,55 @@
enum chreSensorConfigureMode mode,
uint64_t interval, uint64_t latency)
{
- // TODO
- osLog(LOG_ERROR, "%s: not implemented\n", __func__);
- return false;
+ uint32_t rate, interval_us;
+ bool ret;
+ struct Sensor *s = sensorFindByHandle(sensorHandle);
+ if (!s)
+ return false;
+
+ if (mode & CHRE_SENSOR_CONFIGURE_RAW_POWER_ON) {
+ if (interval == CHRE_SENSOR_INTERVAL_DEFAULT) {
+ // use first rate in supported rates list
+ const struct SensorInfo *si = s->si;
+ if (!si)
+ return false;
+
+ if (!si->supportedRates || si->supportedRates[0] == 0)
+ rate = SENSOR_RATE_ONCHANGE;
+ else
+ rate = si->supportedRates[0];
+ } else {
+ interval_us = U64_DIV_BY_CONST_U16(interval, 1000);
+ rate = UINT32_C(1024000000) / interval_us;
+ }
+ if (!rate) // 0 is a reserved value. minimum is 1
+ rate = 1;
+ if (latency == CHRE_SENSOR_LATENCY_DEFAULT)
+ latency = 0ULL;
+ if (s->currentRate == SENSOR_RATE_OFF) {
+ if ((ret = sensorRequest(0, sensorHandle, rate, latency)))
+ ret = osEventSubscribe(0, sensorGetMyEventType(s->si->sensorType));
+ else
+ sensorRelease(0, sensorHandle);
+ } else {
+ ret = sensorRequestRateChange(0, sensorHandle, rate, latency);
+ }
+ } else if (mode & (CHRE_SENSOR_CONFIGURE_RAW_REPORT_CONTINUOUS|CHRE_SENSOR_CONFIGURE_RAW_REPORT_ONE_SHOT)) {
+ // TODO: ONE_SHOT: unsubscribe after receiving a sample
+ if (s->currentRate == SENSOR_RATE_OFF)
+ ret = osEventSubscribe(0, sensorGetMyEventType(s->si->sensorType));
+ else
+ ret = true;
+ } else {
+ if (s->currentRate != SENSOR_RATE_OFF) {
+ if ((ret = sensorRelease(0, sensorHandle)))
+ ret = osEventUnsubscribe(0, sensorGetMyEventType(s->si->sensorType));
+ } else {
+ ret = true;
+ }
+ }
+
+ return ret;
}
static void osChreApiSensorConfig(uintptr_t *retValP, va_list args)
