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, count = 0;
551
552 ret = simpleQueueDequeue(mOutputQ, data);
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, data);
563 count++;
564 } else {
565 break;
566 }
567 } else {
568 break;
569 }
570 }
571
572 if (!ret) {
573 // nothing in queue. look for partial buffers to flush
574 for (i = 0; i < mNumSensors; i++, mLastSensor = (mLastSensor + 1) % mNumSensors) {
575 if (mActiveSensorTable[mLastSensor].buffer.length > 0) {
576 memcpy(data, &mActiveSensorTable[mLastSensor].buffer, sizeof(struct HostIntfDataBuffer));
577 resetBuffer(mActiveSensorTable + mLastSensor);
578 ret = true;
579 mLastSensor = (mLastSensor + 1) % mNumSensors;
580 break;
581 }
582 }
583 }
584
585 if (ret) {
586 if (buffer->sensType > SENS_TYPE_INVALID && buffer->sensType <= SENS_TYPE_LAST_USER && mSensorList[buffer->sensType - 1] < MAX_REGISTERED_SENSORS) {
587 sensor = mActiveSensorTable + mSensorList[buffer->sensType - 1];
588 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
589 mWakeupBlocks--;
590 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
591 mNonWakeupBlocks--;
592 sensor->curSamples -= buffer->firstSample.numSamples;
593 sensor->firstTime = 0ull;
594 } else {
595 if (buffer->interrupt == NANOHUB_INT_WAKEUP)
596 mWakeupBlocks--;
597 else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
598 mNonWakeupBlocks--;
599 }
600 }
601
602 *wakeup = mWakeupBlocks;
603 *nonwakeup = mNonWakeupBlocks;
604
605 return ret;
606 }
607
initCompleteCallback(uint32_t timerId,void * data)608 static void initCompleteCallback(uint32_t timerId, void *data)
609 {
610 osEnqueuePrivateEvt(EVT_APP_START, NULL, NULL, mHostIntfTid);
611 }
612
queueDiscard(void * data,bool onDelete)613 static bool queueDiscard(void *data, bool onDelete)
614 {
615 struct HostIntfDataBuffer *buffer = data;
616 struct ActiveSensor *sensor;
617
618 if (buffer->sensType > SENS_TYPE_INVALID && buffer->sensType <= SENS_TYPE_LAST_USER && mSensorList[buffer->sensType - 1] < MAX_REGISTERED_SENSORS) { // data
619 sensor = mActiveSensorTable + mSensorList[buffer->sensType - 1];
620
621 if (sensor->curSamples - buffer->firstSample.numSamples >= sensor->minSamples || onDelete) {
622 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
623 mWakeupBlocks--;
624 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
625 mNonWakeupBlocks--;
626 sensor->curSamples -= buffer->firstSample.numSamples;
627
628 return true;
629 } else {
630 return false;
631 }
632 } else {
633 if (buffer->interrupt == NANOHUB_INT_WAKEUP)
634 mWakeupBlocks--;
635 else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
636 mNonWakeupBlocks--;
637 return true;
638 }
639 }
640
latencyTimerCallback(uint32_t timerId,void * data)641 static void latencyTimerCallback(uint32_t timerId, void* data)
642 {
643 osEnqueuePrivateEvt(EVT_LATENCY_TIMER, data, NULL, mHostIntfTid);
644 }
645
initSensors()646 static bool initSensors()
647 {
648 uint32_t i, j, blocks, maxBlocks, numAxis, packetSamples;
649 bool present, error;
650 const struct SensorInfo *si;
651 uint32_t handle;
652 static uint8_t errorCnt = 0;
653
654 mTotalBlocks = 0;
655 mNumSensors = 0;
656
657 for (i = SENS_TYPE_INVALID + 1; i <= SENS_TYPE_LAST_USER; i++) {
658 for (j = 0, present = 0, error = 0; (si = sensorFind(i, j, &handle)) != NULL; j++) {
659 if (!sensorGetInitComplete(handle)) {
660 if (errorCnt >= SENSOR_INIT_ERROR_MAX) {
661 osLog(LOG_ERROR, "initSensors: %s not ready - skipping!\n", si->sensorName);
662 continue;
663 } else {
664 osLog(LOG_INFO, "initSensors: %s not ready!\n", si->sensorName);
665 timTimerSet(SENSOR_INIT_DELAY, 0, 50, initCompleteCallback, NULL, true);
666 errorCnt ++;
667 return false;
668 }
669 } else if (!(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
670 if (!present) {
671 present = 1;
672 numAxis = si->numAxis;
673 switch (si->numAxis) {
674 case NUM_AXIS_EMBEDDED:
675 case NUM_AXIS_ONE:
676 packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
677 break;
678 case NUM_AXIS_THREE:
679 if (si->flags1 & SENSOR_INFO_FLAGS1_RAW)
680 packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
681 else
682 packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
683 break;
684 default:
685 packetSamples = 1;
686 error = true;
687 }
688 if (si->minSamples > MAX_MIN_SAMPLES)
689 maxBlocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
690 else
691 maxBlocks = (si->minSamples + packetSamples - 1) / packetSamples;
692 } else {
693 if (si->numAxis != numAxis) {
694 error = true;
695 } else {
696 if (si->minSamples > MAX_MIN_SAMPLES)
697 blocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
698 else
699 blocks = (si->minSamples + packetSamples - 1) / packetSamples;
700
701 maxBlocks = maxBlocks > blocks ? maxBlocks : blocks;
702 }
703 }
704 }
705 }
706
707 if (present && !error) {
708 mNumSensors++;
709 mTotalBlocks += maxBlocks;
710 }
711 }
712
713 if (mTotalBlocks > MAX_NUM_BLOCKS) {
714 osLog(LOG_INFO, "initSensors: mTotalBlocks of %ld exceeds maximum of %d\n", mTotalBlocks, MAX_NUM_BLOCKS);
715 mTotalBlocks = MAX_NUM_BLOCKS;
716 } else if (mTotalBlocks < MIN_NUM_BLOCKS) {
717 mTotalBlocks = MIN_NUM_BLOCKS;
718 }
719
720 mOutputQ = simpleQueueAlloc(mTotalBlocks, sizeof(struct HostIntfDataBuffer), queueDiscard);
721 mActiveSensorTable = heapAlloc(mNumSensors * sizeof(struct ActiveSensor));
722 memset(mActiveSensorTable, 0x00, mNumSensors * sizeof(struct ActiveSensor));
723
724 for (i = SENS_TYPE_INVALID; i < SENS_TYPE_LAST_USER; i++) {
725 mSensorList[i] = MAX_REGISTERED_SENSORS;
726 }
727
728 for (i = SENS_TYPE_INVALID + 1, j = 0; i <= SENS_TYPE_LAST_USER && j < mNumSensors; i++) {
729 if ((si = sensorFind(i, 0, &handle)) != NULL && !(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
730 mSensorList[i - 1] = j;
731 resetBuffer(mActiveSensorTable + j);
732 mActiveSensorTable[j].buffer.sensType = i;
733 mActiveSensorTable[j].biasReportType = 0;
734 mActiveSensorTable[j].rate = 0;
735 mActiveSensorTable[j].latency = 0;
736 mActiveSensorTable[j].numAxis = si->numAxis;
737 mActiveSensorTable[j].interrupt = si->interrupt;
738 if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) {
739 mSensorList[si->rawType - 1] = j;
740 mActiveSensorTable[j].buffer.sensType = si->rawType;
741 mActiveSensorTable[j].raw = true;
742 mActiveSensorTable[j].rawScale = si->rawScale;
743 }
744 if (si->flags1 & SENSOR_INFO_FLAGS1_BIAS) {
745 mSensorList[si->biasType - 1] = j;
746 mActiveSensorTable[j].biasReportType = i;
747 osEventSubscribe(mHostIntfTid, sensorGetMyEventType(si->biasType));
748 }
749 if (si->minSamples > MAX_MIN_SAMPLES) {
750 mActiveSensorTable[j].minSamples = MAX_MIN_SAMPLES;
751 osLog(LOG_INFO, "initSensors: %s: minSamples of %d exceeded max of %d\n", si->sensorName, si->minSamples, MAX_MIN_SAMPLES);
752 } else {
753 mActiveSensorTable[j].minSamples = si->minSamples;
754 }
755 mActiveSensorTable[j].curSamples = 0;
756 mActiveSensorTable[j].oneshot = false;
757 mActiveSensorTable[j].firstTime = 0ull;
758 switch (si->numAxis) {
759 case NUM_AXIS_EMBEDDED:
760 case NUM_AXIS_ONE:
761 mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
762 break;
763 case NUM_AXIS_THREE:
764 if (mActiveSensorTable[j].raw)
765 mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
766 else
767 mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
768 break;
769 }
770 j++;
771 }
772 }
773
774 return true;
775 }
776
floatToInt16(float val)777 static inline int16_t floatToInt16(float val)
778 {
779 if (val < (INT16_MIN + 0.5f))
780 return INT16_MIN;
781 else if (val > (INT16_MAX - 0.5f))
782 return INT16_MAX;
783 else if (val >= 0.0f)
784 return val + 0.5f;
785 else
786 return val - 0.5f;
787 }
788
encodeDeltaTime(uint64_t time)789 static uint32_t encodeDeltaTime(uint64_t time)
790 {
791 uint32_t deltaTime;
792
793 if (time <= UINT32_MAX) {
794 deltaTime = time | delta_time_fine_mask;
795 } else {
796 deltaTime = ((time + delta_time_rounding) >> delta_time_multiplier_order) & delta_time_coarse_mask;
797 }
798 return deltaTime;
799 }
800
enqueueSensorBufferEx(struct ActiveSensor * sensor,bool doResetOnError)801 static int enqueueSensorBufferEx(struct ActiveSensor *sensor, bool doResetOnError)
802 {
803 bool queued = simpleQueueEnqueue(mOutputQ, &sensor->buffer,
804 sizeof(uint32_t) + sensor->buffer.length, sensor->discard);
805
806 if (!queued) {
807 if (!doResetOnError)
808 return -1;
809 // undo counters if failed to add buffer
810 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
811 mWakeupBlocks--;
812 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
813 mNonWakeupBlocks--;
814 }
815 resetBuffer(sensor);
816 return queued ? 1 : 0;
817 }
818
enqueueSensorBuffer(struct ActiveSensor * sensor)819 static inline int enqueueSensorBuffer(struct ActiveSensor *sensor)
820 {
821 return enqueueSensorBufferEx(sensor, true);
822 }
823
copySingleSamples(struct ActiveSensor * sensor,const struct SingleAxisDataEvent * single)824 static void copySingleSamples(struct ActiveSensor *sensor, const struct SingleAxisDataEvent *single)
825 {
826 int i;
827 uint32_t deltaTime;
828 uint8_t numSamples;
829
830 for (i = 0; i < single->samples[0].firstSample.numSamples; i++) {
831 if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
832 enqueueSensorBuffer(sensor);
833 }
834
835 if (sensor->buffer.firstSample.numSamples == 0) {
836 if (i == 0) {
837 sensor->lastTime = sensor->buffer.referenceTime = single->referenceTime;
838 } else {
839 sensor->lastTime += single->samples[i].deltaTime;
840 sensor->buffer.referenceTime = sensor->lastTime;
841 }
842 sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
843 sensor->buffer.single[0].idata = single->samples[i].idata;
844 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
845 mWakeupBlocks++;
846 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
847 mNonWakeupBlocks++;
848 sensor->buffer.firstSample.numSamples = 1;
849 sensor->buffer.firstSample.interrupt = sensor->interrupt;
850 if (sensor->curSamples++ == 0)
851 sensor->firstTime = sensor->buffer.referenceTime;
852 } else {
853 if (i == 0) {
854 if (sensor->lastTime > single->referenceTime) {
855 // shouldn't happen. flush current packet
856 enqueueSensorBuffer(sensor);
857 i--;
858 } else if (single->referenceTime - sensor->lastTime >= delta_time_max) {
859 enqueueSensorBuffer(sensor);
860 i--;
861 } else {
862 deltaTime = encodeDeltaTime(single->referenceTime - sensor->lastTime);
863 numSamples = sensor->buffer.firstSample.numSamples;
864
865 sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
866 sensor->buffer.single[numSamples].deltaTime = deltaTime;
867 sensor->buffer.single[numSamples].idata = single->samples[0].idata;
868 sensor->lastTime = single->referenceTime;
869 sensor->buffer.firstSample.numSamples++;
870 sensor->curSamples++;
871 }
872 } else {
873 deltaTime = single->samples[i].deltaTime;
874 numSamples = sensor->buffer.firstSample.numSamples;
875
876 sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
877 sensor->buffer.single[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
878 sensor->buffer.single[numSamples].idata = single->samples[i].idata;
879 sensor->lastTime += deltaTime;
880 sensor->buffer.firstSample.numSamples++;
881 sensor->curSamples++;
882 }
883 }
884 }
885 }
886
copyTripleSamples(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)887 static void copyTripleSamples(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
888 {
889 int i;
890 uint32_t deltaTime;
891 uint8_t numSamples;
892
893 for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
894 if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
895 enqueueSensorBuffer(sensor);
896 }
897
898 if (sensor->buffer.firstSample.numSamples == 0) {
899 if (i == 0) {
900 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
901 } else {
902 sensor->lastTime += triple->samples[i].deltaTime;
903 sensor->buffer.referenceTime = sensor->lastTime;
904 }
905 sensor->buffer.length = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
906 sensor->buffer.triple[0].ix = triple->samples[i].ix;
907 sensor->buffer.triple[0].iy = triple->samples[i].iy;
908 sensor->buffer.triple[0].iz = triple->samples[i].iz;
909 if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
910 sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
911 sensor->buffer.firstSample.biasPresent = 1;
912 sensor->buffer.firstSample.biasSample = 0;
913 sensor->discard = false;
914 }
915 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
916 mWakeupBlocks++;
917 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
918 mNonWakeupBlocks++;
919 sensor->buffer.firstSample.numSamples = 1;
920 sensor->buffer.firstSample.interrupt = sensor->interrupt;
921 if (sensor->curSamples++ == 0)
922 sensor->firstTime = sensor->buffer.referenceTime;
923 } else {
924 if (i == 0) {
925 if (sensor->lastTime > triple->referenceTime) {
926 // shouldn't happen. flush current packet
927 enqueueSensorBuffer(sensor);
928 i--;
929 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
930 enqueueSensorBuffer(sensor);
931 i--;
932 } else {
933 deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
934 numSamples = sensor->buffer.firstSample.numSamples;
935
936 sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
937 sensor->buffer.triple[numSamples].deltaTime = deltaTime;
938 sensor->buffer.triple[numSamples].ix = triple->samples[0].ix;
939 sensor->buffer.triple[numSamples].iy = triple->samples[0].iy;
940 sensor->buffer.triple[numSamples].iz = triple->samples[0].iz;
941 sensor->lastTime = triple->referenceTime;
942 if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == 0) {
943 sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
944 sensor->buffer.firstSample.biasPresent = 1;
945 sensor->buffer.firstSample.biasSample = numSamples;
946 sensor->discard = false;
947 }
948 sensor->buffer.firstSample.numSamples++;
949 sensor->curSamples++;
950 }
951 } else {
952 deltaTime = triple->samples[i].deltaTime;
953 numSamples = sensor->buffer.firstSample.numSamples;
954
955 sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
956 sensor->buffer.triple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
957 sensor->buffer.triple[numSamples].ix = triple->samples[i].ix;
958 sensor->buffer.triple[numSamples].iy = triple->samples[i].iy;
959 sensor->buffer.triple[numSamples].iz = triple->samples[i].iz;
960 sensor->lastTime += deltaTime;
961 if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
962 sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
963 sensor->buffer.firstSample.biasPresent = 1;
964 sensor->buffer.firstSample.biasSample = numSamples;
965 sensor->discard = false;
966 }
967 sensor->buffer.firstSample.numSamples++;
968 sensor->curSamples++;
969 }
970 }
971 }
972 }
973
copyTripleSamplesBias(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)974 static void copyTripleSamplesBias(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
975 {
976 uint8_t sensType = sensor->buffer.sensType;
977
978 if (sensType == sensor->biasReportType) {
979 copyTripleSamples(sensor, triple);
980 } else {
981 // Bias needs to be sent with a different sensType, so enqueue any pending buffer, enqueue
982 // bias with a different sensor type, then restore the sensType
983 if (sensor->buffer.firstSample.numSamples > 0) {
984 enqueueSensorBuffer(sensor);
985 }
986 sensor->buffer.sensType = sensor->biasReportType;
987 copyTripleSamples(sensor, triple);
988 if (sensor->buffer.firstSample.numSamples > 0) {
989 enqueueSensorBuffer(sensor);
990 }
991 sensor->buffer.sensType = sensType;
992 }
993 }
994
copyTripleSamplesRaw(struct ActiveSensor * sensor,const struct TripleAxisDataEvent * triple)995 static void copyTripleSamplesRaw(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
996 {
997 int i;
998 uint32_t deltaTime;
999 uint8_t numSamples;
1000
1001 // Bias not supported in raw format; treat as regular format triple samples (potentially
1002 // handling alternate bias report type)
1003 if (triple->samples[0].firstSample.biasPresent) {
1004 copyTripleSamplesBias(sensor, triple);
1005 return;
1006 }
1007
1008 for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
1009 if (sensor->buffer.firstSample.numSamples == sensor->packetSamples) {
1010 enqueueSensorBuffer(sensor);
1011 }
1012
1013 if (sensor->buffer.firstSample.numSamples == 0) {
1014 if (i == 0) {
1015 sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
1016 } else {
1017 sensor->lastTime += triple->samples[i].deltaTime;
1018 sensor->buffer.referenceTime = sensor->lastTime;
1019 }
1020 sensor->buffer.length = sizeof(struct RawTripleAxisDataEvent) + sizeof(struct RawTripleAxisDataPoint);
1021 sensor->buffer.rawTriple[0].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
1022 sensor->buffer.rawTriple[0].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
1023 sensor->buffer.rawTriple[0].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
1024 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1025 mWakeupBlocks++;
1026 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1027 mNonWakeupBlocks++;
1028 sensor->buffer.firstSample.numSamples = 1;
1029 sensor->buffer.firstSample.interrupt = sensor->interrupt;
1030 if (sensor->curSamples++ == 0)
1031 sensor->firstTime = sensor->buffer.referenceTime;
1032 } else {
1033 if (i == 0) {
1034 if (sensor->lastTime > triple->referenceTime) {
1035 // shouldn't happen. flush current packet
1036 enqueueSensorBuffer(sensor);
1037 i--;
1038 } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
1039 enqueueSensorBuffer(sensor);
1040 i--;
1041 } else {
1042 deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
1043 numSamples = sensor->buffer.firstSample.numSamples;
1044
1045 sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
1046 sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime;
1047 sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[0].x * sensor->rawScale);
1048 sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[0].y * sensor->rawScale);
1049 sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[0].z * sensor->rawScale);
1050 sensor->lastTime = triple->referenceTime;
1051 sensor->buffer.firstSample.numSamples++;
1052 sensor->curSamples++;
1053 }
1054 } else {
1055 deltaTime = triple->samples[i].deltaTime;
1056 numSamples = sensor->buffer.firstSample.numSamples;
1057
1058 sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
1059 sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
1060 sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
1061 sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
1062 sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
1063 sensor->lastTime += deltaTime;
1064 sensor->buffer.firstSample.numSamples++;
1065 sensor->curSamples++;
1066 }
1067 }
1068 }
1069 }
1070
hostIntfAddBlock(struct HostIntfDataBuffer * data,bool discardable)1071 static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable)
1072 {
1073 if (!simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, discardable))
1074 return;
1075
1076 if (data->interrupt == NANOHUB_INT_WAKEUP)
1077 mWakeupBlocks++;
1078 else if (data->interrupt == NANOHUB_INT_NONWAKEUP)
1079 mNonWakeupBlocks++;
1080 nanohubPrefetchTx(data->interrupt, mWakeupBlocks, mNonWakeupBlocks);
1081 }
1082
hostIntfNotifyReboot(uint32_t reason)1083 static void hostIntfNotifyReboot(uint32_t reason)
1084 {
1085 struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp));
1086 __le32 raw_reason = htole32(reason);
1087
1088 if (resp) {
1089 resp->hdr = (struct NanohubHalHdr){
1090 .appId = APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0),
1091 .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
1092 .msg = NANOHUB_HAL_REBOOT,
1093 };
1094 memcpy(&resp->reason, &raw_reason, sizeof(resp->reason));
1095 osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
1096 }
1097 }
1098
queueFlush(struct ActiveSensor * sensor)1099 static void queueFlush(struct ActiveSensor *sensor)
1100 {
1101 if (sensor->buffer.length == 0) {
1102 sensor->buffer.length = sizeof(sensor->buffer.referenceTime) + sizeof(struct SensorFirstSample);
1103 sensor->buffer.referenceTime = 0ull;
1104 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1105 mWakeupBlocks++;
1106 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1107 mNonWakeupBlocks++;
1108 sensor->buffer.firstSample.numFlushes = 1;
1109 } else {
1110 sensor->buffer.firstSample.numFlushes++;
1111 }
1112 sensor->discard = false;
1113 hostIntfSetInterrupt(sensor->interrupt);
1114 }
1115
fakeFlush(struct ConfigCmd * cmd)1116 static void fakeFlush(struct ConfigCmd *cmd)
1117 {
1118 struct HostIntfDataBuffer *buffer;
1119 uint8_t size = sizeof(buffer->evtType) + sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1120 buffer = alloca(size);
1121 memset(buffer, 0x00, size);
1122
1123 buffer->sensType = cmd->sensType;
1124 buffer->length = sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
1125 buffer->interrupt = NANOHUB_INT_WAKEUP;
1126 mWakeupBlocks++;
1127 buffer->firstSample.numFlushes = 1;
1128 if (!simpleQueueEnqueue(mOutputQ, buffer, size, false))
1129 mWakeupBlocks--;
1130 }
1131
hostIntfHandleEvent(uint32_t evtType,const void * evtData)1132 static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
1133 {
1134 struct ConfigCmd *cmd;
1135 uint32_t i, cnt;
1136 uint64_t sensorTime;
1137 struct ActiveSensor *sensor;
1138 uint32_t tempSensorHandle;
1139 const struct HostHubRawPacket *hostMsg;
1140 struct HostIntfDataBuffer *data;
1141 const struct NanohubHalCommand *halCmd;
1142 const uint8_t *halMsg;
1143 uint32_t reason;
1144 uint32_t interrupt = HOSTINTF_MAX_INTERRUPTS;
1145
1146 if (evtType == EVT_APP_START) {
1147 if (initSensors()) {
1148 osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
1149 osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
1150 osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
1151 #ifdef DEBUG_LOG_EVT
1152 osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
1153 platEarlyLogFlush();
1154 #endif
1155 reason = pwrResetReason();
1156 data = alloca(sizeof(uint32_t) + sizeof(reason));
1157 data->sensType = SENS_TYPE_INVALID;
1158 data->length = sizeof(reason);
1159 data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON;
1160 data->interrupt = NANOHUB_INT_WAKEUP;
1161 memcpy(data->buffer, &reason, sizeof(reason));
1162 hostIntfAddBlock(data, false);
1163 hostIntfNotifyReboot(reason);
1164 }
1165 } else if (evtType == EVT_APP_TO_HOST) {
1166 hostMsg = evtData;
1167 if (hostMsg->dataLen <= HOST_HUB_RAW_PACKET_MAX_LEN) {
1168 data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->dataLen);
1169 data->sensType = SENS_TYPE_INVALID;
1170 data->length = sizeof(*hostMsg) + hostMsg->dataLen;
1171 data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
1172 data->interrupt = NANOHUB_INT_WAKEUP;
1173 memcpy(data->buffer, evtData, data->length);
1174 hostIntfAddBlock(data, false);
1175 }
1176 } else if (evtType == EVT_APP_FROM_HOST) {
1177 halMsg = evtData;
1178 if ((halCmd = nanohubHalFindCommand(halMsg[1])))
1179 halCmd->handler((void *)&halMsg[2], halMsg[0] - 1);
1180 }
1181 #ifdef DEBUG_LOG_EVT
1182 else if (evtType == EVT_DEBUG_LOG) {
1183 data = (struct HostIntfDataBuffer *)evtData;
1184 if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG) {
1185 hostIntfAddBlock(data, true);
1186 }
1187 }
1188 #endif
1189 else if (evtType == EVT_LATENCY_TIMER) {
1190 sensorTime = sensorGetTime();
1191
1192 for (i = 0, cnt = 0; i < mNumSensors && cnt < mLatencyCnt; i++) {
1193 if (mActiveSensorTable[i].latency > 0) {
1194 cnt++;
1195 if (mActiveSensorTable[i].firstTime && sensorTime >= mActiveSensorTable[i].firstTime + mActiveSensorTable[i].latency) {
1196 hostIntfSetInterrupt(mActiveSensorTable[i].interrupt);
1197 }
1198 }
1199 }
1200 } else if (evtType == EVT_NO_SENSOR_CONFIG_EVENT) { // config
1201 cmd = (struct ConfigCmd *)evtData;
1202 if (cmd->sensType > SENS_TYPE_INVALID && cmd->sensType <= SENS_TYPE_LAST_USER && mSensorList[cmd->sensType - 1] < MAX_REGISTERED_SENSORS) {
1203 sensor = mActiveSensorTable + mSensorList[cmd->sensType - 1];
1204
1205 if (sensor->sensorHandle) {
1206 if (cmd->cmd == CONFIG_CMD_FLUSH) {
1207 sensorFlush(sensor->sensorHandle);
1208 } else if (cmd->cmd == CONFIG_CMD_ENABLE) {
1209 if (sensorRequestRateChange(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1210 sensor->rate = cmd->rate;
1211 if (sensor->latency != cmd->latency) {
1212 if (!sensor->latency) {
1213 if (mLatencyCnt++ == 0)
1214 mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1215 } else if (!cmd->latency) {
1216 if (--mLatencyCnt == 0) {
1217 timTimerCancel(mLatencyTimer);
1218 mLatencyTimer = 0;
1219 }
1220 }
1221 sensor->latency = cmd->latency;
1222 }
1223 }
1224 } else if (cmd->cmd == CONFIG_CMD_DISABLE) {
1225 sensorRelease(mHostIntfTid, sensor->sensorHandle);
1226 osEventUnsubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1227 if (sensor->latency) {
1228 if (--mLatencyCnt == 0) {
1229 timTimerCancel(mLatencyTimer);
1230 mLatencyTimer = 0;
1231 }
1232 }
1233 sensor->rate = 0;
1234 sensor->latency = 0;
1235 sensor->oneshot = false;
1236 sensor->sensorHandle = 0;
1237 if (sensor->buffer.length) {
1238 enqueueSensorBuffer(sensor);
1239 hostIntfSetInterrupt(sensor->interrupt);
1240 }
1241 }
1242 } else if (cmd->cmd == CONFIG_CMD_ENABLE) {
1243 for (i = 0; sensorFind(cmd->sensType, i, &sensor->sensorHandle) != NULL; i++) {
1244 if (cmd->rate == SENSOR_RATE_ONESHOT) {
1245 cmd->rate = SENSOR_RATE_ONCHANGE;
1246 sensor->oneshot = true;
1247 } else {
1248 sensor->oneshot = false;
1249 }
1250
1251 if (sensorRequest(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
1252 if (cmd->latency) {
1253 if (mLatencyCnt++ == 0)
1254 mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
1255 }
1256 sensor->rate = cmd->rate;
1257 sensor->latency = cmd->latency;
1258 osEventSubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
1259 break;
1260 } else {
1261 sensor->sensorHandle = 0;
1262 }
1263 }
1264 } else if (cmd->cmd == CONFIG_CMD_CALIBRATE) {
1265 for (i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1266 sensorCalibrate(tempSensorHandle);
1267 } else if (cmd->cmd == CONFIG_CMD_SELF_TEST) {
1268 for (i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1269 sensorSelfTest(tempSensorHandle);
1270 } else if (cmd->cmd == CONFIG_CMD_CFG_DATA) {
1271 for (i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
1272 sensorCfgData(tempSensorHandle, (void *)(cmd+1));
1273 } else if (cmd->cmd == CONFIG_CMD_FLUSH) {
1274 queueFlush(sensor);
1275 }
1276 } else if (cmd->cmd == CONFIG_CMD_FLUSH && cmd->sensType > SENS_TYPE_INVALID) {
1277 // if a flush event is for an unknown sensor, we just return a fake flush event.
1278 osLog(LOG_INFO, "Flush request from unrecognized sensor, returning a fake flush\n");
1279 fakeFlush(cmd);
1280 }
1281 } else if (evtType > EVT_NO_FIRST_SENSOR_EVENT && evtType < EVT_NO_SENSOR_CONFIG_EVENT && mSensorList[(evtType & 0xFF)-1] < MAX_REGISTERED_SENSORS) { // data
1282 sensor = mActiveSensorTable + mSensorList[(evtType & 0xFF) - 1];
1283
1284 if (sensor->sensorHandle) {
1285 if (evtData == SENSOR_DATA_EVENT_FLUSH) {
1286 queueFlush(sensor);
1287 } else {
1288 bool haveFlush = sensor->buffer.firstSample.numFlushes > 0;
1289 if (sensor->buffer.length > 0 &&
1290 (haveFlush || sensor->buffer.firstSample.numSamples == sensor->packetSamples)) {
1291 // processing will be aborted if we have pending flush and are not able to send;
1292 // in this case, send eventually will be retried, otherwise data will be lost.
1293 if (enqueueSensorBufferEx(sensor, !haveFlush) < 0)
1294 return;
1295 }
1296
1297 switch (sensor->numAxis) {
1298 case NUM_AXIS_EMBEDDED:
1299 sensorTime = sensorGetTime();
1300 if (sensor->buffer.length > 0 && sensorTime - sensor->lastTime >= delta_time_max) {
1301 enqueueSensorBuffer(sensor);
1302 }
1303 if (sensor->buffer.length == 0) {
1304 sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
1305 sensor->lastTime = sensor->buffer.referenceTime = sensorTime;
1306 if (sensor->interrupt == NANOHUB_INT_WAKEUP)
1307 mWakeupBlocks++;
1308 else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
1309 mNonWakeupBlocks++;
1310 sensor->buffer.firstSample.numSamples = 1;
1311 sensor->buffer.firstSample.interrupt = sensor->interrupt;
1312 sensor->buffer.single[0].idata = (uint32_t)evtData;
1313 } else {
1314 sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
1315 sensor->buffer.single[sensor->buffer.firstSample.numSamples].deltaTime = encodeDeltaTime(sensorTime - sensor->lastTime);
1316 sensor->lastTime = sensorTime;
1317 sensor->buffer.single[sensor->buffer.firstSample.numSamples].idata = (uint32_t)evtData;
1318 sensor->buffer.firstSample.numSamples++;
1319 }
1320 if (sensor->curSamples++ == 0)
1321 sensor->firstTime = sensor->buffer.referenceTime;
1322 break;
1323 case NUM_AXIS_ONE:
1324 copySingleSamples(sensor, evtData);
1325 break;
1326 case NUM_AXIS_THREE:
1327 if (sensor->raw)
1328 copyTripleSamplesRaw(sensor, evtData);
1329 else
1330 copyTripleSamples(sensor, evtData);
1331 break;
1332 default:
1333 return;
1334 }
1335 }
1336
1337 sensorTime = sensorGetTime();
1338
1339 if (sensor->firstTime &&
1340 ((sensorTime >= sensor->firstTime + sensor->latency) ||
1341 ((sensor->latency > sensorGetCurLatency(sensor->sensorHandle)) &&
1342 (sensorTime + sensorGetCurLatency(sensor->sensorHandle) > sensor->firstTime + sensor->latency)))) {
1343 interrupt = sensor->interrupt;
1344 } else if (mWakeupBlocks + mNonWakeupBlocks >= mTotalBlocks) {
1345 interrupt = sensor->interrupt;
1346 }
1347
1348 nanohubPrefetchTx(interrupt, mWakeupBlocks, mNonWakeupBlocks);
1349
1350 if (sensor->oneshot) {
1351 sensorRelease(mHostIntfTid, sensor->sensorHandle);
1352 osEventUnsubscribe(mHostIntfTid, evtType);
1353 sensor->sensorHandle = 0;
1354 sensor->oneshot = false;
1355 }
1356 } else if (evtData != SENSOR_DATA_EVENT_FLUSH) {
1357 // handle bias data which can be generated for sensors that are
1358 // not currently requested by the AP
1359 switch (sensor->numAxis) {
1360 case NUM_AXIS_THREE:
1361 if (((const struct TripleAxisDataEvent *)evtData)->samples[0].firstSample.biasPresent) {
1362 copyTripleSamplesBias(sensor, evtData);
1363 nanohubPrefetchTx(HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
1364 }
1365 break;
1366 default:
1367 break;
1368 }
1369 }
1370 }
1371 }
1372
hostIntfCopyInterrupts(void * dst,uint32_t numBits)1373 void hostIntfCopyInterrupts(void *dst, uint32_t numBits)
1374 {
1375 if (mInterrupt->numBits != numBits)
1376 return;
1377
1378 atomicBitsetBulkRead(mInterrupt, dst, numBits);
1379 }
1380
hostIntfClearInterrupts()1381 void hostIntfClearInterrupts()
1382 {
1383 uint32_t i;
1384
1385 for (i = 0; i < HOSTINTF_MAX_INTERRUPTS; i++) {
1386 if (atomicBitsetGetBit(mInterrupt, i))
1387 hostIntfClearInterrupt(i);
1388 }
1389 }
1390
hostIntfSetInterrupt(uint32_t bit)1391 void hostIntfSetInterrupt(uint32_t bit)
1392 {
1393 uint64_t state = cpuIntsOff();
1394 if (mHostIntfTid) {
1395 if (!atomicBitsetGetBit(mInterrupt, bit)) {
1396 atomicBitsetSetBit(mInterrupt, bit);
1397 if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1398 if (mInterruptCntWkup++ == 0)
1399 apIntSet(true);
1400 } else {
1401 if (mInterruptCntNonWkup++ == 0)
1402 apIntSet(false);
1403 }
1404 }
1405 }
1406 cpuIntsRestore(state);
1407 }
1408
hostIntfGetInterrupt(uint32_t bit)1409 bool hostIntfGetInterrupt(uint32_t bit)
1410 {
1411 return atomicBitsetGetBit(mInterrupt, bit);
1412 }
1413
hostIntfClearInterrupt(uint32_t bit)1414 void hostIntfClearInterrupt(uint32_t bit)
1415 {
1416 uint64_t state = cpuIntsOff();
1417 if (mHostIntfTid) {
1418 if (atomicBitsetGetBit(mInterrupt, bit)) {
1419 atomicBitsetClearBit(mInterrupt, bit);
1420 if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1421 if (--mInterruptCntWkup == 0)
1422 apIntClear(true);
1423 } else {
1424 if (--mInterruptCntNonWkup == 0)
1425 apIntClear(false);
1426 }
1427 }
1428 }
1429 cpuIntsRestore(state);
1430 }
1431
hostIntfSetInterruptMask(uint32_t bit)1432 void hostIntfSetInterruptMask(uint32_t bit)
1433 {
1434 uint64_t state = cpuIntsOff();
1435 if (mHostIntfTid) {
1436 if (!atomicBitsetGetBit(mInterruptMask, bit)) {
1437 atomicBitsetSetBit(mInterruptMask, bit);
1438 if (atomicBitsetGetBit(mInterrupt, bit)) {
1439 if (--mInterruptCntWkup == 0)
1440 apIntClear(true);
1441 if (mInterruptCntNonWkup++ == 0)
1442 apIntSet(false);
1443 }
1444 }
1445 }
1446 cpuIntsRestore(state);
1447 }
1448
hostIntfGetInterruptMask(uint32_t bit)1449 bool hostIntfGetInterruptMask(uint32_t bit)
1450 {
1451 return atomicBitsetGetBit(mInterruptMask, bit);
1452 }
1453
hostIntfClearInterruptMask(uint32_t bit)1454 void hostIntfClearInterruptMask(uint32_t bit)
1455 {
1456 uint64_t state = cpuIntsOff();
1457 if (mHostIntfTid) {
1458 if (atomicBitsetGetBit(mInterruptMask, bit)) {
1459 atomicBitsetClearBit(mInterruptMask, bit);
1460 if (atomicBitsetGetBit(mInterrupt, bit)) {
1461 if (mInterruptCntWkup++ == 0)
1462 apIntSet(true);
1463 if (--mInterruptCntNonWkup == 0)
1464 apIntClear(false);
1465 }
1466 }
1467 }
1468 cpuIntsRestore(state);
1469 }
1470
1471 INTERNAL_APP_INIT(APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
1472