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