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