1 /*
2  * Copyright (C) 2016 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <inttypes.h>
18 #include <stdint.h>
19 #include <sys/endian.h>
20 #include <string.h>
21 #include <alloca.h>
22 
23 #include <variant/variant.h>
24 #include <eventnums.h>
25 
26 #include <plat/pwr.h>
27 
28 #include <nanohub/crc.h>
29 
30 #include <platform.h>
31 #include <cpu.h>
32 #include <halIntf.h>
33 #include <hostIntf.h>
34 #include <hostIntf_priv.h>
35 #include <nanohubCommand.h>
36 #include <nanohubPacket.h>
37 #include <seos.h>
38 #include <seos_priv.h>
39 #include <util.h>
40 #include <atomicBitset.h>
41 #include <atomic.h>
42 #include <gpio.h>
43 #include <apInt.h>
44 #include <sensors.h>
45 #include <timer.h>
46 #include <heap.h>
47 #include <simpleQ.h>
48 
49 #define HOSTINTF_MAX_ERR_MSG    8
50 #define MAX_NUM_BLOCKS          280         /* times 256 = 71680 bytes */
51 #define MIN_NUM_BLOCKS          10          /* times 256 = 2560 bytes */
52 #define SENSOR_INIT_DELAY       500000000   /* ns */
53 #define SENSOR_INIT_ERROR_MAX   4
54 #define CHECK_LATENCY_TIME      500000000   /* ns */
55 #define EVT_LATENCY_TIMER       EVT_NO_FIRST_USER_EVENT
56 
57 static const uint32_t delta_time_multiplier_order = 9;
58 static const uint32_t delta_time_coarse_mask = ~1;
59 static const uint32_t delta_time_fine_mask = 1;
60 static const uint32_t delta_time_rounding = 0x200;      /* 1ul << delta_time_multiplier_order */
61 static const uint64_t delta_time_max = 0x1FFFFFFFE00;   /* UINT32_MAX << delta_time_multiplier_order */
62 
63 enum ConfigCmds
64 {
65     CONFIG_CMD_DISABLE      = 0,
66     CONFIG_CMD_ENABLE       = 1,
67     CONFIG_CMD_FLUSH        = 2,
68     CONFIG_CMD_CFG_DATA     = 3,
69     CONFIG_CMD_CALIBRATE    = 4,
70     CONFIG_CMD_SELF_TEST    = 5,
71 };
72 
73 struct ConfigCmd
74 {
75     uint64_t latency;
76     uint32_t rate;
77     uint8_t sensType;
78     uint8_t cmd;
79     uint16_t flags;
80 } __attribute__((packed));
81 
82 struct ActiveSensor
83 {
84     uint64_t latency;
85     uint64_t firstTime;
86     uint64_t lastTime;
87     struct HostIntfDataBuffer buffer;
88     uint32_t rate;
89     uint32_t sensorHandle;
90     float rawScale;
91     uint16_t minSamples;
92     uint16_t curSamples;
93     uint8_t numAxis;
94     uint8_t interrupt;
95     uint8_t numSamples;
96     uint8_t packetSamples;
97     // The sensorType used to report bias samples; normally the same as
98     // buffer.sensorType, but in the case of raw, this gets set to the base
99     // sensorType matching struct SensorInfo (because the sensor can have a
100     // different rawType). Note that this is different than biasType in struct
101     // SensorInfo.
102     uint8_t biasReportType;
103     uint8_t oneshot : 1;
104     uint8_t discard : 1;
105     uint8_t raw : 1;
106     uint8_t reserved : 5;
107 } __attribute__((packed));
108 
109 static uint8_t mSensorList[SENS_TYPE_LAST_USER];
110 static struct SimpleQueue *mOutputQ;
111 static struct ActiveSensor *mActiveSensorTable;
112 static uint8_t mNumSensors;
113 static uint8_t mLastSensor;
114 
115 static const struct HostIntfComm *mComm;
116 static bool mBusy;
117 static uint64_t mRxTimestamp;
118 static uint8_t mRxBuf[NANOHUB_PACKET_SIZE_MAX];
119 static size_t mRxSize;
120 static struct
121 {
122     const struct NanohubCommand *cmd;
123     uint32_t seq;
124     bool seqMatch;
125 } mTxRetrans;
126 static struct
127 {
128     uint8_t pad; // packet header is 10 bytes. + 2 to word align
129     uint8_t prePreamble;
130     uint8_t buf[NANOHUB_PACKET_SIZE_MAX];
131     uint8_t postPreamble;
132 } mTxBuf;
133 static struct
134 {
135     uint8_t pad; // packet header is 10 bytes. + 2 to word align
136     uint8_t prePreamble;
137     uint8_t buf[NANOHUB_PACKET_SIZE_MIN];
138     uint8_t postPreamble;
139 } mTxNakBuf;
140 static size_t mTxSize;
141 static uint8_t *mTxBufPtr;
142 static const struct NanohubCommand *mRxCmd;
143 ATOMIC_BITSET_DECL(mInterrupt, HOSTINTF_MAX_INTERRUPTS, static);
144 ATOMIC_BITSET_DECL(mInterruptMask, HOSTINTF_MAX_INTERRUPTS, static);
145 static uint32_t mInterruptCntWkup, mInterruptCntNonWkup;
146 static uint32_t mWakeupBlocks, mNonWakeupBlocks, mTotalBlocks;
147 static uint32_t mHostIntfTid;
148 static uint32_t mLatencyTimer;
149 static uint8_t mLatencyCnt;
150 
151 static uint8_t mRxIdle;
152 static uint8_t mWakeActive;
153 static uint8_t mActiveWrite;
154 static uint8_t mRestartRx;
155 static uint8_t mIntErrMsgIdx;
156 static volatile uint32_t mIntErrMsgCnt;
157 
158 enum hostIntfIntErrReason
159 {
160     HOSTINTF_ERR_PKG_INCOMPELETE = 0,
161     HOSTINTF_ERR_PGK_SIZE,
162     HOSTINTF_ERR_PKG_PAYLOAD_SIZE,
163     HOSTINTF_ERR_PKG_CRC,
164     HOSTINTF_ERR_RECEIVE,
165     HOSTINTF_ERR_SEND,
166     HOSTINTF_ERR_ACK,
167     HOSTINTF_ERR_NAK,
168     HOSTINTF_ERR_UNKNOWN
169 };
170 
171 struct hostIntfIntErrMsg
172 {
173     enum LogLevel level;
174     enum hostIntfIntErrReason reason;
175     const char *func;
176 };
177 static struct hostIntfIntErrMsg mIntErrMsg[HOSTINTF_MAX_ERR_MSG];
178 
179 static void hostIntfTxPacket(uint32_t reason, uint8_t len, uint32_t seq,
180         HostIntfCommCallbackF callback);
181 
182 static void hostIntfRxDone(size_t rx, int err);
183 static void hostIntfGenerateAck(void *cookie);
184 
185 static void hostIntfTxAckDone(size_t tx, int err);
186 static void hostIntfGenerateResponse(void *cookie);
187 
188 static void hostIntfTxPayloadDone(size_t tx, int err);
189 
hostIntfGetPayload(uint8_t * buf)190 static inline void *hostIntfGetPayload(uint8_t *buf)
191 {
192     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
193     return packet->data;
194 }
195 
hostIntfGetPayloadLen(uint8_t * buf)196 static inline uint8_t hostIntfGetPayloadLen(uint8_t *buf)
197 {
198     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
199     return packet->len;
200 }
201 
hostIntfGetFooter(uint8_t * buf)202 static inline struct NanohubPacketFooter *hostIntfGetFooter(uint8_t *buf)
203 {
204     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
205     return (struct NanohubPacketFooter *)(buf + sizeof(*packet) + packet->len);
206 }
207 
hostIntfComputeCrc(uint8_t * buf)208 static inline __le32 hostIntfComputeCrc(uint8_t *buf)
209 {
210     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
211     uint32_t crc = crc32(packet, packet->len + sizeof(*packet), CRC_INIT);
212     return htole32(crc);
213 }
214 
hostIntfPrintErrMsg(void * cookie)215 static void hostIntfPrintErrMsg(void *cookie)
216 {
217     struct hostIntfIntErrMsg *msg = (struct hostIntfIntErrMsg *)cookie;
218     osLog(msg->level, "%s failed with: %d\n", msg->func, msg->reason);
219     atomicAdd32bits(&mIntErrMsgCnt, -1UL);
220 }
221 
hostIntfDeferErrLog(enum LogLevel level,enum hostIntfIntErrReason reason,const char * func)222 static void hostIntfDeferErrLog(enum LogLevel level, enum hostIntfIntErrReason reason, const char *func)
223 {
224     // If the message buffer is full, we drop the newer messages.
225     if (atomicRead32bits(&mIntErrMsgCnt) == HOSTINTF_MAX_ERR_MSG)
226         return;
227 
228     mIntErrMsg[mIntErrMsgIdx].level = level;
229     mIntErrMsg[mIntErrMsgIdx].reason = reason;
230     mIntErrMsg[mIntErrMsgIdx].func = func;
231     if (osDefer(hostIntfPrintErrMsg, &mIntErrMsg[mIntErrMsgIdx], false)) {
232         atomicAdd32bits(&mIntErrMsgCnt, 1UL);
233         mIntErrMsgIdx = (mIntErrMsgIdx + 1) % HOSTINTF_MAX_ERR_MSG;
234     }
235 }
236 
hostIntfFindHandler(uint8_t * buf,size_t size,uint32_t * seq)237 static inline const struct NanohubCommand *hostIntfFindHandler(uint8_t *buf, size_t size, uint32_t *seq)
238 {
239     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
240     struct NanohubPacketFooter *footer;
241     __le32 packetCrc;
242     uint32_t packetReason;
243     const struct NanohubCommand *cmd;
244 
245     if (size < NANOHUB_PACKET_SIZE(0)) {
246         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_INCOMPELETE, __func__);
247         return NULL;
248     }
249 
250     if (size != NANOHUB_PACKET_SIZE(packet->len)) {
251         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PGK_SIZE, __func__);
252         return NULL;
253     }
254 
255     footer = hostIntfGetFooter(buf);
256     packetCrc = hostIntfComputeCrc(buf);
257     if (footer->crc != packetCrc) {
258         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_CRC, __func__);
259         return NULL;
260     }
261 
262     if (mTxRetrans.seq == packet->seq) {
263         mTxRetrans.seqMatch = true;
264         return mTxRetrans.cmd;
265     } else {
266         mTxRetrans.seqMatch = false;
267     }
268 
269     *seq = packet->seq;
270 
271     if (mBusy)
272         return NULL;
273 
274     packetReason = le32toh(packet->reason);
275 
276     if ((cmd = nanohubFindCommand(packetReason)) != NULL) {
277         if (packet->len < cmd->minDataLen || packet->len > cmd->maxDataLen) {
278             hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_PAYLOAD_SIZE, __func__);
279             return NULL;
280         }
281 
282         return cmd;
283     }
284 
285     hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_UNKNOWN, __func__);
286     return NULL;
287 }
288 
hostIntfTxBuf(int size,uint8_t * buf,HostIntfCommCallbackF callback)289 static void hostIntfTxBuf(int size, uint8_t *buf, HostIntfCommCallbackF callback)
290 {
291     mTxSize = size;
292     mTxBufPtr = buf;
293     mComm->txPacket(mTxBufPtr, mTxSize, callback);
294 }
295 
hostIntfTxPacket(__le32 reason,uint8_t len,uint32_t seq,HostIntfCommCallbackF callback)296 static void hostIntfTxPacket(__le32 reason, uint8_t len, uint32_t seq,
297         HostIntfCommCallbackF callback)
298 {
299     struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxBuf.buf);
300     txPacket->reason = reason;
301     txPacket->seq = seq;
302     txPacket->sync = NANOHUB_SYNC_BYTE;
303     txPacket->len = len;
304 
305     struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxBuf.buf);
306     txFooter->crc = hostIntfComputeCrc(mTxBuf.buf);
307 
308     // send starting with the prePremable byte
309     hostIntfTxBuf(1+NANOHUB_PACKET_SIZE(len), &mTxBuf.prePreamble, callback);
310 }
311 
hostIntfTxNakPacket(__le32 reason,uint32_t seq,HostIntfCommCallbackF callback)312 static void hostIntfTxNakPacket(__le32 reason, uint32_t seq,
313         HostIntfCommCallbackF callback)
314 {
315     struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxNakBuf.buf);
316     txPacket->reason = reason;
317     txPacket->seq = seq;
318     txPacket->sync = NANOHUB_SYNC_BYTE;
319     txPacket->len = 0;
320 
321     struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxNakBuf.buf);
322     txFooter->crc = hostIntfComputeCrc(mTxNakBuf.buf);
323 
324     // send starting with the prePremable byte
325     hostIntfTxBuf(1+NANOHUB_PACKET_SIZE_MIN, &mTxNakBuf.prePreamble, callback);
326 }
327 
hostIntfTxPacketDone(int err,size_t tx,HostIntfCommCallbackF callback)328 static inline bool hostIntfTxPacketDone(int err, size_t tx,
329         HostIntfCommCallbackF callback)
330 {
331     if (!err && tx < mTxSize) {
332         mTxSize -= tx;
333         mTxBufPtr += tx;
334 
335         mComm->txPacket(mTxBufPtr, mTxSize, callback);
336         return false;
337     }
338 
339     return true;
340 }
341 
hostIntfRequest(uint32_t tid)342 static bool hostIntfRequest(uint32_t tid)
343 {
344     mHostIntfTid = tid;
345     atomicBitsetInit(mInterrupt, HOSTINTF_MAX_INTERRUPTS);
346     atomicBitsetInit(mInterruptMask, HOSTINTF_MAX_INTERRUPTS);
347 #ifdef AP_INT_NONWAKEUP
348     hostIntfSetInterruptMask(NANOHUB_INT_NONWAKEUP);
349 #endif
350     mTxBuf.prePreamble = NANOHUB_PREAMBLE_BYTE;
351     mTxBuf.postPreamble = NANOHUB_PREAMBLE_BYTE;
352     mTxNakBuf.prePreamble = NANOHUB_PREAMBLE_BYTE;
353     mTxNakBuf.postPreamble = NANOHUB_PREAMBLE_BYTE;
354 
355     mComm = platHostIntfInit();
356     if (mComm) {
357         int err = mComm->request();
358         if (!err) {
359             nanohubInitCommand();
360             mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
361             osEventSubscribe(mHostIntfTid, EVT_APP_START);
362             return true;
363         }
364     }
365 
366     return false;
367 }
368 
hostIntfRxPacket(bool wakeupActive)369 void hostIntfRxPacket(bool wakeupActive)
370 {
371     if (mWakeActive) {
372         if (atomicXchgByte(&mRxIdle, false)) {
373             if (!wakeupActive)
374                 hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
375             mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
376             if (wakeupActive)
377                 hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
378         } else if (atomicReadByte(&mActiveWrite)) {
379             atomicWriteByte(&mRestartRx, true);
380         } else {
381             if (!wakeupActive)
382                 hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
383             else
384                 hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
385         }
386     } else if (wakeupActive && !atomicReadByte(&mActiveWrite))
387         hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
388 
389     mWakeActive = wakeupActive;
390 }
391 
hostIntfRxDone(size_t rx,int err)392 static void hostIntfRxDone(size_t rx, int err)
393 {
394     mRxTimestamp = sensorGetTime();
395     mRxSize = rx;
396 
397     if (err != 0) {
398         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_RECEIVE, __func__);
399         return;
400     }
401 
402     hostIntfGenerateAck(NULL);
403 }
404 
hostIntfTxSendAck(uint32_t resp)405 static void hostIntfTxSendAck(uint32_t resp)
406 {
407     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
408 
409     if (resp == NANOHUB_FAST_UNHANDLED_ACK) {
410         hostIntfCopyInterrupts(txPayload, HOSTINTF_MAX_INTERRUPTS);
411         hostIntfTxPacket(NANOHUB_REASON_ACK, 32, mTxRetrans.seq, hostIntfTxAckDone);
412     } else if (resp == NANOHUB_FAST_DONT_ACK) {
413         // do nothing. something else will do the ack
414     } else {
415         hostIntfTxPacket(mRxCmd->reason, resp, mTxRetrans.seq, hostIntfTxPayloadDone);
416     }
417 }
418 
hostIntfTxAck(void * buffer,uint8_t len)419 void hostIntfTxAck(void *buffer, uint8_t len)
420 {
421     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
422 
423     memcpy(txPayload, buffer, len);
424 
425     hostIntfTxSendAck(len);
426 }
427 
hostIntfGenerateAck(void * cookie)428 static void hostIntfGenerateAck(void *cookie)
429 {
430     uint32_t seq = 0;
431     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
432     void *rxPayload = hostIntfGetPayload(mRxBuf);
433     uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
434     uint32_t resp = NANOHUB_FAST_UNHANDLED_ACK;
435 
436     atomicWriteByte(&mActiveWrite, true);
437     hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
438     mRxCmd = hostIntfFindHandler(mRxBuf, mRxSize, &seq);
439 
440     if (mRxCmd) {
441         if (mTxRetrans.seqMatch) {
442             hostIntfTxBuf(mTxSize, &mTxBuf.prePreamble, hostIntfTxPayloadDone);
443         } else {
444             mTxRetrans.seq = seq;
445             mTxRetrans.cmd = mRxCmd;
446             if (mRxCmd->fastHandler)
447                 resp = mRxCmd->fastHandler(rxPayload, rx_len, txPayload, mRxTimestamp);
448 
449             hostIntfTxSendAck(resp);
450         }
451     } else {
452         if (mBusy)
453             hostIntfTxNakPacket(NANOHUB_REASON_NAK_BUSY, seq, hostIntfTxAckDone);
454         else
455             hostIntfTxNakPacket(NANOHUB_REASON_NAK, seq, hostIntfTxAckDone);
456     }
457 }
458 
459 
hostIntfTxComplete(bool clearInt,bool restartRx)460 static void hostIntfTxComplete(bool clearInt, bool restartRx)
461 {
462     if (restartRx || clearInt || !mWakeActive)
463         hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
464     atomicWriteByte(&mActiveWrite, false);
465     atomicWriteByte(&mRestartRx, false);
466     if (restartRx) {
467         mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
468         hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
469     } else {
470         atomicWriteByte(&mRxIdle, true);
471     }
472 }
473 
hostIntfTxAckDone(size_t tx,int err)474 static void hostIntfTxAckDone(size_t tx, int err)
475 {
476     hostIntfTxPacketDone(err, tx, hostIntfTxAckDone);
477 
478     if (err) {
479         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_ACK, __func__);
480         hostIntfTxComplete(false, false);
481         return;
482     }
483 
484     if (!mRxCmd) {
485         if (!mBusy)
486             hostIntfDeferErrLog(LOG_DEBUG, HOSTINTF_ERR_NAK, __func__);
487         if (atomicReadByte(&mRestartRx))
488             hostIntfTxComplete(false, true);
489         else
490             hostIntfTxComplete(false, false);
491         return;
492     } else if (atomicReadByte(&mRestartRx)) {
493         mTxRetrans.seq = 0;
494         mTxRetrans.cmd = NULL;
495         hostIntfTxComplete(false, true);
496     } else {
497         if (!osDefer(hostIntfGenerateResponse, NULL, true)) {
498             mTxRetrans.seq = 0;
499             mTxRetrans.cmd = NULL;
500             hostIntfTxComplete(false, false);
501         }
502     }
503 }
504 
hostIntfGenerateResponse(void * cookie)505 static void hostIntfGenerateResponse(void *cookie)
506 {
507     void *rxPayload = hostIntfGetPayload(mRxBuf);
508     uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
509     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
510     uint8_t respLen = mRxCmd->handler(rxPayload, rx_len, txPayload, mRxTimestamp);
511 
512     hostIntfTxPacket(mRxCmd->reason, respLen, mTxRetrans.seq, hostIntfTxPayloadDone);
513 }
514 
hostIntfTxPayloadDone(size_t tx,int err)515 static void hostIntfTxPayloadDone(size_t tx, int err)
516 {
517     bool done = hostIntfTxPacketDone(err, tx, hostIntfTxPayloadDone);
518 
519     if (err)
520         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_SEND, __func__);
521 
522     if (done) {
523         if (atomicReadByte(&mRestartRx))
524             hostIntfTxComplete(true, true);
525         else
526             hostIntfTxComplete(true, false);
527     }
528 }
529 
hostIntfRelease()530 static void hostIntfRelease()
531 {
532     mComm->release();
533 }
534 
resetBuffer(struct ActiveSensor * sensor)535 static void resetBuffer(struct ActiveSensor *sensor)
536 {
537     sensor->discard = true;
538     sensor->buffer.length = 0;
539     memset(&sensor->buffer.firstSample, 0x00, sizeof(struct SensorFirstSample));
540 }
541 
hostIntfSetBusy(bool busy)542 void hostIntfSetBusy(bool busy)
543 {
544     mBusy = busy;
545 }
546 
getActiveSensorByType(uint32_t sensorType)547 static inline struct ActiveSensor *getActiveSensorByType(uint32_t sensorType)
548 {
549     struct ActiveSensor *sensor = NULL;
550 
551     if (sensorType > SENS_TYPE_INVALID && sensorType <= SENS_TYPE_LAST_USER &&
552         mSensorList[sensorType - 1] < MAX_REGISTERED_SENSORS)
553         sensor = mActiveSensorTable + mSensorList[sensorType - 1];
554 
555     return sensor;
556 }
557 
hostIntfPacketDequeue(void * data,uint32_t * wakeup,uint32_t * nonwakeup)558 bool hostIntfPacketDequeue(void *data, uint32_t *wakeup, uint32_t *nonwakeup)
559 {
560     struct HostIntfDataBuffer *buffer = data;
561     bool ret;
562     struct ActiveSensor *sensor;
563     uint32_t i;
564 
565     ret = simpleQueueDequeue(mOutputQ, buffer);
566     while (ret) {
567         sensor = getActiveSensorByType(buffer->sensType);
568         if (sensor) {
569             // do not sent sensor data if sensor is not requested; only maintain stats
570             if (sensor->sensorHandle == 0 && !buffer->firstSample.biasPresent && !buffer->firstSample.numFlushes) {
571                 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
572                     mWakeupBlocks--;
573                 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
574                     mNonWakeupBlocks--;
575                 sensor->curSamples -= buffer->firstSample.numSamples;
576                 ret = simpleQueueDequeue(mOutputQ, buffer);
577             } else {
578                 break;
579             }
580         } else {
581             break;
582         }
583     }
584 
585     if (!ret) {
586         // nothing in queue. look for partial buffers to flush
587         for (i = 0; i < mNumSensors; i++, mLastSensor = (mLastSensor + 1) % mNumSensors) {
588             sensor = mActiveSensorTable + mLastSensor;
589 
590             if (sensor->curSamples != sensor->buffer.firstSample.numSamples) {
591                 osLog(LOG_ERROR, "hostIntfPacketDequeue: sensor(%d)->curSamples=%d != buffer->numSamples=%d\n", sensor->buffer.sensType, sensor->curSamples, sensor->buffer.firstSample.numSamples);
592                 sensor->curSamples = sensor->buffer.firstSample.numSamples;
593             }
594 
595             if (sensor->buffer.length > 0) {
596                 memcpy(buffer, &sensor->buffer, sizeof(struct HostIntfDataBuffer));
597                 resetBuffer(sensor);
598                 ret = true;
599                 mLastSensor = (mLastSensor + 1) % mNumSensors;
600                 break;
601             }
602         }
603     }
604 
605     if (ret) {
606         sensor = getActiveSensorByType(buffer->sensType);
607         if (sensor) {
608             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
609                 mWakeupBlocks--;
610             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
611                 mNonWakeupBlocks--;
612             sensor->curSamples -= buffer->firstSample.numSamples;
613             sensor->firstTime = 0ull;
614         } else {
615             if (buffer->interrupt == NANOHUB_INT_WAKEUP)
616                 mWakeupBlocks--;
617             else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
618                 mNonWakeupBlocks--;
619         }
620     }
621 
622     *wakeup = mWakeupBlocks;
623     *nonwakeup = mNonWakeupBlocks;
624 
625     return ret;
626 }
627 
initCompleteCallback(uint32_t timerId,void * data)628 static void initCompleteCallback(uint32_t timerId, void *data)
629 {
630     osEnqueuePrivateEvt(EVT_APP_START, NULL, NULL, mHostIntfTid);
631 }
632 
queueDiscard(void * data,bool onDelete)633 static bool queueDiscard(void *data, bool onDelete)
634 {
635     struct HostIntfDataBuffer *buffer = data;
636     struct ActiveSensor *sensor = getActiveSensorByType(buffer->sensType);
637 
638     if (sensor) {
639         if (sensor->curSamples - buffer->firstSample.numSamples >= sensor->minSamples || onDelete) {
640             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
641                 mWakeupBlocks--;
642             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
643                 mNonWakeupBlocks--;
644             sensor->curSamples -= buffer->firstSample.numSamples;
645 
646             return true;
647         } else {
648             return false;
649         }
650     } else {
651         if (buffer->interrupt == NANOHUB_INT_WAKEUP)
652             mWakeupBlocks--;
653         else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
654             mNonWakeupBlocks--;
655         return true;
656     }
657 }
658 
latencyTimerCallback(uint32_t timerId,void * data)659 static void latencyTimerCallback(uint32_t timerId, void* data)
660 {
661     osEnqueuePrivateEvt(EVT_LATENCY_TIMER, data, NULL, mHostIntfTid);
662 }
663 
initSensors()664 static bool initSensors()
665 {
666     uint32_t i, j, blocks, maxBlocks, numAxis, packetSamples;
667     bool present, error;
668     const struct SensorInfo *si;
669     uint32_t handle;
670     static uint8_t errorCnt = 0;
671     uint32_t totalBlocks = 0;
672     uint8_t numSensors = 0;
673     ATOMIC_BITSET_DECL(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID,);
674 
675     atomicBitsetInit(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID);
676 
677     for (i = SENS_TYPE_INVALID + 1; i <= SENS_TYPE_LAST_USER; i++) {
678         for (j = 0, present = 0, error = 0; (si = sensorFind(i, j, &handle)) != NULL; j++) {
679             if (!sensorGetInitComplete(handle)) {
680                 if (errorCnt >= SENSOR_INIT_ERROR_MAX) {
681                     osLog(LOG_ERROR, "initSensors: %s not ready - skipping!\n", si->sensorName);
682                     continue;
683                 } else {
684                     osLog(LOG_INFO, "initSensors: %s not ready!\n", si->sensorName);
685                     timTimerSet(SENSOR_INIT_DELAY, 0, 50, initCompleteCallback, NULL, true);
686                     errorCnt ++;
687                     return false;
688                 }
689             } else if (!(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
690                 if (!present) {
691                     present = 1;
692                     numAxis = si->numAxis;
693                     switch (si->numAxis) {
694                     case NUM_AXIS_EMBEDDED:
695                     case NUM_AXIS_ONE:
696                         packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
697                         break;
698                     case NUM_AXIS_THREE:
699                         if (si->flags1 & SENSOR_INFO_FLAGS1_RAW)
700                             packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
701                         else
702                             packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
703                         break;
704                     default:
705                         packetSamples = 1;
706                         error = true;
707                     }
708                     if (si->minSamples > MAX_MIN_SAMPLES)
709                         maxBlocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
710                     else
711                         maxBlocks = (si->minSamples + packetSamples - 1) / packetSamples;
712                 } else {
713                     if (si->numAxis != numAxis) {
714                         error = true;
715                     } else {
716                         if (si->minSamples > MAX_MIN_SAMPLES)
717                             blocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
718                         else
719                             blocks = (si->minSamples + packetSamples - 1) / packetSamples;
720 
721                         maxBlocks = maxBlocks > blocks ? maxBlocks : blocks;
722                     }
723                 }
724             }
725         }
726 
727         if (present && !error) {
728             atomicBitsetSetBit(sensorPresent, i - 1);
729             numSensors++;
730             totalBlocks += maxBlocks;
731         }
732     }
733 
734     if (totalBlocks > MAX_NUM_BLOCKS) {
735         osLog(LOG_INFO, "initSensors: totalBlocks of %ld exceeds maximum of %d\n", totalBlocks, MAX_NUM_BLOCKS);
736         totalBlocks = MAX_NUM_BLOCKS;
737     } else if (totalBlocks < MIN_NUM_BLOCKS) {
738         totalBlocks = MIN_NUM_BLOCKS;
739     }
740 
741     mOutputQ = simpleQueueAlloc(totalBlocks, sizeof(struct HostIntfDataBuffer), queueDiscard);
742     if (!mOutputQ) {
743         osLog(LOG_ERROR, "initSensors: failed to allocate data buffer queue!\n");
744         return false;
745     }
746 
747     mActiveSensorTable = heapAlloc(numSensors * sizeof(struct ActiveSensor));
748     if (!mActiveSensorTable) {
749         osLog(LOG_ERROR, "initSensors: failed to allocate active sensor table!\n");
750         simpleQueueDestroy(mOutputQ);
751         return false;
752     }
753 
754     memset(mActiveSensorTable, 0x00, numSensors * sizeof(struct ActiveSensor));
755 
756     for (i = SENS_TYPE_INVALID; i < SENS_TYPE_LAST_USER; i++) {
757         mSensorList[i] = MAX_REGISTERED_SENSORS;
758     }
759 
760     for (i = SENS_TYPE_INVALID + 1, j = 0; i <= SENS_TYPE_LAST_USER && j < numSensors; i++) {
761         if (atomicBitsetGetBit(sensorPresent, i - 1)
762             && (si = sensorFind(i, 0, &handle)) != NULL
763             && !(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
764             mSensorList[i - 1] = j;
765             resetBuffer(mActiveSensorTable + j);
766             mActiveSensorTable[j].buffer.sensType = i;
767             mActiveSensorTable[j].biasReportType = 0;
768             mActiveSensorTable[j].rate = 0;
769             mActiveSensorTable[j].latency = 0;
770             mActiveSensorTable[j].numAxis = si->numAxis;
771             mActiveSensorTable[j].interrupt = si->interrupt;
772             if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) {
773                 mSensorList[si->rawType - 1] = j;
774                 mActiveSensorTable[j].buffer.sensType = si->rawType;
775                 mActiveSensorTable[j].raw = true;
776                 mActiveSensorTable[j].rawScale = si->rawScale;
777             }
778             if (si->flags1 & SENSOR_INFO_FLAGS1_BIAS) {
779                 mSensorList[si->biasType - 1] = j;
780                 mActiveSensorTable[j].biasReportType = i;
781                 osEventSubscribe(mHostIntfTid, sensorGetMyEventType(si->biasType));
782             }
783             if (si->minSamples > MAX_MIN_SAMPLES) {
784                 mActiveSensorTable[j].minSamples = MAX_MIN_SAMPLES;
785                 osLog(LOG_INFO, "initSensors: %s: minSamples of %d exceeded max of %d\n", si->sensorName, si->minSamples, MAX_MIN_SAMPLES);
786             } else {
787                 mActiveSensorTable[j].minSamples = si->minSamples;
788             }
789             mActiveSensorTable[j].curSamples = 0;
790             mActiveSensorTable[j].oneshot = false;
791             mActiveSensorTable[j].firstTime = 0ull;
792             switch (si->numAxis) {
793             case NUM_AXIS_EMBEDDED:
794             case NUM_AXIS_ONE:
795                 mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
796                 break;
797             case NUM_AXIS_THREE:
798                 if (mActiveSensorTable[j].raw)
799                     mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
800                 else
801                     mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
802                 break;
803             }
804             j++;
805         }
806     }
807 
808     mTotalBlocks = totalBlocks;
809     mNumSensors = numSensors;
810 
811     return true;
812 }
813 
floatToInt16(float val)814 static inline int16_t floatToInt16(float val)
815 {
816     if (val < (INT16_MIN + 0.5f))
817         return INT16_MIN;
818     else if (val > (INT16_MAX - 0.5f))
819         return INT16_MAX;
820     else if (val >= 0.0f)
821         return val + 0.5f;
822     else
823         return val - 0.5f;
824 }
825 
encodeDeltaTime(uint64_t time)826 static uint32_t encodeDeltaTime(uint64_t time)
827 {
828     uint32_t deltaTime;
829 
830     if (time <= UINT32_MAX) {
831         deltaTime = time | delta_time_fine_mask;
832     } else {
833         deltaTime = ((time + delta_time_rounding) >> delta_time_multiplier_order) & delta_time_coarse_mask;
834     }
835     return deltaTime;
836 }
837 
enqueueSensorBuffer(struct ActiveSensor * sensor)838 static bool enqueueSensorBuffer(struct ActiveSensor *sensor)
839 {
840     bool queued = simpleQueueEnqueue(mOutputQ, &sensor->buffer,
841                                      sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
842 
843     if (!queued) {
844         // undo counters if failed to add buffer
845         if (sensor->interrupt == NANOHUB_INT_WAKEUP)
846             mWakeupBlocks--;
847         else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
848             mNonWakeupBlocks--;
849         sensor->curSamples -= sensor->buffer.firstSample.numSamples;
850     }
851     resetBuffer(sensor);
852     return queued;
853 }
854 
copySingleSamples(struct ActiveSensor * sensor,const struct SingleAxisDataEvent * single)855 static void copySingleSamples(struct ActiveSensor *sensor, const struct SingleAxisDataEvent *single)
856 {
857     int i;
858     uint32_t deltaTime;
859     uint8_t numSamples;
860     uint8_t evtNumSamples = single->samples[0].firstSample.numSamples;
861 
862     for (i = 0; i < evtNumSamples; i++) {
863         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
864             enqueueSensorBuffer(sensor);
865 
866         if (sensor->buffer.firstSample.numSamples == 0) {
867             if (i == 0) {
868                 sensor->lastTime = sensor->buffer.referenceTime = single->referenceTime;
869             } else {
870                 sensor->lastTime += single->samples[i].deltaTime;
871                 sensor->buffer.referenceTime = sensor->lastTime;
872             }
873             sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
874             sensor->buffer.single[0].idata = single->samples[i].idata;
875             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
876                 mWakeupBlocks++;
877             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
878                 mNonWakeupBlocks++;
879             sensor->buffer.firstSample.numSamples = 1;
880             sensor->buffer.firstSample.interrupt = sensor->interrupt;
881             if (sensor->curSamples++ == 0)
882                 sensor->firstTime = sensor->buffer.referenceTime;
883         } else {
884             if (i == 0) {
885                 if (sensor->lastTime > single->referenceTime) {
886                     // shouldn't happen. flush current packet
887                     enqueueSensorBuffer(sensor);
888                     i--;
889                 } else if (single->referenceTime - sensor->lastTime >= delta_time_max) {
890                     enqueueSensorBuffer(sensor);
891                     i--;
892                 } else {
893                     deltaTime = encodeDeltaTime(single->referenceTime - sensor->lastTime);
894                     numSamples = sensor->buffer.firstSample.numSamples;
895 
896                     sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
897                     sensor->buffer.single[numSamples].deltaTime = deltaTime;
898                     sensor->buffer.single[numSamples].idata = single->samples[0].idata;
899                     sensor->lastTime = single->referenceTime;
900                     sensor->buffer.firstSample.numSamples++;
901                     sensor->curSamples++;
902                 }
903             } else {
904                 deltaTime = single->samples[i].deltaTime;
905                 numSamples = sensor->buffer.firstSample.numSamples;
906 
907                 sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
908                 sensor->buffer.single[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
909                 sensor->buffer.single[numSamples].idata = single->samples[i].idata;
910                 sensor->lastTime += deltaTime;
911                 sensor->buffer.firstSample.numSamples++;
912                 sensor->curSamples++;
913             }
914         }
915     }
916 }
917 
copyTripleSamples(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)918 static void copyTripleSamples(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
919 {
920     int i;
921     uint32_t deltaTime;
922     uint8_t numSamples;
923 
924     for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
925         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
926             enqueueSensorBuffer(sensor);
927 
928         if (sensor->buffer.firstSample.numSamples == 0) {
929             if (i == 0) {
930                 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
931             } else {
932                 sensor->lastTime += triple->samples[i].deltaTime;
933                 sensor->buffer.referenceTime = sensor->lastTime;
934             }
935             sensor->buffer.length = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
936             sensor->buffer.triple[0].ix = triple->samples[i].ix;
937             sensor->buffer.triple[0].iy = triple->samples[i].iy;
938             sensor->buffer.triple[0].iz = triple->samples[i].iz;
939             if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
940                 sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
941                 sensor->buffer.firstSample.biasPresent = 1;
942                 sensor->buffer.firstSample.biasSample = 0;
943                 sensor->discard = false;
944             }
945             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
946                 mWakeupBlocks++;
947             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
948                 mNonWakeupBlocks++;
949             sensor->buffer.firstSample.numSamples = 1;
950             sensor->buffer.firstSample.interrupt = sensor->interrupt;
951             if (sensor->curSamples++ == 0)
952                 sensor->firstTime = sensor->buffer.referenceTime;
953         } else {
954             if (i == 0) {
955                 if (sensor->lastTime > triple->referenceTime) {
956                     // shouldn't happen. flush current packet
957                     enqueueSensorBuffer(sensor);
958                     i--;
959                 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
960                     enqueueSensorBuffer(sensor);
961                     i--;
962                 } else {
963                     deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
964                     numSamples = sensor->buffer.firstSample.numSamples;
965 
966                     sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
967                     sensor->buffer.triple[numSamples].deltaTime = deltaTime;
968                     sensor->buffer.triple[numSamples].ix = triple->samples[0].ix;
969                     sensor->buffer.triple[numSamples].iy = triple->samples[0].iy;
970                     sensor->buffer.triple[numSamples].iz = triple->samples[0].iz;
971                     sensor->lastTime = triple->referenceTime;
972                     if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == 0) {
973                         sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
974                         sensor->buffer.firstSample.biasPresent = 1;
975                         sensor->buffer.firstSample.biasSample = numSamples;
976                         sensor->discard = false;
977                     }
978                     sensor->buffer.firstSample.numSamples++;
979                     sensor->curSamples++;
980                 }
981             } else {
982                 deltaTime = triple->samples[i].deltaTime;
983                 numSamples = sensor->buffer.firstSample.numSamples;
984 
985                 sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
986                 sensor->buffer.triple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
987                 sensor->buffer.triple[numSamples].ix = triple->samples[i].ix;
988                 sensor->buffer.triple[numSamples].iy = triple->samples[i].iy;
989                 sensor->buffer.triple[numSamples].iz = triple->samples[i].iz;
990                 sensor->lastTime += deltaTime;
991                 if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
992                     sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
993                     sensor->buffer.firstSample.biasPresent = 1;
994                     sensor->buffer.firstSample.biasSample = numSamples;
995                     sensor->discard = false;
996                 }
997                 sensor->buffer.firstSample.numSamples++;
998                 sensor->curSamples++;
999             }
1000         }
1001     }
1002 }
1003 
copyTripleSamplesBias(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)1004 static void copyTripleSamplesBias(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
1005 {
1006     uint8_t sensType = sensor->buffer.sensType;
1007 
1008     if (sensType == sensor->biasReportType) {
1009         copyTripleSamples(sensor, triple);
1010     } else {
1011         // Bias needs to be sent with a different sensType, so enqueue any pending buffer, enqueue
1012         // bias with a different sensor type, then restore the sensType
1013         if (sensor->buffer.firstSample.numSamples > 0)
1014             enqueueSensorBuffer(sensor);
1015         sensor->buffer.sensType = sensor->biasReportType;
1016         copyTripleSamples(sensor, triple);
1017         if (sensor->buffer.firstSample.numSamples > 0)
1018             enqueueSensorBuffer(sensor);
1019         sensor->buffer.sensType = sensType;
1020     }
1021 }
1022 
copyTripleSamplesRaw(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)1023 static void copyTripleSamplesRaw(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
1024 {
1025     int i;
1026     uint32_t deltaTime;
1027     uint8_t numSamples;
1028 
1029     // Bias not supported in raw format; treat as regular format triple samples (potentially
1030     // handling alternate bias report type)
1031     if (triple->samples[0].firstSample.biasPresent) {
1032         copyTripleSamplesBias(sensor, triple);
1033         return;
1034     }
1035 
1036     for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
1037         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
1038             enqueueSensorBuffer(sensor);
1039 
1040         if (sensor->buffer.firstSample.numSamples == 0) {
1041             if (i == 0) {
1042                 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
1043             } else {
1044                 sensor->lastTime += triple->samples[i].deltaTime;
1045                 sensor->buffer.referenceTime = sensor->lastTime;
1046             }
1047             sensor->buffer.length = sizeof(struct RawTripleAxisDataEvent) + sizeof(struct RawTripleAxisDataPoint);
1048             sensor->buffer.rawTriple[0].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
1049             sensor->buffer.rawTriple[0].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
1050             sensor->buffer.rawTriple[0].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
1051             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1052                 mWakeupBlocks++;
1053             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1054                 mNonWakeupBlocks++;
1055             sensor->buffer.firstSample.numSamples = 1;
1056             sensor->buffer.firstSample.interrupt = sensor->interrupt;
1057             if (sensor->curSamples++ == 0)
1058                 sensor->firstTime = sensor->buffer.referenceTime;
1059         } else {
1060             if (i == 0) {
1061                 if (sensor->lastTime > triple->referenceTime) {
1062                     // shouldn't happen. flush current packet
1063                     enqueueSensorBuffer(sensor);
1064                     i--;
1065                 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
1066                     enqueueSensorBuffer(sensor);
1067                     i--;
1068                 } else {
1069                     deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
1070                     numSamples = sensor->buffer.firstSample.numSamples;
1071 
1072                     sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
1073                     sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime;
1074                     sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[0].x * sensor->rawScale);
1075                     sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[0].y * sensor->rawScale);
1076                     sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[0].z * sensor->rawScale);
1077                     sensor->lastTime = triple->referenceTime;
1078                     sensor->buffer.firstSample.numSamples++;
1079                     sensor->curSamples++;
1080                 }
1081             } else {
1082                 deltaTime = triple->samples[i].deltaTime;
1083                 numSamples = sensor->buffer.firstSample.numSamples;
1084 
1085                 sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
1086                 sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
1087                 sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
1088                 sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
1089                 sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
1090                 sensor->lastTime += deltaTime;
1091                 sensor->buffer.firstSample.numSamples++;
1092                 sensor->curSamples++;
1093             }
1094         }
1095     }
1096 }
1097 
hostIntfAddBlock(struct HostIntfDataBuffer * data,bool discardable,bool interrupt)1098 static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable, bool interrupt)
1099 {
1100     if (!simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, discardable))
1101         return;
1102 
1103     if (data->interrupt == NANOHUB_INT_WAKEUP)
1104         mWakeupBlocks++;
1105     else if (data->interrupt == NANOHUB_INT_NONWAKEUP)
1106         mNonWakeupBlocks++;
1107     nanohubPrefetchTx(interrupt ? data->interrupt : HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
1108 }
1109 
hostIntfNotifyReboot(uint32_t reason)1110 static void hostIntfNotifyReboot(uint32_t reason)
1111 {
1112     __le32 raw_reason = htole32(reason);
1113 
1114     struct NanohubHalSysMgmtTx *resp;
1115     resp = heapAlloc(sizeof(*resp));
1116     if (resp) {
1117         resp->hdr = (struct NanohubHalHdr) {
1118             .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
1119             .len = sizeof(*resp) - sizeof(resp->hdr),
1120         };
1121         resp->ret = (struct NanohubHalRet) {
1122             .msg = NANOHUB_HAL_SYS_MGMT,
1123             .status = raw_reason,
1124         };
1125         resp->cmd = NANOHUB_HAL_SYS_MGMT_REBOOT;
1126         osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
1127     }
1128 
1129 #ifdef LEGACY_HAL_ENABLED
1130     struct NanohubHalLegacyRebootTx *respLegacy;
1131     respLegacy = heapAlloc(sizeof(*respLegacy));
1132     if (respLegacy) {
1133         respLegacy->hdr = (struct NanohubHalLegacyHdr) {
1134             .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
1135             .len = sizeof(*respLegacy) - sizeof(respLegacy->hdr) + sizeof(respLegacy->hdr.msg),
1136             .msg = NANOHUB_HAL_LEGACY_REBOOT,
1137         };
1138         memcpy(&respLegacy->reason, &raw_reason, sizeof(respLegacy->reason));
1139         osEnqueueEvtOrFree(EVT_APP_TO_HOST, respLegacy, heapFree);
1140     }
1141 #endif
1142 }
1143 
queueFlush(struct ActiveSensor * sensor)1144 static void queueFlush(struct ActiveSensor *sensor)
1145 {
1146     if (sensor->buffer.length == 0) {
1147         sensor->buffer.length = sizeof(sensor->buffer.referenceTime) + sizeof(struct SensorFirstSample);
1148         sensor->buffer.referenceTime = 0ull;
1149         if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1150             mWakeupBlocks++;
1151         else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1152             mNonWakeupBlocks++;
1153         sensor->buffer.firstSample.numFlushes = 1;
1154     } else {
1155         sensor->buffer.firstSample.numFlushes++;
1156     }
1157     sensor->discard = false;
1158     hostIntfSetInterrupt(sensor->interrupt);
1159 }
1160 
fakeFlush(struct ConfigCmd * cmd)1161 static void fakeFlush(struct ConfigCmd *cmd)
1162 {
1163     struct HostIntfDataBuffer *buffer;
1164     uint8_t size = sizeof(buffer->evtType) + sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1165     buffer = alloca(size);
1166     memset(buffer, 0x00, size);
1167 
1168     buffer->sensType = cmd->sensType;
1169     buffer->length = sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1170     buffer->interrupt = NANOHUB_INT_WAKEUP;
1171     mWakeupBlocks++;
1172     buffer->firstSample.numFlushes = 1;
1173     if (!simpleQueueEnqueue(mOutputQ, buffer, size, false))
1174         mWakeupBlocks--;
1175 }
1176 
onEvtAppStart(const void * evtData)1177 static void onEvtAppStart(const void *evtData)
1178 {
1179     if (initSensors()) {
1180         uint32_t reason;
1181         struct HostIntfDataBuffer *data;
1182 
1183         osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
1184         osEventsSubscribe(4, EVT_NO_SENSOR_CONFIG_EVENT,
1185                              EVT_APP_TO_SENSOR_HAL_DATA,
1186                              EVT_APP_TO_HOST,
1187                              EVT_APP_TO_HOST_CHRE);
1188 #ifdef DEBUG_LOG_EVT
1189         osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
1190         platEarlyLogFlush();
1191 #endif
1192         reason = pwrResetReason();
1193         data = alloca(sizeof(uint32_t) + sizeof(reason));
1194         data->sensType = SENS_TYPE_INVALID;
1195         data->length = sizeof(reason);
1196         data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON;
1197         data->interrupt = NANOHUB_INT_WAKEUP;
1198         memcpy(data->buffer, &reason, sizeof(reason));
1199         hostIntfAddBlock(data, false, true);
1200         hostIntfNotifyReboot(reason);
1201     }
1202 }
1203 
onEvtAppToHost(const void * evtData)1204 static void onEvtAppToHost(const void *evtData)
1205 {
1206     const struct HostHubRawPacket *hostMsg = evtData;
1207 
1208     if (hostMsg->dataLen <= HOST_HUB_RAW_PACKET_MAX_LEN) {
1209         struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->dataLen);
1210 
1211         data->sensType = SENS_TYPE_INVALID;
1212         data->length = sizeof(*hostMsg) + hostMsg->dataLen;
1213         data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
1214         data->interrupt = NANOHUB_INT_WAKEUP;
1215         memcpy(data->buffer, evtData, data->length);
1216         hostIntfAddBlock(data, false, true);
1217     }
1218 }
1219 
onEvtAppToHostChre(const void * evtData)1220 static void onEvtAppToHostChre(const void *evtData)
1221 {
1222     const struct HostHubChrePacket *hostMsg = evtData;
1223 
1224     if (hostMsg->messageSize <= HOST_HUB_CHRE_PACKET_MAX_LEN) {
1225         struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->messageSize);
1226 
1227         data->sensType = SENS_TYPE_INVALID;
1228         data->length = sizeof(*hostMsg) + hostMsg->messageSize;
1229         data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
1230         data->interrupt = NANOHUB_INT_WAKEUP;
1231         memcpy(data->buffer, evtData, data->length);
1232         hostIntfAddBlock(data, false, true);
1233     }
1234 }
1235 
1236 #ifdef LEGACY_HAL_ENABLED
handleLegacyHalCmd(const uint8_t * halData,uint8_t size)1237 static void handleLegacyHalCmd(const uint8_t *halData, uint8_t size)
1238 {
1239     const struct NanohubHalLegacyCommand *halCmd = nanohubHalLegacyFindCommand(halData[0]);
1240     if (halCmd)
1241         halCmd->handler((void *)&halData[1], size - 1);
1242 }
1243 
onEvtAppFromHost(const void * evtData)1244 static void onEvtAppFromHost(const void *evtData)
1245 {
1246     const uint8_t *halMsg = evtData;
1247     handleLegacyHalCmd(&halMsg[1], halMsg[0]);
1248 }
1249 #endif
1250 
onEvtAppFromHostChre(const void * evtData)1251 static void onEvtAppFromHostChre(const void *evtData)
1252 {
1253     const struct NanohubMsgChreHdr *halMsg = (const struct NanohubMsgChreHdr *)evtData;
1254     const struct NanohubHalCommand *halCmd;
1255     const uint8_t *halData = (const uint8_t *)(halMsg+1);
1256     uint8_t len;
1257     uint32_t transactionId;
1258 
1259     memcpy(&transactionId, &halMsg->appEvent, sizeof(halMsg->appEvent));
1260 
1261     if (halMsg->size >= 1) {
1262         len = halMsg->size - 1;
1263         halCmd = nanohubHalFindCommand(halData[0]);
1264         if (halCmd) {
1265             if (len >= halCmd->minDataLen && len <= halCmd->maxDataLen)
1266                 halCmd->handler((void *)&halData[1], len, transactionId);
1267             return;
1268         }
1269     }
1270 #ifdef LEGACY_HAL_ENABLED
1271     handleLegacyHalCmd(halData, halMsg->size);
1272 #endif
1273 }
1274 
1275 #ifdef DEBUG_LOG_EVT
onEvtDebugLog(const void * evtData)1276 static void onEvtDebugLog(const void *evtData)
1277 {
1278     struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;
1279 
1280     if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG)
1281         hostIntfAddBlock(data, true, true);
1282 }
1283 #endif
1284 
onEvtLatencyTimer(const void * evtData)1285 static void onEvtLatencyTimer(const void *evtData)
1286 {
1287     uint64_t sensorTime = sensorGetTime();
1288     uint32_t i, cnt;
1289 
1290     for (i = 0, cnt = 0; i < mNumSensors && cnt < mLatencyCnt; i++) {
1291         if (mActiveSensorTable[i].latency > 0) {
1292             cnt++;
1293             if (mActiveSensorTable[i].firstTime &&
1294                 sensorTime >= mActiveSensorTable[i].firstTime + mActiveSensorTable[i].latency) {
1295                 hostIntfSetInterrupt(mActiveSensorTable[i].interrupt);
1296             }
1297         }
1298     }
1299 }
1300 
onConfigCmdFlushOne(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1301 static void onConfigCmdFlushOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1302 {
1303     sensorFlush(sensor->sensorHandle);
1304 }
1305 
onConfigCmdEnableOne(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1306 static void onConfigCmdEnableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1307 {
1308     if (sensorRequestRateChange(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1309         sensor->rate = cmd->rate;
1310         if (sensor->latency != cmd->latency) {
1311             if (!sensor->latency) {
1312                 if (mLatencyCnt++ == 0)
1313                     mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1314             } else if (!cmd->latency) {
1315                 if (--mLatencyCnt == 0) {
1316                     timTimerCancel(mLatencyTimer);
1317                     mLatencyTimer = 0;
1318                 }
1319             }
1320             sensor->latency = cmd->latency;
1321         }
1322     }
1323 }
1324 
onConfigCmdEnableAll(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1325 static void onConfigCmdEnableAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1326 {
1327     for (uint32_t i = 0; sensorFind(cmd->sensType, i, &sensor->sensorHandle) != NULL; i++) {
1328         if (cmd->rate == SENSOR_RATE_ONESHOT) {
1329             cmd->rate = SENSOR_RATE_ONCHANGE;
1330             sensor->oneshot = true;
1331         } else {
1332             sensor->oneshot = false;
1333         }
1334 
1335         if (sensorRequest(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1336             if (cmd->latency) {
1337                 if (mLatencyCnt++ == 0)
1338                     mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1339             }
1340             sensor->rate = cmd->rate;
1341             sensor->latency = cmd->latency;
1342             osEventSubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1343             break;
1344         } else {
1345             sensor->sensorHandle = 0;
1346         }
1347     }
1348 }
1349 
onConfigCmdDisableOne(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1350 static void onConfigCmdDisableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1351 {
1352     sensorRelease(mHostIntfTid, sensor->sensorHandle);
1353     osEventUnsubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1354     if (sensor->latency) {
1355         if (--mLatencyCnt == 0) {
1356             timTimerCancel(mLatencyTimer);
1357             mLatencyTimer = 0;
1358         }
1359     }
1360     sensor->rate = 0;
1361     sensor->latency = 0;
1362     sensor->oneshot = false;
1363     sensor->sensorHandle = 0;
1364     if (sensor->buffer.length) {
1365         enqueueSensorBuffer(sensor);
1366         hostIntfSetInterrupt(sensor->interrupt);
1367     }
1368 }
1369 
onConfigCmdCalibrateAll(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1370 static void onConfigCmdCalibrateAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1371 {
1372     uint32_t tempSensorHandle;
1373     for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1374         sensorCalibrate(tempSensorHandle);
1375 }
1376 
onConfigCmdSelfTestAll(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1377 static void onConfigCmdSelfTestAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1378 {
1379     uint32_t tempSensorHandle;
1380     for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1381         sensorSelfTest(tempSensorHandle);
1382 }
1383 
onConfigCmdCfgDataAll(struct ActiveSensor * sensor,struct ConfigCmd * cmd)1384 static void onConfigCmdCfgDataAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
1385 {
1386     uint32_t tempSensorHandle;
1387     for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1388         sensorCfgData(tempSensorHandle, (void *)(cmd+1));
1389 }
1390 
onEvtNoSensorConfigEvent(const void * evtData)1391 static void onEvtNoSensorConfigEvent(const void *evtData)
1392 {
1393     struct ConfigCmd *cmd = (struct ConfigCmd *)evtData;
1394     struct ActiveSensor *sensor = getActiveSensorByType(cmd->sensType);
1395     if (sensor) {
1396         if (sensor->sensorHandle) {
1397             switch (cmd->cmd) {
1398             case CONFIG_CMD_FLUSH:
1399                 onConfigCmdFlushOne(sensor, cmd);
1400                 break;
1401             case CONFIG_CMD_ENABLE:
1402                 onConfigCmdEnableOne(sensor, cmd);
1403                 break;
1404             case CONFIG_CMD_DISABLE:
1405                 onConfigCmdDisableOne(sensor, cmd);
1406                 break;
1407             case CONFIG_CMD_CFG_DATA:
1408                 onConfigCmdCfgDataAll(sensor, cmd);
1409                 break;
1410             }
1411         } else {
1412             switch (cmd->cmd) {
1413             case CONFIG_CMD_ENABLE:
1414                 onConfigCmdEnableAll(sensor, cmd);
1415                 break;
1416             case CONFIG_CMD_CALIBRATE:
1417                 onConfigCmdCalibrateAll(sensor, cmd);
1418                 break;
1419             case CONFIG_CMD_SELF_TEST:
1420                 onConfigCmdSelfTestAll(sensor, cmd);
1421                 break;
1422             case CONFIG_CMD_CFG_DATA:
1423                 onConfigCmdCfgDataAll(sensor, cmd);
1424                 break;
1425             case CONFIG_CMD_FLUSH:
1426                 queueFlush(sensor);
1427                 break;
1428             }
1429         }
1430     } else if (cmd->cmd == CONFIG_CMD_FLUSH && cmd->sensType > SENS_TYPE_INVALID) {
1431         // if a flush event is for an unknown sensor, we just return a fake flush event.
1432         osLog(LOG_INFO, "Flush request from unrecognized sensor, returning a fake flush\n");
1433         fakeFlush(cmd);
1434     }
1435 }
1436 
onEvtAppToSensorHalData(const void * evtData)1437 static void onEvtAppToSensorHalData(const void *evtData)
1438 {
1439     struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;
1440     if (data->sensType == SENS_TYPE_INVALID
1441             && data->dataType == HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL) {
1442         struct AppToSensorHalDataBuffer *buffer = (struct AppToSensorHalDataBuffer *)data;
1443         hostIntfAddBlock(data, (buffer->payload.type & EVENT_TYPE_BIT_DISCARDABLE) != 0, false);
1444     }
1445 }
1446 
copyEmbeddedSamples(struct ActiveSensor * sensor,const void * evtData)1447 static void copyEmbeddedSamples(struct ActiveSensor *sensor, const void* evtData)
1448 {
1449     uint64_t sensorTime = sensorGetTime();
1450 
1451     if (sensor->buffer.length > 0 && sensorTime - sensor->lastTime >= delta_time_max)
1452         enqueueSensorBuffer(sensor);
1453 
1454     if (sensor->buffer.length == 0) {
1455         sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
1456         sensor->lastTime = sensor->buffer.referenceTime = sensorTime;
1457         if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1458             mWakeupBlocks++;
1459         else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1460             mNonWakeupBlocks++;
1461         sensor->buffer.firstSample.numSamples = 1;
1462         sensor->buffer.firstSample.interrupt = sensor->interrupt;
1463         sensor->buffer.single[0].idata = (uint32_t)evtData;
1464     } else {
1465         sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
1466         sensor->buffer.single[sensor->buffer.firstSample.numSamples].deltaTime =
1467                 encodeDeltaTime(sensorTime - sensor->lastTime);
1468         sensor->lastTime = sensorTime;
1469         sensor->buffer.single[sensor->buffer.firstSample.numSamples].idata = (uint32_t)evtData;
1470         sensor->buffer.firstSample.numSamples++;
1471     }
1472     if (sensor->curSamples++ == 0)
1473         sensor->firstTime = sensor->buffer.referenceTime;
1474 }
1475 
getSensorInterrupt(struct ActiveSensor * sensor)1476 static uint32_t getSensorInterrupt(struct ActiveSensor *sensor)
1477 {
1478     uint32_t interrupt = HOSTINTF_MAX_INTERRUPTS;
1479     uint64_t sensorTime = sensorGetTime();
1480 
1481     if (sensor->firstTime &&
1482         ((sensorTime >= sensor->firstTime + sensor->latency) ||
1483          ((sensor->latency > sensorGetCurLatency(sensor->sensorHandle)) &&
1484           (sensorTime + sensorGetCurLatency(sensor->sensorHandle) > sensor->firstTime + sensor->latency)))) {
1485         interrupt = sensor->interrupt;
1486     } else if (mWakeupBlocks + mNonWakeupBlocks >= mTotalBlocks) {
1487         interrupt = sensor->interrupt;
1488     }
1489 
1490     return interrupt;
1491 }
1492 
onEvtSensorDataActive(struct ActiveSensor * sensor,uint32_t evtType,const void * evtData)1493 static void onEvtSensorDataActive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData)
1494 {
1495     if (evtData == SENSOR_DATA_EVENT_FLUSH) {
1496         queueFlush(sensor);
1497     } else {
1498         bool haveFlush = sensor->buffer.firstSample.numFlushes > 0;
1499         if (sensor->buffer.length > 0 &&
1500             (haveFlush || sensor->buffer.firstSample.numSamples == sensor->packetSamples)) {
1501                 // processing will be aborted if we have pending flush and are not able to send
1502                 // in this case, send eventually will be retried, otherwise data will be lost
1503                 if (!enqueueSensorBuffer(sensor) && haveFlush)
1504                     return;
1505         }
1506 
1507         switch (sensor->numAxis) {
1508         case NUM_AXIS_EMBEDDED:
1509             copyEmbeddedSamples(sensor, evtData);
1510             break;
1511         case NUM_AXIS_ONE:
1512             copySingleSamples(sensor, evtData);
1513             break;
1514         case NUM_AXIS_THREE:
1515             if (sensor->raw)
1516                 copyTripleSamplesRaw(sensor, evtData);
1517             else
1518                 copyTripleSamples(sensor, evtData);
1519             break;
1520         default:
1521             return;
1522         }
1523     }
1524 
1525     nanohubPrefetchTx(getSensorInterrupt(sensor), mWakeupBlocks, mNonWakeupBlocks);
1526 
1527     if (sensor->oneshot) {
1528         sensorRelease(mHostIntfTid, sensor->sensorHandle);
1529         osEventUnsubscribe(mHostIntfTid, evtType);
1530         sensor->sensorHandle = 0;
1531         sensor->oneshot = false;
1532     }
1533 }
1534 
onEvtSensorDataInactive(struct ActiveSensor * sensor,uint32_t evtType,const void * evtData)1535 static void onEvtSensorDataInactive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData)
1536 {
1537     if (evtData != SENSOR_DATA_EVENT_FLUSH) {
1538         // handle bias data which can be generated for sensors that are
1539         // not currently requested by the AP
1540         switch (sensor->numAxis) {
1541         case NUM_AXIS_THREE:
1542             if (((const struct TripleAxisDataEvent *)evtData)->samples[0].firstSample.biasPresent) {
1543                 copyTripleSamplesBias(sensor, evtData);
1544                 nanohubPrefetchTx(HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
1545             }
1546             break;
1547         }
1548     }
1549 }
1550 
onEvtSensorData(uint32_t evtType,const void * evtData)1551 static void onEvtSensorData(uint32_t evtType, const void* evtData)
1552 {
1553     if (evtType > EVT_NO_FIRST_SENSOR_EVENT && evtType < EVT_NO_SENSOR_CONFIG_EVENT) {
1554         struct ActiveSensor *sensor = getActiveSensorByType(evtType & 0xFF);
1555         if (sensor) {
1556             if (sensor->sensorHandle)
1557                 onEvtSensorDataActive(sensor, evtType, evtData);
1558             else
1559                 onEvtSensorDataInactive(sensor, evtType, evtData);
1560         }
1561     }
1562 }
1563 
hostIntfHandleEvent(uint32_t evtType,const void * evtData)1564 static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
1565 {
1566     switch (EVENT_GET_EVENT(evtType)) {
1567     case EVT_APP_START:
1568         onEvtAppStart(evtData);
1569         break;
1570     case EVT_APP_TO_HOST:
1571         onEvtAppToHost(evtData);
1572         break;
1573     case EVT_APP_TO_HOST_CHRE:
1574         onEvtAppToHostChre(evtData);
1575         break;
1576 #ifdef LEGACY_HAL_ENABLED
1577     case EVT_APP_FROM_HOST:
1578         onEvtAppFromHost(evtData);
1579         break;
1580 #endif
1581     case EVT_APP_FROM_HOST_CHRE:
1582         onEvtAppFromHostChre(evtData);
1583         break;
1584 #ifdef DEBUG_LOG_EVT
1585     case EVT_DEBUG_LOG:
1586         onEvtDebugLog(evtData);
1587         break;
1588 #endif
1589     case EVT_LATENCY_TIMER:
1590         onEvtLatencyTimer(evtData);
1591         break;
1592     case EVT_NO_SENSOR_CONFIG_EVENT:
1593         onEvtNoSensorConfigEvent(evtData);
1594         break;
1595     case EVT_APP_TO_SENSOR_HAL_DATA:
1596         onEvtAppToSensorHalData(evtData);
1597         break;
1598     default:
1599         onEvtSensorData(EVENT_GET_EVENT(evtType), evtData);
1600         break;
1601     }
1602 }
1603 
hostIntfCopyInterrupts(void * dst,uint32_t numBits)1604 void hostIntfCopyInterrupts(void *dst, uint32_t numBits)
1605 {
1606     if (mInterrupt->numBits != numBits)
1607         return;
1608 
1609     atomicBitsetBulkRead(mInterrupt, dst, numBits);
1610 }
1611 
hostIntfClearInterrupts()1612 void hostIntfClearInterrupts()
1613 {
1614     uint32_t i;
1615 
1616     for (i = 0; i < HOSTINTF_MAX_INTERRUPTS; i++) {
1617         if (atomicBitsetGetBit(mInterrupt, i))
1618             hostIntfClearInterrupt(i);
1619     }
1620 }
1621 
hostIntfSetInterrupt(uint32_t bit)1622 void hostIntfSetInterrupt(uint32_t bit)
1623 {
1624     uint64_t state = cpuIntsOff();
1625     if (mHostIntfTid) {
1626         if (!atomicBitsetGetBit(mInterrupt, bit)) {
1627             atomicBitsetSetBit(mInterrupt, bit);
1628             if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1629                 if (mInterruptCntWkup++ == 0)
1630                     apIntSet(true);
1631             } else {
1632                 if (mInterruptCntNonWkup++ == 0)
1633                     apIntSet(false);
1634             }
1635         }
1636     }
1637     cpuIntsRestore(state);
1638 }
1639 
hostIntfGetInterrupt(uint32_t bit)1640 bool hostIntfGetInterrupt(uint32_t bit)
1641 {
1642     return atomicBitsetGetBit(mInterrupt, bit);
1643 }
1644 
hostIntfClearInterrupt(uint32_t bit)1645 void hostIntfClearInterrupt(uint32_t bit)
1646 {
1647     uint64_t state = cpuIntsOff();
1648     if (mHostIntfTid) {
1649         if (atomicBitsetGetBit(mInterrupt, bit)) {
1650             atomicBitsetClearBit(mInterrupt, bit);
1651             if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1652                 if (--mInterruptCntWkup == 0)
1653                     apIntClear(true);
1654             } else {
1655                 if (--mInterruptCntNonWkup == 0)
1656                     apIntClear(false);
1657             }
1658         }
1659     }
1660     cpuIntsRestore(state);
1661 }
1662 
hostIntfSetInterruptMask(uint32_t bit)1663 void hostIntfSetInterruptMask(uint32_t bit)
1664 {
1665     uint64_t state = cpuIntsOff();
1666     if (mHostIntfTid) {
1667         if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1668             atomicBitsetSetBit(mInterruptMask, bit);
1669             if (atomicBitsetGetBit(mInterrupt, bit)) {
1670                 if (--mInterruptCntWkup == 0)
1671                     apIntClear(true);
1672                 if (mInterruptCntNonWkup++ == 0)
1673                     apIntSet(false);
1674             }
1675         }
1676     }
1677     cpuIntsRestore(state);
1678 }
1679 
hostIntfGetInterruptMask(uint32_t bit)1680 bool hostIntfGetInterruptMask(uint32_t bit)
1681 {
1682     return atomicBitsetGetBit(mInterruptMask, bit);
1683 }
1684 
hostIntfClearInterruptMask(uint32_t bit)1685 void hostIntfClearInterruptMask(uint32_t bit)
1686 {
1687     uint64_t state = cpuIntsOff();
1688     if (mHostIntfTid) {
1689         if (atomicBitsetGetBit(mInterruptMask, bit)) {
1690             atomicBitsetClearBit(mInterruptMask, bit);
1691             if (atomicBitsetGetBit(mInterrupt, bit)) {
1692                 if (mInterruptCntWkup++ == 0)
1693                     apIntSet(true);
1694                 if (--mInterruptCntNonWkup == 0)
1695                     apIntClear(false);
1696             }
1697         }
1698     }
1699     cpuIntsRestore(state);
1700 }
1701 
1702 INTERNAL_CHRE_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
1703