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