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