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