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 <plat/taggedPtr.h> 18 #include <plat/rtc.h> 19 #include <cpu/barrier.h> 20 #include <atomicBitset.h> 21 #include <inttypes.h> 22 #include <sensors.h> 23 #include <atomic.h> 24 #include <stdio.h> 25 #include <slab.h> 26 #include <seos.h> 27 #include <util.h> 28 29 #include <sensors_priv.h> 30 31 32 static struct Sensor mSensors[MAX_REGISTERED_SENSORS]; 33 ATOMIC_BITSET_DECL(mSensorsUsed, MAX_REGISTERED_SENSORS, static); 34 static struct SlabAllocator *mInternalEvents; 35 static struct SlabAllocator *mCliSensMatrix; 36 static uint32_t mNextSensorHandle; 37 struct SingleAxisDataEvent singleAxisFlush = { .referenceTime = 0 }; 38 struct TripleAxisDataEvent tripleAxisFlush = { .referenceTime = 0 }; 39 40 static inline uint32_t newSensorHandle() 41 { 42 // FIXME: only let lower 8 bits of counter to the id; should use all 16 bits, but this 43 // somehow confuses upper layers; pending investigation 44 return (osGetCurrentTid() << 16) | (atomicAdd32bits(&mNextSensorHandle, 1) & 0xFF); 45 } 46 47 bool sensorsInit(void) 48 { 49 atomicBitsetInit(mSensorsUsed, MAX_REGISTERED_SENSORS); 50 51 mInternalEvents = slabAllocatorNew(sizeof(struct SensorsInternalEvent), alignof(struct SensorsInternalEvent), MAX_INTERNAL_EVENTS); 52 if (!mInternalEvents) 53 return false; 54 55 mCliSensMatrix = slabAllocatorNew(sizeof(struct SensorsClientRequest), alignof(struct SensorsClientRequest), MAX_CLI_SENS_MATRIX_SZ); 56 if (mCliSensMatrix) 57 return true; 58 59 slabAllocatorDestroy(mInternalEvents); 60 61 return false; 62 } 63 64 struct Sensor* sensorFindByHandle(uint32_t handle) 65 { 66 uint32_t i; 67 68 for (i = 0; i < MAX_REGISTERED_SENSORS; i++) 69 if (mSensors[i].handle == handle) 70 return mSensors + i; 71 72 return NULL; 73 } 74 75 static struct SensorsClientRequest* sensorClientRequestFind(uint32_t sensorHandle, uint32_t clientTid) 76 { 77 uint32_t i; 78 79 for (i = 0; i < MAX_CLI_SENS_MATRIX_SZ; i++) { 80 struct SensorsClientRequest *req = slabAllocatorGetNth(mCliSensMatrix, i); 81 82 if (req && req->handle == sensorHandle && req->clientTid == clientTid) 83 return req; 84 } 85 86 return NULL; 87 } 88 89 static uint32_t sensorRegisterEx(const struct SensorInfo *si, TaggedPtr callInfo, void *callData, bool initComplete) 90 { 91 int32_t idx = atomicBitsetFindClearAndSet(mSensorsUsed); 92 uint32_t handle, i; 93 struct Sensor *s; 94 95 /* grab a slot */ 96 if (idx < 0) 97 return 0; 98 99 /* grab a handle: 100 * this is safe since nobody else could have "JUST" taken this handle, 101 * we'll need to circle around 16 bits before that happens, and have the same TID 102 */ 103 do { 104 handle = newSensorHandle(); 105 } while (!handle || sensorFindByHandle(handle)); 106 107 /* fill the struct in and mark it valid (by setting handle) */ 108 s = mSensors + idx; 109 s->si = si; 110 s->currentRate = SENSOR_RATE_OFF; 111 s->currentLatency = SENSOR_LATENCY_INVALID; 112 s->callInfo = callInfo; 113 // TODO: is internal app, callinfo is OPS struct; shall we validate it here? 114 s->callData = callData; 115 s->initComplete = initComplete ? 1 : 0; 116 mem_reorder_barrier(); 117 s->handle = handle; 118 s->hasOnchange = 0; 119 s->hasOndemand = 0; 120 121 if (si->supportedRates) { 122 for (i = 0; si->supportedRates[i]; i++) { 123 if (si->supportedRates[i] == SENSOR_RATE_ONCHANGE) 124 s->hasOnchange = 1; 125 if (si->supportedRates[i] == SENSOR_RATE_ONDEMAND) 126 s->hasOndemand = 1; 127 } 128 } 129 130 return handle; 131 } 132 133 uint32_t sensorRegister(const struct SensorInfo *si, const struct SensorOps *ops, void *callData, bool initComplete) 134 { 135 return sensorRegisterEx(si, taggedPtrMakeFromPtr(ops), callData, initComplete); 136 } 137 138 uint32_t sensorRegisterAsApp(const struct SensorInfo *si, uint32_t unusedTid, void *callData, bool initComplete) 139 { 140 (void)unusedTid; 141 return sensorRegisterEx(si, taggedPtrMakeFromUint(0), callData, initComplete); 142 } 143 144 bool sensorRegisterInitComplete(uint32_t handle) 145 { 146 struct Sensor *s = sensorFindByHandle(handle); 147 148 if (!s) 149 return false; 150 151 s->initComplete = true; 152 mem_reorder_barrier(); 153 154 return true; 155 } 156 157 bool sensorUnregister(uint32_t handle) 158 { 159 struct Sensor *s = sensorFindByHandle(handle); 160 161 if (!s) 162 return false; 163 164 /* mark as invalid */ 165 s->handle = 0; 166 mem_reorder_barrier(); 167 168 /* free struct */ 169 atomicBitsetClearBit(mSensorsUsed, s - mSensors); 170 171 return true; 172 } 173 174 static void sensorCallFuncPowerEvtFreeF(void* event) 175 { 176 slabAllocatorFree(mInternalEvents, event); 177 } 178 179 #define INVOKE_AS_OWNER_AND_RETURN(func, ...) \ 180 { \ 181 if (!func) \ 182 return false; \ 183 uint16_t oldTid = osSetCurrentTid(HANDLE_TO_TID(s->handle)); \ 184 bool done = func(__VA_ARGS__); \ 185 osSetCurrentTid(oldTid); \ 186 return done; \ 187 } 188 189 static bool sensorCallFuncPower(struct Sensor* s, bool on) 190 { 191 if (IS_LOCAL_APP(s)) { 192 INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorPower, on, s->callData); 193 } else { 194 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 195 196 if (!evt) 197 return false; 198 199 evt->externalPowerEvt.on = on; 200 evt->externalPowerEvt.callData = s->callData; 201 202 if (osEnqueuePrivateEvt(EVT_APP_SENSOR_POWER, &evt->externalPowerEvt, 203 sensorCallFuncPowerEvtFreeF, EXT_APP_TID(s))) 204 return true; 205 206 slabAllocatorFree(mInternalEvents, evt); 207 return false; 208 } 209 } 210 211 // the most common callback goes as a helper function 212 static bool sensorCallAsOwner(struct Sensor* s, bool (*callback)(void*)) 213 { 214 INVOKE_AS_OWNER_AND_RETURN(callback, s->callData); 215 } 216 217 static bool sensorCallFuncFwUpld(struct Sensor* s) 218 { 219 if (IS_LOCAL_APP(s)) 220 return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorFirmwareUpload); 221 else 222 return osEnqueuePrivateEvt(EVT_APP_SENSOR_FW_UPLD, s->callData, NULL, EXT_APP_TID(s)); 223 } 224 225 static void sensorCallFuncExternalEvtFreeF(void* event) 226 { 227 slabAllocatorFree(mInternalEvents, event); 228 } 229 230 static bool sensorCallFuncSetRate(struct Sensor* s, uint32_t rate, uint64_t latency) 231 { 232 if (IS_LOCAL_APP(s)) { 233 INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorSetRate, rate, latency, s->callData); 234 } else { 235 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 236 237 if (!evt) 238 return false; 239 240 evt->externalSetRateEvt.latency = latency; 241 evt->externalSetRateEvt.rate = rate; 242 evt->externalSetRateEvt.callData = s->callData; 243 if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SET_RATE, &evt->externalSetRateEvt, 244 sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s))) 245 return true; 246 247 slabAllocatorFree(mInternalEvents, evt); 248 return false; 249 } 250 } 251 252 static bool sensorCallFuncCalibrate(struct Sensor* s) 253 { 254 if (IS_LOCAL_APP(s)) 255 return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorCalibrate); 256 else 257 return osEnqueuePrivateEvt(EVT_APP_SENSOR_CALIBRATE, s->callData, NULL, EXT_APP_TID(s)); 258 } 259 260 static bool sensorCallFuncSelfTest(struct Sensor* s) 261 { 262 if (IS_LOCAL_APP(s)) 263 return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorSelfTest); 264 else 265 return osEnqueuePrivateEvt(EVT_APP_SENSOR_SELF_TEST, s->callData, NULL, EXT_APP_TID(s)); 266 } 267 268 static bool sensorCallFuncFlush(struct Sensor* s) 269 { 270 if (IS_LOCAL_APP(s)) 271 return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorFlush); 272 else 273 return osEnqueuePrivateEvt(EVT_APP_SENSOR_FLUSH, s->callData, NULL, EXT_APP_TID(s)); 274 } 275 276 static bool sensorCallFuncCfgData(struct Sensor* s, void* cfgData) 277 { 278 if (IS_LOCAL_APP(s)) { 279 INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorCfgData, cfgData, s->callData); 280 } else { 281 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 282 283 if (!evt) 284 return false; 285 286 evt->externalCfgDataEvt.data = cfgData; 287 evt->externalCfgDataEvt.callData = s->callData; 288 if (osEnqueuePrivateEvt(EVT_APP_SENSOR_CFG_DATA, &evt->externalCfgDataEvt, 289 sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s))) 290 return true; 291 292 slabAllocatorFree(mInternalEvents, evt); 293 return false; 294 } 295 } 296 297 static bool sensorCallFuncMarshall(struct Sensor* s, uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP) 298 { 299 if (IS_LOCAL_APP(s)) { 300 INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorMarshallData, evtType, evtData, evtFreeingInfoP, s->callData); 301 } else { 302 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 303 304 if (!evt) 305 return false; 306 307 evt->externalMarshallEvt.origEvtType = evtType; 308 evt->externalMarshallEvt.origEvtData = evtData; 309 evt->externalMarshallEvt.evtFreeingInfo = *evtFreeingInfoP; 310 evt->externalMarshallEvt.callData = s->callData; 311 if (osEnqueuePrivateEvt(EVT_APP_SENSOR_MARSHALL, &evt->externalMarshallEvt, 312 sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s))) 313 return true; 314 315 slabAllocatorFree(mInternalEvents, evt); 316 return false; 317 } 318 } 319 320 static bool sensorCallFuncTrigger(struct Sensor* s) 321 { 322 if (IS_LOCAL_APP(s)) 323 return sensorCallAsOwner(s, LOCAL_APP_OPS(s)->sensorTriggerOndemand); 324 else 325 return osEnqueuePrivateEvt(EVT_APP_SENSOR_TRIGGER, s->callData, NULL, EXT_APP_TID(s)); 326 } 327 328 static bool sensorCallFuncSendOneDirectEvt(struct Sensor* s, uint32_t tid) 329 { 330 if (IS_LOCAL_APP(s)) { 331 INVOKE_AS_OWNER_AND_RETURN(LOCAL_APP_OPS(s)->sensorSendOneDirectEvt, s->callData, tid); 332 } else { 333 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 334 335 if (!evt) 336 return false; 337 338 evt->externalSendDirectEvt.tid = tid; 339 evt->externalSendDirectEvt.callData = s->callData; 340 if (osEnqueuePrivateEvt(EVT_APP_SENSOR_SEND_ONE_DIR_EVT, &evt->externalSendDirectEvt, 341 sensorCallFuncExternalEvtFreeF, EXT_APP_TID(s))) 342 return true; 343 344 slabAllocatorFree(mInternalEvents, evt); 345 } 346 347 return false; 348 } 349 350 static void sensorReconfig(struct Sensor* s, uint32_t newHwRate, uint64_t newHwLatency) 351 { 352 if (s->currentRate == newHwRate && s->currentLatency == newHwLatency) { 353 /* do nothing */ 354 } 355 else if (s->currentRate == SENSOR_RATE_OFF) { 356 /* if it was off or is off, tell it to come on */ 357 if (sensorCallFuncPower(s, true)) { 358 s->currentRate = SENSOR_RATE_POWERING_ON; 359 s->currentLatency = SENSOR_LATENCY_INVALID; 360 } 361 } 362 else if (s->currentRate == SENSOR_RATE_POWERING_OFF) { 363 /* if it was going to be off or is off, tell it to come back on */ 364 s->currentRate = SENSOR_RATE_POWERING_ON; 365 s->currentLatency = SENSOR_LATENCY_INVALID; 366 } 367 else if (s->currentRate == SENSOR_RATE_POWERING_ON || s->currentRate == SENSOR_RATE_FW_UPLOADING) { 368 /* if it is powering on - do nothing - all will be done for us */ 369 } 370 else if (newHwRate > SENSOR_RATE_OFF || newHwLatency < SENSOR_LATENCY_INVALID) { 371 /* simple rate change - > do it, there is nothing we can do if this fails, so we ignore the immediate errors :( */ 372 (void)sensorCallFuncSetRate(s, newHwRate, newHwLatency); 373 } 374 else { 375 /* powering off */ 376 if (sensorCallFuncPower(s, false)) { 377 s->currentRate = SENSOR_RATE_POWERING_OFF; 378 s->currentLatency = SENSOR_LATENCY_INVALID; 379 } 380 } 381 } 382 383 static uint64_t sensorCalcHwLatency(struct Sensor* s) 384 { 385 uint64_t smallestLatency = SENSOR_LATENCY_INVALID; 386 uint32_t i; 387 388 for (i = 0; i < MAX_CLI_SENS_MATRIX_SZ; i++) { 389 struct SensorsClientRequest *req = slabAllocatorGetNth(mCliSensMatrix, i); 390 391 /* we only care about this sensor's stuff */ 392 if (!req || req->handle != s->handle) 393 continue; 394 395 if (smallestLatency > req->latency) 396 smallestLatency = req->latency; 397 } 398 399 return smallestLatency; 400 } 401 402 static uint32_t sensorCalcHwRate(struct Sensor* s, uint32_t extraReqedRate, uint32_t removedRate) 403 { 404 bool haveUsers = false, haveOnChange = extraReqedRate == SENSOR_RATE_ONCHANGE; 405 uint32_t highestReq = 0; 406 uint32_t i; 407 408 if (s->si->supportedRates && 409 ((extraReqedRate == SENSOR_RATE_ONCHANGE && !s->hasOnchange) || 410 (extraReqedRate == SENSOR_RATE_ONDEMAND && !s->hasOndemand))) { 411 osLog(LOG_WARN, "Bad rate 0x%08" PRIX32 " for sensor %u", extraReqedRate, s->si->sensorType); 412 return SENSOR_RATE_IMPOSSIBLE; 413 } 414 415 if (extraReqedRate) { 416 haveUsers = true; 417 highestReq = (extraReqedRate == SENSOR_RATE_ONDEMAND || extraReqedRate == SENSOR_RATE_ONCHANGE) ? 0 : extraReqedRate; 418 } 419 420 for (i = 0; i < MAX_CLI_SENS_MATRIX_SZ; i++) { 421 struct SensorsClientRequest *req = slabAllocatorGetNth(mCliSensMatrix, i); 422 423 /* we only care about this sensor's stuff */ 424 if (!req || req->handle != s->handle) 425 continue; 426 427 /* skip an instance of a removed rate if one was given */ 428 if (req->rate == removedRate) { 429 removedRate = SENSOR_RATE_OFF; 430 continue; 431 } 432 433 haveUsers = true; 434 435 /* we can always do ondemand and if we see an on-change then we already checked and do allow it */ 436 if (req->rate == SENSOR_RATE_ONDEMAND) 437 continue; 438 if (req->rate == SENSOR_RATE_ONCHANGE) { 439 haveOnChange = true; 440 continue; 441 } 442 443 if (highestReq < req->rate) 444 highestReq = req->rate; 445 } 446 447 if (!highestReq) { /* no requests -> we can definitely do that */ 448 if (!haveUsers) 449 return SENSOR_RATE_OFF; 450 else if (haveOnChange) 451 return SENSOR_RATE_ONCHANGE; 452 else 453 return SENSOR_RATE_ONDEMAND; 454 } 455 456 for (i = 0; s->si->supportedRates && s->si->supportedRates[i]; i++) 457 if (s->si->supportedRates[i] >= highestReq) 458 return s->si->supportedRates[i]; 459 460 return SENSOR_RATE_IMPOSSIBLE; 461 } 462 463 static void sensorInternalEvtFreeF(void *evtP) 464 { 465 slabAllocatorFree(mInternalEvents, evtP); 466 } 467 468 static void sensorInternalFwStateChanged(void *evtP) 469 { 470 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)evtP; 471 struct Sensor* s = sensorFindByHandle(evt->handle); 472 473 if (s) { 474 475 if (!evt->value1) { //we failed -> give up 476 s->currentRate = SENSOR_RATE_POWERING_OFF; 477 s->currentLatency = SENSOR_LATENCY_INVALID; 478 sensorCallFuncPower(s, false); 479 } 480 else if (s->currentRate == SENSOR_RATE_FW_UPLOADING) { //we're up 481 s->currentRate = evt->value1; 482 s->currentLatency = evt->value2; 483 sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s)); 484 } 485 else if (s->currentRate == SENSOR_RATE_POWERING_OFF) { //we need to power off 486 sensorCallFuncPower(s, false); 487 } 488 } 489 slabAllocatorFree(mInternalEvents, evt); 490 } 491 492 static void sensorInternalPowerStateChanged(void *evtP) 493 { 494 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)evtP; 495 struct Sensor* s = sensorFindByHandle(evt->handle); 496 497 if (s) { 498 499 if (s->currentRate == SENSOR_RATE_POWERING_ON && evt->value1) { //we're now on - upload firmware 500 s->currentRate = SENSOR_RATE_FW_UPLOADING; 501 s->currentLatency = SENSOR_LATENCY_INVALID; 502 sensorCallFuncFwUpld(s); 503 } 504 else if (s->currentRate == SENSOR_RATE_POWERING_OFF && !evt->value1) { //we're now off 505 s->currentRate = SENSOR_RATE_OFF; 506 s->currentLatency = SENSOR_LATENCY_INVALID; 507 osEnqueueEvtOrFree(sensorGetMyCfgEventType(s->si->sensorType), evt, sensorInternalEvtFreeF); 508 return; 509 } 510 else if (s->currentRate == SENSOR_RATE_POWERING_ON && !evt->value1) { //we need to power back on 511 sensorCallFuncPower(s, true); 512 } 513 else if (s->currentRate == SENSOR_RATE_POWERING_OFF && evt->value1) { //we need to power back off 514 sensorCallFuncPower(s, false); 515 } 516 } 517 slabAllocatorFree(mInternalEvents, evt); 518 } 519 520 static void sensorInternalRateChanged(void *evtP) 521 { 522 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)evtP; 523 struct Sensor* s = sensorFindByHandle(evt->handle); 524 525 /* If the current rate is a state, do not change the rate */ 526 if (s && s->currentRate != SENSOR_RATE_OFF && s->currentRate < SENSOR_RATE_POWERING_ON) { 527 s->currentRate = evt->value1; 528 s->currentLatency = evt->value2; 529 sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s)); 530 osEnqueueEvtOrFree(sensorGetMyCfgEventType(s->si->sensorType), evt, sensorInternalEvtFreeF); 531 } else { 532 slabAllocatorFree(mInternalEvents, evt); 533 } 534 } 535 536 bool sensorSignalInternalEvt(uint32_t handle, uint32_t intEvtNum, uint32_t value1, uint64_t value2) 537 { 538 static const OsDeferCbkF internalEventCallbacks[] = { 539 [SENSOR_INTERNAL_EVT_POWER_STATE_CHG] = sensorInternalPowerStateChanged, 540 [SENSOR_INTERNAL_EVT_FW_STATE_CHG] = sensorInternalFwStateChanged, 541 [SENSOR_INTERNAL_EVT_RATE_CHG] = sensorInternalRateChanged, 542 }; 543 struct SensorsInternalEvent *evt = (struct SensorsInternalEvent*)slabAllocatorAlloc(mInternalEvents); 544 545 if (!evt) 546 return false; 547 548 evt->handle = handle; 549 evt->value1 = value1; 550 evt->value2 = value2; 551 552 if (osDefer(internalEventCallbacks[intEvtNum], evt, false)) 553 return true; 554 555 slabAllocatorFree(mInternalEvents, evt); 556 return false; 557 } 558 559 const struct SensorInfo* sensorFind(uint32_t sensorType, uint32_t idx, uint32_t *handleP) 560 { 561 uint32_t i; 562 563 for (i = 0; i < MAX_REGISTERED_SENSORS; i++) { 564 if (mSensors[i].handle && mSensors[i].si->sensorType == sensorType && !idx--) { 565 if (handleP) 566 *handleP = mSensors[i].handle; 567 return mSensors[i].si; 568 } 569 } 570 571 return NULL; 572 } 573 574 static bool sensorAddRequestor(uint32_t sensorHandle, uint32_t clientTid, uint32_t rate, uint64_t latency) 575 { 576 struct SensorsClientRequest *req = slabAllocatorAlloc(mCliSensMatrix); 577 578 if (!req) 579 return false; 580 581 req->handle = sensorHandle; 582 req->clientTid = clientTid; 583 mem_reorder_barrier(); 584 req->rate = rate; 585 req->latency = latency; 586 587 return true; 588 } 589 590 static bool sensorGetCurRequestorRate(uint32_t sensorHandle, uint32_t clientTid, uint32_t *rateP) 591 { 592 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid); 593 594 if (req) { 595 if (rateP) 596 *rateP = req->rate; 597 return true; 598 } else { 599 return false; 600 } 601 } 602 603 static bool sensorAmendRequestor(uint32_t sensorHandle, uint32_t clientTid, uint32_t newRate, uint64_t newLatency) 604 { 605 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid); 606 607 if (req) { 608 req->rate = newRate; 609 req->latency = newLatency; 610 return true; 611 } else { 612 return false; 613 } 614 } 615 616 static bool sensorDeleteRequestor(uint32_t sensorHandle, uint32_t clientTid) 617 { 618 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid); 619 620 if (req) { 621 req->rate = SENSOR_RATE_OFF; 622 req->latency = SENSOR_LATENCY_INVALID; 623 req->clientTid = 0; 624 req->handle = 0; 625 mem_reorder_barrier(); 626 slabAllocatorFree(mCliSensMatrix, req); 627 return true; 628 } else { 629 return false; 630 } 631 } 632 633 bool sensorRequest(uint32_t unusedTid, uint32_t sensorHandle, uint32_t rate, uint64_t latency) 634 { 635 struct Sensor* s = sensorFindByHandle(sensorHandle); 636 uint32_t newSensorRate; 637 uint64_t samplingPeriod; 638 uint32_t clientTid; 639 640 (void)unusedTid; 641 642 if (!s || !s->initComplete) 643 return false; 644 645 clientTid = osGetCurrentTid(); 646 647 /* verify the rate is possible */ 648 newSensorRate = sensorCalcHwRate(s, rate, 0); 649 if (newSensorRate == SENSOR_RATE_IMPOSSIBLE) 650 return false; 651 652 /* the latency should be lower bounded by sampling period */ 653 samplingPeriod = ((uint64_t)(1000000000 / rate)) << 10; 654 latency = latency > samplingPeriod ? latency : samplingPeriod; 655 656 /* record the request */ 657 if (!sensorAddRequestor(sensorHandle, clientTid, rate, latency)) 658 return false; 659 660 /* update actual sensor if needed */ 661 sensorReconfig(s, newSensorRate, sensorCalcHwLatency(s)); 662 663 /* if onchange request, ask sensor to send last state */ 664 if (s->hasOnchange && !sensorCallFuncSendOneDirectEvt(s, clientTid)) 665 osLog(LOG_WARN, "Cannot send last state for onchange sensor: enqueue fail"); 666 667 return true; 668 } 669 670 bool sensorRequestRateChange(uint32_t unusedTid, uint32_t sensorHandle, uint32_t newRate, uint64_t newLatency) 671 { 672 struct Sensor* s = sensorFindByHandle(sensorHandle); 673 uint32_t oldRate, newSensorRate; 674 uint64_t samplingPeriod; 675 uint32_t clientTid; 676 677 (void)unusedTid; 678 679 if (!s) 680 return false; 681 682 clientTid = osGetCurrentTid(); 683 /* get current rate */ 684 if (!sensorGetCurRequestorRate(sensorHandle, clientTid, &oldRate)) 685 return false; 686 687 /* verify the new rate is possible given all other ongoing requests */ 688 newSensorRate = sensorCalcHwRate(s, newRate, oldRate); 689 if (newSensorRate == SENSOR_RATE_IMPOSSIBLE) 690 return false; 691 692 /* the latency should be lower bounded by sampling period */ 693 samplingPeriod = ((uint64_t)(1000000000 / newRate)) << 10; 694 newLatency = newLatency > samplingPeriod ? newLatency : samplingPeriod; 695 696 /* record the request */ 697 if (!sensorAmendRequestor(sensorHandle, clientTid, newRate, newLatency)) 698 return false; 699 700 /* update actual sensor if needed */ 701 sensorReconfig(s, newSensorRate, sensorCalcHwLatency(s)); 702 return true; 703 } 704 705 bool sensorRelease(uint32_t unusedTid, uint32_t sensorHandle) 706 { 707 struct Sensor* s = sensorFindByHandle(sensorHandle); 708 709 (void) unusedTid; 710 711 if (!s) 712 return false; 713 714 /* record the request */ 715 if (!sensorDeleteRequestor(sensorHandle, osGetCurrentTid())) 716 return false; 717 718 /* update actual sensor if needed */ 719 sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s)); 720 return true; 721 } 722 723 uint32_t sensorFreeAll(uint32_t clientTid) 724 { 725 int i; 726 uint16_t count1 = 0, count2 = 0; 727 struct Sensor *s; 728 729 for (i = 0; i < MAX_REGISTERED_SENSORS; i++) { 730 if (mSensors[i].handle) { 731 s = mSensors + i; 732 if (sensorDeleteRequestor(s->handle, clientTid)) { 733 sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s)); 734 count1 ++; 735 } 736 if (HANDLE_TO_TID(s->handle) == clientTid) { 737 sensorUnregister(s->handle); 738 count2 ++; 739 } 740 } 741 } 742 743 return (count1 << 16) | count2; 744 } 745 746 bool sensorTriggerOndemand(uint32_t unusedTid, uint32_t sensorHandle) 747 { 748 struct Sensor* s = sensorFindByHandle(sensorHandle); 749 750 (void)unusedTid; 751 752 if (!s || !s->hasOndemand) 753 return false; 754 755 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid()); 756 757 if (req) 758 return sensorCallFuncTrigger(s); 759 760 // not found -> do not report 761 return false; 762 } 763 764 bool sensorFlush(uint32_t sensorHandle) 765 { 766 struct Sensor* s = sensorFindByHandle(sensorHandle); 767 768 if (!s) 769 return false; 770 771 return sensorCallFuncFlush(s); 772 } 773 774 bool sensorCalibrate(uint32_t sensorHandle) 775 { 776 struct Sensor* s = sensorFindByHandle(sensorHandle); 777 778 if (!s) 779 return false; 780 781 return sensorCallFuncCalibrate(s); 782 } 783 784 bool sensorSelfTest(uint32_t sensorHandle) 785 { 786 struct Sensor* s = sensorFindByHandle(sensorHandle); 787 788 if (!s) 789 return false; 790 791 return sensorCallFuncSelfTest(s); 792 } 793 794 bool sensorCfgData(uint32_t sensorHandle, void* cfgData) 795 { 796 struct Sensor* s = sensorFindByHandle(sensorHandle); 797 798 if (!s) 799 return false; 800 801 return sensorCallFuncCfgData(s, cfgData); 802 } 803 804 uint32_t sensorGetCurRate(uint32_t sensorHandle) 805 { 806 struct Sensor* s = sensorFindByHandle(sensorHandle); 807 808 return s ? s->currentRate : SENSOR_RATE_OFF; 809 } 810 811 uint64_t sensorGetCurLatency(uint32_t sensorHandle) 812 { 813 struct Sensor* s = sensorFindByHandle(sensorHandle); 814 815 return s ? s->currentLatency : SENSOR_LATENCY_INVALID; 816 } 817 818 uint32_t sensorGetHwRate(uint32_t sensorHandle) 819 { 820 struct Sensor* s = sensorFindByHandle(sensorHandle); 821 822 return s ? sensorCalcHwRate(s, 0, 0) : SENSOR_RATE_OFF; 823 } 824 825 uint64_t sensorGetHwLatency(uint32_t sensorHandle) 826 { 827 struct Sensor* s = sensorFindByHandle(sensorHandle); 828 829 return s ? sensorCalcHwLatency(s) : SENSOR_LATENCY_INVALID; 830 } 831 832 uint32_t sensorGetReqRate(uint32_t sensorHandle) 833 { 834 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid()); 835 836 return req ? req->rate : SENSOR_RATE_OFF; 837 } 838 839 uint64_t sensorGetReqLatency(uint32_t sensorHandle) 840 { 841 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid()); 842 843 return req ? req->latency : SENSOR_LATENCY_INVALID; 844 } 845 846 uint64_t sensorGetTime(void) 847 { 848 return rtcGetTime(); 849 } 850 851 bool sensorGetInitComplete(uint32_t sensorHandle) 852 { 853 struct Sensor* s = sensorFindByHandle(sensorHandle); 854 855 return s ? s->initComplete : false; 856 } 857 858 bool sensorMarshallEvent(uint32_t sensorHandle, uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP) 859 { 860 struct Sensor* s = sensorFindByHandle(sensorHandle); 861 862 if (!s) 863 return false; 864 865 return sensorCallFuncMarshall(s, evtType, evtData, evtFreeingInfoP); 866 } 867
