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/inc/variant.h>
24 #include <eventnums.h>
25 
26 #include <plat/inc/pwr.h>
27 
28 #include <nanohub/crc.h>
29 
30 #include <platform.h>
31 #include <cpu.h>
32 #include <hostIntf.h>
33 #include <hostIntf_priv.h>
34 #include <nanohubCommand.h>
35 #include <nanohubPacket.h>
36 #include <seos.h>
37 #include <util.h>
38 #include <atomicBitset.h>
39 #include <atomic.h>
40 #include <gpio.h>
41 #include <apInt.h>
42 #include <sensors.h>
43 #include <timer.h>
44 #include <heap.h>
45 #include <simpleQ.h>
46 
47 #define HOSTINTF_MAX_ERR_MSG    8
48 #define MAX_NUM_BLOCKS          280         /* times 256 = 71680 bytes */
49 #define MIN_NUM_BLOCKS          10          /* times 256 = 2560 bytes */
50 #define SENSOR_INIT_DELAY       500000000   /* ns */
51 #define SENSOR_INIT_ERROR_MAX   4
52 #define CHECK_LATENCY_TIME      500000000   /* ns */
53 #define EVT_LATENCY_TIMER       EVT_NO_FIRST_USER_EVENT
54 
55 static const uint32_t delta_time_multiplier_order = 9;
56 static const uint32_t delta_time_coarse_mask = ~1;
57 static const uint32_t delta_time_fine_mask = 1;
58 static const uint32_t delta_time_rounding = 0x200;      /* 1ul << delta_time_multiplier_order */
59 static const uint64_t delta_time_max = 0x1FFFFFFFE00;   /* UINT32_MAX << delta_time_multiplier_order */
60 
61 enum ConfigCmds
62 {
63     CONFIG_CMD_DISABLE      = 0,
64     CONFIG_CMD_ENABLE       = 1,
65     CONFIG_CMD_FLUSH        = 2,
66     CONFIG_CMD_CFG_DATA     = 3,
67     CONFIG_CMD_CALIBRATE    = 4,
68 };
69 
70 struct ConfigCmd
71 {
72     uint64_t latency;
73     uint32_t rate;
74     uint8_t sensType;
75     uint8_t cmd;
76     uint16_t flags;
77 } __attribute__((packed));
78 
79 struct ActiveSensor
80 {
81     uint64_t latency;
82     uint64_t firstTime;
83     uint64_t lastTime;
84     struct HostIntfDataBuffer buffer;
85     uint32_t rate;
86     uint32_t sensorHandle;
87     uint16_t minSamples;
88     uint16_t curSamples;
89     uint8_t numAxis;
90     uint8_t interrupt;
91     uint8_t numSamples;
92     uint8_t packetSamples;
93     uint8_t oneshot : 1;
94     uint8_t discard : 1;
95     uint8_t raw : 1;
96     uint8_t reserved : 5;
97     float rawScale;
98 } __attribute__((packed));
99 
100 static uint8_t mSensorList[SENS_TYPE_LAST_USER];
101 static struct SimpleQueue *mOutputQ;
102 static struct ActiveSensor *mActiveSensorTable;
103 static uint8_t mNumSensors;
104 static uint8_t mLastSensor;
105 
106 static const struct HostIntfComm *mComm;
107 static bool mBusy;
108 static uint64_t mRxTimestamp;
109 static uint8_t mRxBuf[NANOHUB_PACKET_SIZE_MAX];
110 static size_t mRxSize;
111 static struct
112 {
113     const struct NanohubCommand *cmd;
114     uint32_t seq;
115     bool seqMatch;
116 } mTxRetrans;
117 static struct
118 {
119     uint8_t pad; // packet header is 10 bytes. + 2 to word align
120     uint8_t prePreamble;
121     uint8_t buf[NANOHUB_PACKET_SIZE_MAX];
122     uint8_t postPreamble;
123 } mTxBuf;
124 static size_t mTxSize;
125 static uint8_t *mTxBufPtr;
126 static const struct NanohubCommand *mRxCmd;
127 ATOMIC_BITSET_DECL(mInterrupt, HOSTINTF_MAX_INTERRUPTS, static);
128 ATOMIC_BITSET_DECL(mInterruptMask, HOSTINTF_MAX_INTERRUPTS, static);
129 static uint32_t mInterruptCntWkup, mInterruptCntNonWkup;
130 static uint32_t mWakeupBlocks, mNonWakeupBlocks, mTotalBlocks;
131 static uint32_t mHostIntfTid;
132 static uint32_t mLatencyTimer;
133 static uint8_t mLatencyCnt;
134 
135 static uint8_t mRxIdle;
136 static uint8_t mWakeActive;
137 static uint8_t mActiveWrite;
138 static uint8_t mRestartRx;
139 static uint8_t mIntErrMsgIdx;
140 static volatile uint32_t mIntErrMsgCnt;
141 
142 enum hostIntfIntErrReason
143 {
144     HOSTINTF_ERR_PKG_INCOMPELETE = 0,
145     HOSTINTF_ERR_PGK_SIZE,
146     HOSTINTF_ERR_PKG_PAYLOAD_SIZE,
147     HOSTINTF_ERR_PKG_CRC,
148     HOSTINTF_ERR_RECEIVE,
149     HOSTINTF_ERR_SEND,
150     HOSTINTF_ERR_ACK,
151     HOSTINTF_ERR_NACKED,
152     HOSTINTF_ERR_UNKNOWN
153 };
154 
155 struct hostIntfIntErrMsg
156 {
157     enum LogLevel level;
158     enum hostIntfIntErrReason reason;
159     const char *func;
160 };
161 static struct hostIntfIntErrMsg mIntErrMsg[HOSTINTF_MAX_ERR_MSG];
162 
163 static void hostIntfTxPacket(uint32_t reason, uint8_t len, uint32_t seq,
164         HostIntfCommCallbackF callback);
165 
166 static void hostIntfRxDone(size_t rx, int err);
167 static void hostIntfGenerateAck(void *cookie);
168 
169 static void hostIntfTxAckDone(size_t tx, int err);
170 static void hostIntfGenerateResponse(void *cookie);
171 
172 static void hostIntfTxPayloadDone(size_t tx, int err);
173 
hostIntfGetPayload(uint8_t * buf)174 static inline void *hostIntfGetPayload(uint8_t *buf)
175 {
176     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
177     return packet->data;
178 }
179 
hostIntfGetPayloadLen(uint8_t * buf)180 static inline uint8_t hostIntfGetPayloadLen(uint8_t *buf)
181 {
182     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
183     return packet->len;
184 }
185 
hostIntfGetFooter(uint8_t * buf)186 static inline struct NanohubPacketFooter *hostIntfGetFooter(uint8_t *buf)
187 {
188     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
189     return (struct NanohubPacketFooter *)(buf + sizeof(*packet) + packet->len);
190 }
191 
hostIntfComputeCrc(uint8_t * buf)192 static inline __le32 hostIntfComputeCrc(uint8_t *buf)
193 {
194     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
195     uint32_t crc = crc32(packet, packet->len + sizeof(*packet), CRC_INIT);
196     return htole32(crc);
197 }
198 
hostIntfPrintErrMsg(void * cookie)199 static void hostIntfPrintErrMsg(void *cookie)
200 {
201     struct hostIntfIntErrMsg *msg = (struct hostIntfIntErrMsg *)cookie;
202     osLog(msg->level, "%s failed with: %d\n", msg->func, msg->reason);
203     atomicAdd32bits(&mIntErrMsgCnt, -1UL);
204 }
205 
hostIntfDeferErrLog(enum LogLevel level,enum hostIntfIntErrReason reason,const char * func)206 static void hostIntfDeferErrLog(enum LogLevel level, enum hostIntfIntErrReason reason, const char *func)
207 {
208     // If the message buffer is full, we drop the newer messages.
209     if (atomicRead32bits(&mIntErrMsgCnt) == HOSTINTF_MAX_ERR_MSG)
210         return;
211 
212     mIntErrMsg[mIntErrMsgIdx].level = level;
213     mIntErrMsg[mIntErrMsgIdx].reason = reason;
214     mIntErrMsg[mIntErrMsgIdx].func = func;
215     if (osDefer(hostIntfPrintErrMsg, &mIntErrMsg[mIntErrMsgIdx], false)) {
216         atomicAdd32bits(&mIntErrMsgCnt, 1UL);
217         mIntErrMsgIdx = (mIntErrMsgIdx + 1) % HOSTINTF_MAX_ERR_MSG;
218     }
219 }
220 
hostIntfFindHandler(uint8_t * buf,size_t size,uint32_t * seq)221 static inline const struct NanohubCommand *hostIntfFindHandler(uint8_t *buf, size_t size, uint32_t *seq)
222 {
223     struct NanohubPacket *packet = (struct NanohubPacket *)buf;
224     struct NanohubPacketFooter *footer;
225     __le32 packetCrc;
226     uint32_t packetReason;
227     const struct NanohubCommand *cmd;
228 
229     if (size < NANOHUB_PACKET_SIZE(0)) {
230         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_INCOMPELETE, __func__);
231         return NULL;
232     }
233 
234     if (size != NANOHUB_PACKET_SIZE(packet->len)) {
235         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PGK_SIZE, __func__);
236         return NULL;
237     }
238 
239     footer = hostIntfGetFooter(buf);
240     packetCrc = hostIntfComputeCrc(buf);
241     if (footer->crc != packetCrc) {
242         hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_CRC, __func__);
243         return NULL;
244     }
245 
246     if (mTxRetrans.seq == packet->seq) {
247         mTxRetrans.seqMatch = true;
248         return mTxRetrans.cmd;
249     } else {
250         mTxRetrans.seqMatch = false;
251     }
252 
253     *seq = packet->seq;
254 
255     if (mBusy)
256         return NULL;
257 
258     packetReason = le32toh(packet->reason);
259 
260     if ((cmd = nanohubFindCommand(packetReason)) != NULL) {
261         if (packet->len < cmd->minDataLen || packet->len > cmd->maxDataLen) {
262             hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_PAYLOAD_SIZE, __func__);
263             return NULL;
264         }
265 
266         return cmd;
267     }
268 
269     hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_UNKNOWN, __func__);
270     return NULL;
271 }
272 
hostIntfTxBuf(int size,uint8_t * buf,HostIntfCommCallbackF callback)273 static void hostIntfTxBuf(int size, uint8_t *buf, HostIntfCommCallbackF callback)
274 {
275     mTxSize = size;
276     mTxBufPtr = buf;
277     mComm->txPacket(mTxBufPtr, mTxSize, callback);
278 }
279 
hostIntfTxPacket(__le32 reason,uint8_t len,uint32_t seq,HostIntfCommCallbackF callback)280 static void hostIntfTxPacket(__le32 reason, uint8_t len, uint32_t seq,
281         HostIntfCommCallbackF callback)
282 {
283     struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxBuf.buf);
284     txPacket->reason = reason;
285     txPacket->seq = seq;
286     txPacket->sync = NANOHUB_SYNC_BYTE;
287     txPacket->len = len;
288 
289     struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxBuf.buf);
290     txFooter->crc = hostIntfComputeCrc(mTxBuf.buf);
291 
292     // send starting with the prePremable byte
293     hostIntfTxBuf(1+NANOHUB_PACKET_SIZE(len), &mTxBuf.prePreamble, callback);
294 }
295 
hostIntfTxPacketDone(int err,size_t tx,HostIntfCommCallbackF callback)296 static inline bool hostIntfTxPacketDone(int err, size_t tx,
297         HostIntfCommCallbackF callback)
298 {
299     if (!err && tx < mTxSize) {
300         mTxSize -= tx;
301         mTxBufPtr += tx;
302 
303         mComm->txPacket(mTxBufPtr, mTxSize, callback);
304         return false;
305     }
306 
307     return true;
308 }
309 
hostIntfRequest(uint32_t tid)310 static bool hostIntfRequest(uint32_t tid)
311 {
312     mHostIntfTid = tid;
313     atomicBitsetInit(mInterrupt, HOSTINTF_MAX_INTERRUPTS);
314     atomicBitsetInit(mInterruptMask, HOSTINTF_MAX_INTERRUPTS);
315 #ifdef AP_INT_NONWAKEUP
316     hostIntfSetInterruptMask(NANOHUB_INT_NONWAKEUP);
317 #endif
318     mTxBuf.prePreamble = NANOHUB_PREAMBLE_BYTE;
319     mTxBuf.postPreamble = NANOHUB_PREAMBLE_BYTE;
320 
321     mComm = platHostIntfInit();
322     if (mComm) {
323         int err = mComm->request();
324         if (!err) {
325             nanohubInitCommand();
326             mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
327             osEventSubscribe(mHostIntfTid, EVT_APP_START);
328             return true;
329         }
330     }
331 
332     return false;
333 }
334 
hostIntfRxPacket(bool wakeupActive)335 void hostIntfRxPacket(bool wakeupActive)
336 {
337     if (mWakeActive) {
338         if (atomicXchgByte(&mRxIdle, false)) {
339             if (!wakeupActive)
340                 hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
341             mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
342             if (wakeupActive)
343                 hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
344         } else if (atomicReadByte(&mActiveWrite)) {
345             atomicWriteByte(&mRestartRx, true);
346         } else {
347             if (!wakeupActive)
348                 hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
349             else
350                 hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
351         }
352     } else if (wakeupActive && !atomicReadByte(&mActiveWrite))
353         hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
354 
355     mWakeActive = wakeupActive;
356 }
357 
hostIntfRxDone(size_t rx,int err)358 static void hostIntfRxDone(size_t rx, int err)
359 {
360     mRxTimestamp = rtcGetTime();
361     mRxSize = rx;
362 
363     if (err != 0) {
364         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_RECEIVE, __func__);
365         return;
366     }
367 
368     hostIntfGenerateAck(NULL);
369 }
370 
hostIntfTxSendAck(uint32_t resp)371 static void hostIntfTxSendAck(uint32_t resp)
372 {
373     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
374 
375     if (resp == NANOHUB_FAST_UNHANDLED_ACK) {
376         hostIntfCopyInterrupts(txPayload, HOSTINTF_MAX_INTERRUPTS);
377         hostIntfTxPacket(NANOHUB_REASON_ACK, 32, mTxRetrans.seq, hostIntfTxAckDone);
378     } else if (resp == NANOHUB_FAST_DONT_ACK) {
379         // do nothing. something else will do the ack
380     } else {
381         hostIntfTxPacket(mRxCmd->reason, resp, mTxRetrans.seq, hostIntfTxPayloadDone);
382     }
383 }
384 
hostIntfTxAck(void * buffer,uint8_t len)385 void hostIntfTxAck(void *buffer, uint8_t len)
386 {
387     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
388 
389     memcpy(txPayload, buffer, len);
390 
391     hostIntfTxSendAck(len);
392 }
393 
hostIntfGenerateAck(void * cookie)394 static void hostIntfGenerateAck(void *cookie)
395 {
396     uint32_t seq = 0;
397     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
398     void *rxPayload = hostIntfGetPayload(mRxBuf);
399     uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
400     uint32_t resp = NANOHUB_FAST_UNHANDLED_ACK;
401 
402     atomicWriteByte(&mActiveWrite, true);
403     hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
404     mRxCmd = hostIntfFindHandler(mRxBuf, mRxSize, &seq);
405 
406     if (mRxCmd) {
407         if (mTxRetrans.seqMatch) {
408             hostIntfTxBuf(mTxSize, &mTxBuf.prePreamble, hostIntfTxPayloadDone);
409         } else {
410             mTxRetrans.seq = seq;
411             mTxRetrans.cmd = mRxCmd;
412             if (mRxCmd->fastHandler)
413                 resp = mRxCmd->fastHandler(rxPayload, rx_len, txPayload, mRxTimestamp);
414 
415             hostIntfTxSendAck(resp);
416         }
417     } else {
418         if (mBusy)
419             hostIntfTxPacket(NANOHUB_REASON_NAK_BUSY, 0, seq, hostIntfTxAckDone);
420         else
421             hostIntfTxPacket(NANOHUB_REASON_NAK, 0, seq, hostIntfTxAckDone);
422     }
423 }
424 
425 
hostIntfTxComplete(bool restart)426 static void hostIntfTxComplete(bool restart)
427 {
428     hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
429     atomicWriteByte(&mActiveWrite, false);
430     atomicWriteByte(&mRestartRx, false);
431     if (restart) {
432         mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
433         hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
434     } else {
435         atomicWriteByte(&mRxIdle, true);
436     }
437 }
438 
hostIntfTxAckDone(size_t tx,int err)439 static void hostIntfTxAckDone(size_t tx, int err)
440 {
441     hostIntfTxPacketDone(err, tx, hostIntfTxAckDone);
442 
443     if (err) {
444         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_ACK, __func__);
445         hostIntfTxComplete(false);
446         return;
447     }
448 
449     if (!mRxCmd) {
450         if (!mBusy)
451             hostIntfDeferErrLog(LOG_DEBUG, HOSTINTF_ERR_NACKED, __func__);
452         if (atomicReadByte(&mRestartRx))
453             hostIntfTxComplete(true);
454         else
455             hostIntfTxComplete(false);
456         return;
457     } else if (atomicReadByte(&mRestartRx)) {
458         hostIntfTxComplete(true);
459     } else {
460         if (!osDefer(hostIntfGenerateResponse, NULL, true))
461             hostIntfTxComplete(false);
462     }
463 }
464 
hostIntfGenerateResponse(void * cookie)465 static void hostIntfGenerateResponse(void *cookie)
466 {
467     void *rxPayload = hostIntfGetPayload(mRxBuf);
468     uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
469     void *txPayload = hostIntfGetPayload(mTxBuf.buf);
470     uint8_t respLen = mRxCmd->handler(rxPayload, rx_len, txPayload, mRxTimestamp);
471 
472     hostIntfTxPacket(mRxCmd->reason, respLen, mTxRetrans.seq, hostIntfTxPayloadDone);
473 }
474 
hostIntfTxPayloadDone(size_t tx,int err)475 static void hostIntfTxPayloadDone(size_t tx, int err)
476 {
477     bool done = hostIntfTxPacketDone(err, tx, hostIntfTxPayloadDone);
478 
479     if (err)
480         hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_SEND, __func__);
481 
482     if (done) {
483         if (atomicReadByte(&mRestartRx))
484             hostIntfTxComplete(true);
485         else
486             hostIntfTxComplete(false);
487     }
488 }
489 
hostIntfRelease()490 static void hostIntfRelease()
491 {
492     mComm->release();
493 }
494 
resetBuffer(struct ActiveSensor * sensor)495 static void resetBuffer(struct ActiveSensor *sensor)
496 {
497     sensor->discard = true;
498     sensor->buffer.length = 0;
499     memset(&sensor->buffer.firstSample, 0x00, sizeof(struct SensorFirstSample));
500 }
501 
hostIntfSetBusy(bool busy)502 void hostIntfSetBusy(bool busy)
503 {
504     mBusy = busy;
505 }
506 
hostIntfPacketDequeue(void * data,uint32_t * wakeup,uint32_t * nonwakeup)507 bool hostIntfPacketDequeue(void *data, uint32_t *wakeup, uint32_t *nonwakeup)
508 {
509     struct HostIntfDataBuffer *buffer = data;
510     bool ret;
511     struct ActiveSensor *sensor;
512     uint32_t i, count = 0;
513 
514     ret = simpleQueueDequeue(mOutputQ, data);
515     while (ret) {
516         if (buffer->sensType > SENS_TYPE_INVALID && buffer->sensType <= SENS_TYPE_LAST_USER && mSensorList[buffer->sensType - 1] < MAX_REGISTERED_SENSORS) {
517             sensor = mActiveSensorTable + mSensorList[buffer->sensType - 1];
518             if (sensor->sensorHandle == 0 && !buffer->firstSample.biasPresent && !buffer->firstSample.numFlushes) {
519                 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
520                     mWakeupBlocks--;
521                 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
522                     mNonWakeupBlocks--;
523                 sensor->curSamples -= buffer->firstSample.numSamples;
524                 ret = simpleQueueDequeue(mOutputQ, data);
525                 count++;
526             } else {
527                 break;
528             }
529         } else {
530             break;
531         }
532     }
533 
534     if (!ret) {
535         // nothing in queue. look for partial buffers to flush
536         for (i = 0; i < mNumSensors; i++, mLastSensor = (mLastSensor + 1) % mNumSensors) {
537             if (mActiveSensorTable[mLastSensor].buffer.length > 0) {
538                 memcpy(data, &mActiveSensorTable[mLastSensor].buffer, sizeof(struct HostIntfDataBuffer));
539                 resetBuffer(mActiveSensorTable + mLastSensor);
540                 ret = true;
541                 mLastSensor = (mLastSensor + 1) % mNumSensors;
542                 break;
543             }
544         }
545     }
546 
547     if (ret) {
548         if (buffer->sensType > SENS_TYPE_INVALID && buffer->sensType <= SENS_TYPE_LAST_USER && mSensorList[buffer->sensType - 1] < MAX_REGISTERED_SENSORS) {
549             sensor = mActiveSensorTable + mSensorList[buffer->sensType - 1];
550             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
551                 mWakeupBlocks--;
552             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
553                 mNonWakeupBlocks--;
554             sensor->curSamples -= buffer->firstSample.numSamples;
555             sensor->firstTime = 0ull;
556         } else {
557             if (buffer->interrupt == NANOHUB_INT_WAKEUP)
558                 mWakeupBlocks--;
559             else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
560                 mNonWakeupBlocks--;
561         }
562     }
563 
564     *wakeup = mWakeupBlocks;
565     *nonwakeup = mNonWakeupBlocks;
566 
567     return ret;
568 }
569 
initCompleteCallback(uint32_t timerId,void * data)570 static void initCompleteCallback(uint32_t timerId, void *data)
571 {
572     osEnqueuePrivateEvt(EVT_APP_START, NULL, NULL, mHostIntfTid);
573 }
574 
queueDiscard(void * data,bool onDelete)575 static bool queueDiscard(void *data, bool onDelete)
576 {
577     struct HostIntfDataBuffer *buffer = data;
578     struct ActiveSensor *sensor;
579 
580     if (buffer->sensType > SENS_TYPE_INVALID && buffer->sensType <= SENS_TYPE_LAST_USER && mSensorList[buffer->sensType - 1] < MAX_REGISTERED_SENSORS) { // data
581         sensor = mActiveSensorTable + mSensorList[buffer->sensType - 1];
582 
583         if (sensor->curSamples - buffer->firstSample.numSamples >= sensor->minSamples || onDelete) {
584             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
585                 mWakeupBlocks--;
586             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
587                 mNonWakeupBlocks--;
588             sensor->curSamples -= buffer->firstSample.numSamples;
589 
590             return true;
591         } else {
592             return false;
593         }
594     } else {
595         if (buffer->interrupt == NANOHUB_INT_WAKEUP)
596             mWakeupBlocks--;
597         else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
598             mNonWakeupBlocks--;
599         return true;
600     }
601 }
602 
latencyTimerCallback(uint32_t timerId,void * data)603 static void latencyTimerCallback(uint32_t timerId, void* data)
604 {
605     osEnqueuePrivateEvt(EVT_LATENCY_TIMER, data, NULL, mHostIntfTid);
606 }
607 
initSensors()608 static bool initSensors()
609 {
610     uint32_t i, j, blocks, maxBlocks, numAxis, packetSamples;
611     bool present, error;
612     const struct SensorInfo *si;
613     uint32_t handle;
614     static uint8_t errorCnt = 0;
615 
616     mTotalBlocks = 0;
617     mNumSensors = 0;
618 
619     for (i = SENS_TYPE_INVALID + 1; i <= SENS_TYPE_LAST_USER; i++) {
620         for (j = 0, present = 0, error = 0; (si = sensorFind(i, j, &handle)) != NULL; j++) {
621             if (!sensorGetInitComplete(handle)) {
622                 if (errorCnt >= SENSOR_INIT_ERROR_MAX) {
623                     osLog(LOG_ERROR, "initSensors: %s not ready - skipping!\n", si->sensorName);
624                     continue;
625                 } else {
626                     osLog(LOG_INFO, "initSensors: %s not ready!\n", si->sensorName);
627                     timTimerSet(SENSOR_INIT_DELAY, 0, 50, initCompleteCallback, NULL, true);
628                     errorCnt ++;
629                     return false;
630                 }
631             } else if (!(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
632                 if (!present) {
633                     present = 1;
634                     numAxis = si->numAxis;
635                     switch (si->numAxis) {
636                     case NUM_AXIS_EMBEDDED:
637                     case NUM_AXIS_ONE:
638                         packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
639                         break;
640                     case NUM_AXIS_THREE:
641                         if (si->flags1 & SENSOR_INFO_FLAGS1_RAW)
642                             packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
643                         else
644                             packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
645                         break;
646                     default:
647                         packetSamples = 1;
648                         error = true;
649                     }
650                     if (si->minSamples > MAX_MIN_SAMPLES)
651                         maxBlocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
652                     else
653                         maxBlocks = (si->minSamples + packetSamples - 1) / packetSamples;
654                 } else {
655                     if (si->numAxis != numAxis) {
656                         error = true;
657                     } else {
658                         if (si->minSamples > MAX_MIN_SAMPLES)
659                             blocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
660                         else
661                             blocks = (si->minSamples + packetSamples - 1) / packetSamples;
662 
663                         maxBlocks = maxBlocks > blocks ? maxBlocks : blocks;
664                     }
665                 }
666             }
667         }
668 
669         if (present && !error) {
670             mNumSensors++;
671             mTotalBlocks += maxBlocks;
672         }
673     }
674 
675     if (mTotalBlocks > MAX_NUM_BLOCKS) {
676         osLog(LOG_INFO, "initSensors: mTotalBlocks of %ld exceeds maximum of %d\n", mTotalBlocks, MAX_NUM_BLOCKS);
677         mTotalBlocks = MAX_NUM_BLOCKS;
678     } else if (mTotalBlocks < MIN_NUM_BLOCKS) {
679         mTotalBlocks = MIN_NUM_BLOCKS;
680     }
681 
682     mOutputQ = simpleQueueAlloc(mTotalBlocks, sizeof(struct HostIntfDataBuffer), queueDiscard);
683     mActiveSensorTable = heapAlloc(mNumSensors * sizeof(struct ActiveSensor));
684     memset(mActiveSensorTable, 0x00, mNumSensors * sizeof(struct ActiveSensor));
685 
686     for (i = SENS_TYPE_INVALID; i < SENS_TYPE_LAST_USER; i++) {
687         mSensorList[i] = MAX_REGISTERED_SENSORS;
688     }
689 
690     for (i = SENS_TYPE_INVALID + 1, j = 0; i <= SENS_TYPE_LAST_USER && j < mNumSensors; i++) {
691         if ((si = sensorFind(i, 0, &handle)) != NULL && !(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
692             mSensorList[i - 1] = j;
693             resetBuffer(mActiveSensorTable + j);
694             mActiveSensorTable[j].buffer.sensType = i;
695             mActiveSensorTable[j].rate = 0;
696             mActiveSensorTable[j].latency = 0;
697             mActiveSensorTable[j].numAxis = si->numAxis;
698             mActiveSensorTable[j].interrupt = si->interrupt;
699             if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) {
700                 mSensorList[si->rawType - 1] = j;
701                 mActiveSensorTable[j].buffer.sensType = si->rawType;
702                 mActiveSensorTable[j].raw = true;
703                 mActiveSensorTable[j].rawScale = si->rawScale;
704             } else if (si->flags1 & SENSOR_INFO_FLAGS1_BIAS) {
705                 mSensorList[si->biasType - 1] = j;
706                 osEventSubscribe(mHostIntfTid, sensorGetMyEventType(si->biasType));
707             }
708             if (si->minSamples > MAX_MIN_SAMPLES) {
709                 mActiveSensorTable[j].minSamples = MAX_MIN_SAMPLES;
710                 osLog(LOG_INFO, "initSensors: %s: minSamples of %d exceeded max of %d\n", si->sensorName, si->minSamples, MAX_MIN_SAMPLES);
711             } else {
712                 mActiveSensorTable[j].minSamples = si->minSamples;
713             }
714             mActiveSensorTable[j].curSamples = 0;
715             mActiveSensorTable[j].oneshot = false;
716             mActiveSensorTable[j].firstTime = 0ull;
717             switch (si->numAxis) {
718             case NUM_AXIS_EMBEDDED:
719             case NUM_AXIS_ONE:
720                 mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
721                 break;
722             case NUM_AXIS_THREE:
723                 if (mActiveSensorTable[j].raw)
724                     mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
725                 else
726                     mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
727                 break;
728             }
729             j++;
730         }
731     }
732 
733     return true;
734 }
735 
floatToInt16(float val)736 static inline int16_t floatToInt16(float val)
737 {
738     if (val < (INT16_MIN + 0.5f))
739         return INT16_MIN;
740     else if (val > (INT16_MAX - 0.5f))
741         return INT16_MAX;
742     else if (val >= 0.0f)
743         return val + 0.5f;
744     else
745         return val - 0.5f;
746 }
747 
encodeDeltaTime(uint64_t time)748 static uint32_t encodeDeltaTime(uint64_t time)
749 {
750     uint32_t deltaTime;
751 
752     if (time <= UINT32_MAX) {
753         deltaTime = time | delta_time_fine_mask;
754     } else {
755         deltaTime = ((time + delta_time_rounding) >> delta_time_multiplier_order) & delta_time_coarse_mask;
756     }
757     return deltaTime;
758 }
759 
copyTripleSamplesRaw(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)760 static void copyTripleSamplesRaw(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
761 {
762     int i;
763     uint32_t deltaTime;
764     uint8_t numSamples;
765 
766     for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
767         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
768             simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
769             resetBuffer(sensor);
770         }
771 
772         if (sensor->buffer.firstSample.numSamples == 0) {
773             if (i == 0) {
774                 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
775             } else {
776                 sensor->lastTime += triple->samples[i].deltaTime;
777                 sensor->buffer.referenceTime = sensor->lastTime;
778             }
779             sensor->buffer.length = sizeof(struct RawTripleAxisDataEvent) + sizeof(struct RawTripleAxisDataPoint);
780             sensor->buffer.rawTriple[0].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
781             sensor->buffer.rawTriple[0].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
782             sensor->buffer.rawTriple[0].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
783             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
784                 mWakeupBlocks++;
785             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
786                 mNonWakeupBlocks++;
787             sensor->buffer.firstSample.numSamples = 1;
788             sensor->buffer.firstSample.interrupt = sensor->interrupt;
789             if (sensor->curSamples++ == 0)
790                 sensor->firstTime = sensor->buffer.referenceTime;
791         } else {
792             if (i == 0) {
793                 if (sensor->lastTime > triple->referenceTime) {
794                     // shouldn't happen. flush current packet
795                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
796                     resetBuffer(sensor);
797                     i--;
798                 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
799                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
800                     resetBuffer(sensor);
801                     i--;
802                 } else {
803                     deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
804                     numSamples = sensor->buffer.firstSample.numSamples;
805 
806                     sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
807                     sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime;
808                     sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[0].x * sensor->rawScale);
809                     sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[0].y * sensor->rawScale);
810                     sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[0].z * sensor->rawScale);
811                     sensor->lastTime = triple->referenceTime;
812                     sensor->buffer.firstSample.numSamples++;
813                     sensor->curSamples++;
814                 }
815             } else {
816                 deltaTime = triple->samples[i].deltaTime;
817                 numSamples = sensor->buffer.firstSample.numSamples;
818 
819                 sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
820                 sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
821                 sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
822                 sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
823                 sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
824                 sensor->lastTime += deltaTime;
825                 sensor->buffer.firstSample.numSamples++;
826                 sensor->curSamples++;
827             }
828         }
829     }
830 }
831 
copySingleSamples(struct ActiveSensor * sensor,const struct SingleAxisDataEvent * single)832 static void copySingleSamples(struct ActiveSensor *sensor, const struct SingleAxisDataEvent *single)
833 {
834     int i;
835     uint32_t deltaTime;
836     uint8_t numSamples;
837 
838     for (i = 0; i < single->samples[0].firstSample.numSamples; i++) {
839         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
840             simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
841             resetBuffer(sensor);
842         }
843 
844         if (sensor->buffer.firstSample.numSamples == 0) {
845             if (i == 0) {
846                 sensor->lastTime = sensor->buffer.referenceTime = single->referenceTime;
847             } else {
848                 sensor->lastTime += single->samples[i].deltaTime;
849                 sensor->buffer.referenceTime = sensor->lastTime;
850             }
851             sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
852             sensor->buffer.single[0].idata = single->samples[i].idata;
853             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
854                 mWakeupBlocks++;
855             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
856                 mNonWakeupBlocks++;
857             sensor->buffer.firstSample.numSamples = 1;
858             sensor->buffer.firstSample.interrupt = sensor->interrupt;
859             if (sensor->curSamples++ == 0)
860                 sensor->firstTime = sensor->buffer.referenceTime;
861         } else {
862             if (i == 0) {
863                 if (sensor->lastTime > single->referenceTime) {
864                     // shouldn't happen. flush current packet
865                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
866                     resetBuffer(sensor);
867                     i--;
868                 } else if (single->referenceTime - sensor->lastTime >= delta_time_max) {
869                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
870                     resetBuffer(sensor);
871                     i--;
872                 } else {
873                     deltaTime = encodeDeltaTime(single->referenceTime - sensor->lastTime);
874                     numSamples = sensor->buffer.firstSample.numSamples;
875 
876                     sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
877                     sensor->buffer.single[numSamples].deltaTime = deltaTime;
878                     sensor->buffer.single[numSamples].idata = single->samples[0].idata;
879                     sensor->lastTime = single->referenceTime;
880                     sensor->buffer.firstSample.numSamples++;
881                     sensor->curSamples++;
882                 }
883             } else {
884                 deltaTime = single->samples[i].deltaTime;
885                 numSamples = sensor->buffer.firstSample.numSamples;
886 
887                 sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
888                 sensor->buffer.single[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
889                 sensor->buffer.single[numSamples].idata = single->samples[i].idata;
890                 sensor->lastTime += deltaTime;
891                 sensor->buffer.firstSample.numSamples++;
892                 sensor->curSamples++;
893             }
894         }
895     }
896 }
897 
copyTripleSamples(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)898 static void copyTripleSamples(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
899 {
900     int i;
901     uint32_t deltaTime;
902     uint8_t numSamples;
903 
904     for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
905         if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
906             simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
907             resetBuffer(sensor);
908         }
909 
910         if (sensor->buffer.firstSample.numSamples == 0) {
911             if (i == 0) {
912                 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
913             } else {
914                 sensor->lastTime += triple->samples[i].deltaTime;
915                 sensor->buffer.referenceTime = sensor->lastTime;
916             }
917             sensor->buffer.length = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
918             sensor->buffer.triple[0].ix = triple->samples[i].ix;
919             sensor->buffer.triple[0].iy = triple->samples[i].iy;
920             sensor->buffer.triple[0].iz = triple->samples[i].iz;
921             if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
922                 sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
923                 sensor->buffer.firstSample.biasPresent = 1;
924                 sensor->buffer.firstSample.biasSample = 0;
925                 sensor->discard = false;
926             }
927             if (sensor->interrupt == NANOHUB_INT_WAKEUP)
928                 mWakeupBlocks++;
929             else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
930                 mNonWakeupBlocks++;
931             sensor->buffer.firstSample.numSamples = 1;
932             sensor->buffer.firstSample.interrupt = sensor->interrupt;
933             if (sensor->curSamples++ == 0)
934                 sensor->firstTime = sensor->buffer.referenceTime;
935         } else {
936             if (i == 0) {
937                 if (sensor->lastTime > triple->referenceTime) {
938                     // shouldn't happen. flush current packet
939                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
940                     resetBuffer(sensor);
941                     i--;
942                 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
943                     simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
944                     resetBuffer(sensor);
945                     i--;
946                 } else {
947                     deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
948                     numSamples = sensor->buffer.firstSample.numSamples;
949 
950                     sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
951                     sensor->buffer.triple[numSamples].deltaTime = deltaTime;
952                     sensor->buffer.triple[numSamples].ix = triple->samples[0].ix;
953                     sensor->buffer.triple[numSamples].iy = triple->samples[0].iy;
954                     sensor->buffer.triple[numSamples].iz = triple->samples[0].iz;
955                     sensor->lastTime = triple->referenceTime;
956                     if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == 0) {
957                         sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
958                         sensor->buffer.firstSample.biasPresent = 1;
959                         sensor->buffer.firstSample.biasSample = numSamples;
960                         sensor->discard = false;
961                     }
962                     sensor->buffer.firstSample.numSamples++;
963                     sensor->curSamples++;
964                 }
965             } else {
966                 deltaTime = triple->samples[i].deltaTime;
967                 numSamples = sensor->buffer.firstSample.numSamples;
968 
969                 sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
970                 sensor->buffer.triple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
971                 sensor->buffer.triple[numSamples].ix = triple->samples[i].ix;
972                 sensor->buffer.triple[numSamples].iy = triple->samples[i].iy;
973                 sensor->buffer.triple[numSamples].iz = triple->samples[i].iz;
974                 sensor->lastTime += deltaTime;
975                 if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
976                     sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
977                     sensor->buffer.firstSample.biasPresent = 1;
978                     sensor->buffer.firstSample.biasSample = numSamples;
979                     sensor->discard = false;
980                 }
981                 sensor->buffer.firstSample.numSamples++;
982                 sensor->curSamples++;
983             }
984         }
985     }
986 }
987 
hostIntfAddBlock(struct HostIntfDataBuffer * data)988 static void hostIntfAddBlock(struct HostIntfDataBuffer *data)
989 {
990     if (data->interrupt == NANOHUB_INT_WAKEUP)
991         mWakeupBlocks++;
992     else if (data->interrupt == NANOHUB_INT_NONWAKEUP)
993         mNonWakeupBlocks++;
994     nanohubPrefetchTx(data->interrupt, mWakeupBlocks, mNonWakeupBlocks);
995 }
996 
hostIntfNotifyReboot(uint32_t reason)997 static void hostIntfNotifyReboot(uint32_t reason)
998 {
999     struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp));
1000     __le32 raw_reason = htole32(reason);
1001 
1002     if (resp) {
1003         resp->hdr = (struct NanohubHalHdr){
1004             .appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0),
1005             .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
1006             .msg = NANOHUB_HAL_REBOOT,
1007         };
1008         memcpy(&resp->reason, &raw_reason, sizeof(resp->reason));
1009         osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
1010     }
1011 }
1012 
queueFlush(struct ActiveSensor * sensor)1013 static void queueFlush(struct ActiveSensor *sensor)
1014 {
1015     if (sensor->buffer.length == 0) {
1016         sensor->buffer.length = sizeof(sensor->buffer.referenceTime) + sizeof(struct SensorFirstSample);
1017         sensor->buffer.referenceTime = 0ull;
1018         if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1019             mWakeupBlocks++;
1020         else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1021             mNonWakeupBlocks++;
1022         sensor->buffer.firstSample.numFlushes = 1;
1023     } else {
1024         sensor->buffer.firstSample.numFlushes++;
1025     }
1026     sensor->discard = false;
1027     hostIntfSetInterrupt(sensor->interrupt);
1028 }
1029 
fakeFlush(struct ConfigCmd * cmd)1030 static void fakeFlush(struct ConfigCmd *cmd)
1031 {
1032     struct HostIntfDataBuffer *buffer;
1033     uint8_t size = sizeof(buffer->evtType) + sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1034     buffer = alloca(size);
1035     memset(buffer, 0x00, size);
1036 
1037     buffer->sensType = cmd->sensType;
1038     buffer->length = sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1039     buffer->interrupt = NANOHUB_INT_WAKEUP;
1040     buffer->firstSample.numFlushes = 1;
1041     simpleQueueEnqueue(mOutputQ, buffer, size, false);
1042 }
1043 
hostIntfHandleEvent(uint32_t evtType,const void * evtData)1044 static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
1045 {
1046     struct ConfigCmd *cmd;
1047     uint32_t i, cnt;
1048     uint64_t rtcTime;
1049     struct ActiveSensor *sensor;
1050     uint32_t tempSensorHandle;
1051     const struct HostHubRawPacket *hostMsg;
1052     struct HostIntfDataBuffer *data;
1053     const struct NanohubHalCommand *halCmd;
1054     const uint8_t *halMsg;
1055     uint32_t reason;
1056     uint32_t interrupt = HOSTINTF_MAX_INTERRUPTS;
1057 
1058     if (evtType == EVT_APP_START) {
1059         if (initSensors()) {
1060             osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
1061             osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
1062             osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
1063 #ifdef DEBUG_LOG_EVT
1064             osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
1065             platEarlyLogFlush();
1066 #endif
1067             reason = pwrResetReason();
1068             data = alloca(sizeof(uint32_t) + sizeof(reason));
1069             data->sensType = SENS_TYPE_INVALID;
1070             data->length = sizeof(reason);
1071             data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON;
1072             data->interrupt = NANOHUB_INT_WAKEUP;
1073             memcpy(data->buffer, &reason, sizeof(reason));
1074             simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, false);
1075             hostIntfAddBlock(data);
1076             hostIntfNotifyReboot(reason);
1077         }
1078     } else if (evtType == EVT_APP_TO_HOST) {
1079         hostMsg = evtData;
1080         if (hostMsg->dataLen <= HOST_HUB_RAW_PACKET_MAX_LEN) {
1081             data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->dataLen);
1082             data->sensType = SENS_TYPE_INVALID;
1083             data->length = sizeof(*hostMsg) + hostMsg->dataLen;
1084             data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
1085             data->interrupt = NANOHUB_INT_WAKEUP;
1086             memcpy(data->buffer, evtData, data->length);
1087             simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, false);
1088             hostIntfAddBlock(data);
1089         }
1090     } else if (evtType == EVT_APP_FROM_HOST) {
1091         halMsg = evtData;
1092         if ((halCmd = nanohubHalFindCommand(halMsg[1])))
1093             halCmd->handler((void *)&halMsg[2], halMsg[0] - 1);
1094     }
1095 #ifdef DEBUG_LOG_EVT
1096     else if (evtType == EVT_DEBUG_LOG) {
1097         data = (struct HostIntfDataBuffer *)evtData;
1098         if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG) {
1099             simpleQueueEnqueue(mOutputQ, evtData, sizeof(uint32_t) + data->length, true);
1100             hostIntfAddBlock(data);
1101         }
1102     }
1103 #endif
1104     else if (evtType == EVT_LATENCY_TIMER) {
1105         rtcTime = rtcGetTime();
1106 
1107         for (i = 0, cnt = 0; i < mNumSensors && cnt < mLatencyCnt; i++) {
1108             if (mActiveSensorTable[i].latency > 0) {
1109                 cnt++;
1110                 if (mActiveSensorTable[i].firstTime && rtcTime >= mActiveSensorTable[i].firstTime + mActiveSensorTable[i].latency) {
1111                     hostIntfSetInterrupt(mActiveSensorTable[i].interrupt);
1112                 }
1113             }
1114         }
1115     } else if (evtType == EVT_NO_SENSOR_CONFIG_EVENT) { // config
1116         cmd = (struct ConfigCmd *)evtData;
1117         if (cmd->sensType > SENS_TYPE_INVALID && cmd->sensType <= SENS_TYPE_LAST_USER && mSensorList[cmd->sensType - 1] < MAX_REGISTERED_SENSORS) {
1118             sensor = mActiveSensorTable + mSensorList[cmd->sensType - 1];
1119 
1120             if (sensor->sensorHandle) {
1121                 if (cmd->cmd == CONFIG_CMD_FLUSH) {
1122                     sensorFlush(sensor->sensorHandle);
1123                 } else if (cmd->cmd == CONFIG_CMD_ENABLE) {
1124                     if (sensorRequestRateChange(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1125                         sensor->rate = cmd->rate;
1126                         if (sensor->latency != cmd->latency) {
1127                             if (!sensor->latency) {
1128                                 if (mLatencyCnt++ == 0)
1129                                     mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1130                             } else if (!cmd->latency) {
1131                                 if (--mLatencyCnt == 0) {
1132                                     timTimerCancel(mLatencyTimer);
1133                                     mLatencyTimer = 0;
1134                                 }
1135                             }
1136                             sensor->latency = cmd->latency;
1137                         }
1138                     }
1139                 } else if (cmd->cmd == CONFIG_CMD_DISABLE) {
1140                     sensorRelease(mHostIntfTid, sensor->sensorHandle);
1141                     osEventUnsubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1142                     if (sensor->latency) {
1143                         if (--mLatencyCnt == 0) {
1144                             timTimerCancel(mLatencyTimer);
1145                             mLatencyTimer = 0;
1146                         }
1147                     }
1148                     sensor->rate = 0;
1149                     sensor->latency = 0;
1150                     sensor->oneshot = false;
1151                     sensor->sensorHandle = 0;
1152                     if (sensor->buffer.length) {
1153                         simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
1154                         hostIntfSetInterrupt(sensor->interrupt);
1155                         resetBuffer(sensor);
1156                     }
1157                 }
1158             } else if (cmd->cmd == CONFIG_CMD_ENABLE) {
1159                 for (i = 0; sensorFind(cmd->sensType, i, &sensor->sensorHandle) != NULL; i++) {
1160                     if (cmd->rate == SENSOR_RATE_ONESHOT) {
1161                         cmd->rate = SENSOR_RATE_ONCHANGE;
1162                         sensor->oneshot = true;
1163                     } else {
1164                         sensor->oneshot = false;
1165                     }
1166 
1167                     if (sensorRequest(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1168                         if (cmd->latency) {
1169                             if (mLatencyCnt++ == 0)
1170                                 mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1171                         }
1172                         sensor->rate = cmd->rate;
1173                         sensor->latency = cmd->latency;
1174                         osEventSubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1175                         break;
1176                     } else {
1177                         sensor->sensorHandle = 0;
1178                     }
1179                 }
1180             } else if (cmd->cmd == CONFIG_CMD_CALIBRATE) {
1181                 for (i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1182                     sensorCalibrate(tempSensorHandle);
1183             } else if (cmd->cmd == CONFIG_CMD_CFG_DATA) {
1184                 for (i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1185                     sensorCfgData(tempSensorHandle, (void *)(cmd+1));
1186             } else if (cmd->cmd == CONFIG_CMD_FLUSH) {
1187                     queueFlush(sensor);
1188             }
1189         } else if (cmd->cmd == CONFIG_CMD_FLUSH && cmd->sensType > SENS_TYPE_INVALID) {
1190             // if a flush event is for an unknown sensor, we just return a fake flush event.
1191             osLog(LOG_INFO, "Flush request from unrecognized sensor, returning a fake flush\n");
1192             fakeFlush(cmd);
1193         }
1194     } else if (evtType > EVT_NO_FIRST_SENSOR_EVENT && evtType < EVT_NO_SENSOR_CONFIG_EVENT && mSensorList[(evtType & 0xFF)-1] < MAX_REGISTERED_SENSORS) { // data
1195         sensor = mActiveSensorTable + mSensorList[(evtType & 0xFF) - 1];
1196 
1197         if (sensor->sensorHandle) {
1198             if (evtData == SENSOR_DATA_EVENT_FLUSH) {
1199                 queueFlush(sensor);
1200             } else {
1201                 if (sensor->buffer.length > 0) {
1202                     if (sensor->buffer.firstSample.numFlushes > 0) {
1203                         if (!(simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard)))
1204                             return; // flushes more important than samples
1205                         else
1206                             resetBuffer(sensor);
1207                     } else if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
1208                         simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
1209                         resetBuffer(sensor);
1210                     }
1211                 }
1212 
1213                 switch (sensor->numAxis) {
1214                 case NUM_AXIS_EMBEDDED:
1215                     rtcTime = rtcGetTime();
1216                     if (sensor->buffer.length > 0 && rtcTime - sensor->lastTime >= delta_time_max) {
1217                         simpleQueueEnqueue(mOutputQ, &sensor->buffer, sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
1218                         resetBuffer(sensor);
1219                     }
1220                     if (sensor->buffer.length == 0) {
1221                         sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
1222                         sensor->lastTime = sensor->buffer.referenceTime = rtcTime;
1223                         if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1224                             mWakeupBlocks++;
1225                         else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1226                             mNonWakeupBlocks++;
1227                         sensor->buffer.firstSample.numSamples = 1;
1228                         sensor->buffer.firstSample.interrupt = sensor->interrupt;
1229                         sensor->buffer.single[0].idata = (uint32_t)evtData;
1230                     } else {
1231                         sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
1232                         sensor->buffer.single[sensor->buffer.firstSample.numSamples].deltaTime = encodeDeltaTime(rtcTime - sensor->lastTime);
1233                         sensor->lastTime = rtcTime;
1234                         sensor->buffer.single[sensor->buffer.firstSample.numSamples].idata = (uint32_t)evtData;
1235                         sensor->buffer.firstSample.numSamples++;
1236                     }
1237                     if (sensor->curSamples++ == 0)
1238                         sensor->firstTime = sensor->buffer.referenceTime;
1239                     break;
1240                 case NUM_AXIS_ONE:
1241                     copySingleSamples(sensor, evtData);
1242                     break;
1243                 case NUM_AXIS_THREE:
1244                     if (sensor->raw)
1245                         copyTripleSamplesRaw(sensor, evtData);
1246                     else
1247                         copyTripleSamples(sensor, evtData);
1248                     break;
1249                 default:
1250                     return;
1251                 }
1252             }
1253 
1254             rtcTime = rtcGetTime();
1255 
1256             if (sensor->firstTime &&
1257                 ((rtcTime >= sensor->firstTime + sensor->latency) ||
1258                  ((sensor->latency > sensorGetCurLatency(sensor->sensorHandle)) &&
1259                   (rtcTime + sensorGetCurLatency(sensor->sensorHandle) > sensor->firstTime + sensor->latency)))) {
1260                 interrupt = sensor->interrupt;
1261             } else if (mWakeupBlocks + mNonWakeupBlocks >= mTotalBlocks) {
1262                 interrupt = sensor->interrupt;
1263             }
1264 
1265             nanohubPrefetchTx(interrupt, mWakeupBlocks, mNonWakeupBlocks);
1266 
1267             if (sensor->oneshot) {
1268                 sensorRelease(mHostIntfTid, sensor->sensorHandle);
1269                 osEventUnsubscribe(mHostIntfTid, evtType);
1270                 sensor->sensorHandle = 0;
1271                 sensor->oneshot = false;
1272             }
1273         } else if (evtData != SENSOR_DATA_EVENT_FLUSH) {
1274             // handle bias data which can be generated for sensors that are
1275             // not currently requested by the AP
1276             switch (sensor->numAxis) {
1277             case NUM_AXIS_THREE:
1278                 if (((const struct TripleAxisDataEvent *)evtData)->samples[0].firstSample.biasPresent) {
1279                     copyTripleSamples(sensor, evtData);
1280                     nanohubPrefetchTx(sensor->interrupt, mWakeupBlocks, mNonWakeupBlocks);
1281                 }
1282                 break;
1283             default:
1284                 break;
1285             }
1286         }
1287     }
1288 }
1289 
hostIntfCopyInterrupts(void * dst,uint32_t numBits)1290 void hostIntfCopyInterrupts(void *dst, uint32_t numBits)
1291 {
1292     if (mInterrupt->numBits != numBits)
1293         return;
1294 
1295     atomicBitsetBulkRead(mInterrupt, dst, numBits);
1296 }
1297 
hostIntfClearInterrupts()1298 void hostIntfClearInterrupts()
1299 {
1300     uint32_t i;
1301 
1302     for (i = 0; i < HOSTINTF_MAX_INTERRUPTS; i++) {
1303         if (atomicBitsetGetBit(mInterrupt, i))
1304             hostIntfClearInterrupt(i);
1305     }
1306 }
1307 
hostIntfSetInterrupt(uint32_t bit)1308 void hostIntfSetInterrupt(uint32_t bit)
1309 {
1310     uint64_t state = cpuIntsOff();
1311     if (mHostIntfTid) {
1312         if (!atomicBitsetGetBit(mInterrupt, bit)) {
1313             atomicBitsetSetBit(mInterrupt, bit);
1314             if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1315                 if (mInterruptCntWkup++ == 0)
1316                     apIntSet(true);
1317             } else {
1318                 if (mInterruptCntNonWkup++ == 0)
1319                     apIntSet(false);
1320             }
1321         }
1322     }
1323     cpuIntsRestore(state);
1324 }
1325 
hostIntfGetInterrupt(uint32_t bit)1326 bool hostIntfGetInterrupt(uint32_t bit)
1327 {
1328     return atomicBitsetGetBit(mInterrupt, bit);
1329 }
1330 
hostIntfClearInterrupt(uint32_t bit)1331 void hostIntfClearInterrupt(uint32_t bit)
1332 {
1333     uint64_t state = cpuIntsOff();
1334     if (mHostIntfTid) {
1335         if (atomicBitsetGetBit(mInterrupt, bit)) {
1336             atomicBitsetClearBit(mInterrupt, bit);
1337             if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1338                 if (--mInterruptCntWkup == 0)
1339                     apIntClear(true);
1340             } else {
1341                 if (--mInterruptCntNonWkup == 0)
1342                     apIntClear(false);
1343             }
1344         }
1345     }
1346     cpuIntsRestore(state);
1347 }
1348 
hostIntfSetInterruptMask(uint32_t bit)1349 void hostIntfSetInterruptMask(uint32_t bit)
1350 {
1351     uint64_t state = cpuIntsOff();
1352     if (mHostIntfTid) {
1353         if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1354             atomicBitsetSetBit(mInterruptMask, bit);
1355             if (atomicBitsetGetBit(mInterrupt, bit)) {
1356                 if (--mInterruptCntWkup == 0)
1357                     apIntClear(true);
1358                 if (mInterruptCntNonWkup++ == 0)
1359                     apIntSet(false);
1360             }
1361         }
1362     }
1363     cpuIntsRestore(state);
1364 }
1365 
hostIntfGetInterruptMask(uint32_t bit)1366 bool hostIntfGetInterruptMask(uint32_t bit)
1367 {
1368     return atomicBitsetGetBit(mInterruptMask, bit);
1369 }
1370 
hostIntfClearInterruptMask(uint32_t bit)1371 void hostIntfClearInterruptMask(uint32_t bit)
1372 {
1373     uint64_t state = cpuIntsOff();
1374     if (mHostIntfTid) {
1375         if (atomicBitsetGetBit(mInterruptMask, bit)) {
1376             atomicBitsetClearBit(mInterruptMask, bit);
1377             if (atomicBitsetGetBit(mInterrupt, bit)) {
1378                 if (mInterruptCntWkup++ == 0)
1379                     apIntSet(true);
1380                 if (--mInterruptCntNonWkup == 0)
1381                     apIntClear(false);
1382             }
1383         }
1384     }
1385     cpuIntsRestore(state);
1386 }
1387 
1388 INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
1389