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