Home | History | Annotate | Download | only in core
      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, uint64_t *latencyP)
    591 {
    592     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid);
    593 
    594     if (req) {
    595         if (rateP)
    596             *rateP = req->rate;
    597         if (*latencyP)
    598             *latencyP = req->latency;
    599         return true;
    600     } else {
    601         return false;
    602     }
    603 }
    604 
    605 static bool sensorAmendRequestor(uint32_t sensorHandle, uint32_t clientTid, uint32_t newRate, uint64_t newLatency)
    606 {
    607     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid);
    608 
    609     if (req) {
    610         req->rate = newRate;
    611         req->latency = newLatency;
    612         return true;
    613     } else {
    614         return false;
    615     }
    616 }
    617 
    618 static bool sensorDeleteRequestor(uint32_t sensorHandle, uint32_t clientTid)
    619 {
    620     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, clientTid);
    621 
    622     if (req) {
    623         req->rate = SENSOR_RATE_OFF;
    624         req->latency = SENSOR_LATENCY_INVALID;
    625         req->clientTid = 0;
    626         req->handle = 0;
    627         mem_reorder_barrier();
    628         slabAllocatorFree(mCliSensMatrix, req);
    629         return true;
    630     } else {
    631         return false;
    632     }
    633 }
    634 
    635 bool sensorRequest(uint32_t unusedTid, uint32_t sensorHandle, uint32_t rate, uint64_t latency)
    636 {
    637     struct Sensor* s = sensorFindByHandle(sensorHandle);
    638     uint32_t newSensorRate;
    639     uint64_t samplingPeriod;
    640     uint32_t clientTid;
    641 
    642     (void)unusedTid;
    643 
    644     if (!s || !s->initComplete)
    645         return false;
    646 
    647     clientTid = osGetCurrentTid();
    648 
    649     /* verify the rate is possible */
    650     newSensorRate = sensorCalcHwRate(s, rate, 0);
    651     if (newSensorRate == SENSOR_RATE_IMPOSSIBLE)
    652         return false;
    653 
    654     /* the latency should be lower bounded by sampling period */
    655     samplingPeriod = ((uint64_t)(1000000000 / rate)) << 10;
    656     latency = latency > samplingPeriod ? latency : samplingPeriod;
    657 
    658     /* record the request */
    659     if (!sensorAddRequestor(sensorHandle, clientTid, rate, latency))
    660         return false;
    661 
    662     /* update actual sensor if needed */
    663     sensorReconfig(s, newSensorRate, sensorCalcHwLatency(s));
    664 
    665     /* if onchange request, ask sensor to send last state */
    666     if (s->hasOnchange && !sensorCallFuncSendOneDirectEvt(s, clientTid))
    667         osLog(LOG_WARN, "Cannot send last state for onchange sensor: enqueue fail");
    668 
    669     return true;
    670 }
    671 
    672 bool sensorRequestRateChange(uint32_t unusedTid, uint32_t sensorHandle, uint32_t newRate, uint64_t newLatency)
    673 {
    674     struct Sensor* s = sensorFindByHandle(sensorHandle);
    675     uint32_t oldRate, newSensorRate;
    676     uint64_t oldLatency, samplingPeriod;
    677     uint32_t clientTid;
    678 
    679     (void)unusedTid;
    680 
    681     if (!s)
    682         return false;
    683 
    684     clientTid = osGetCurrentTid();
    685     /* get current rate */
    686     if (!sensorGetCurRequestorRate(sensorHandle, clientTid, &oldRate, &oldLatency))
    687         return false;
    688 
    689     /* verify the new rate is possible given all other ongoing requests */
    690     newSensorRate = sensorCalcHwRate(s, newRate, oldRate);
    691     if (newSensorRate == SENSOR_RATE_IMPOSSIBLE)
    692         return false;
    693 
    694     /* the latency should be lower bounded by sampling period */
    695     samplingPeriod = ((uint64_t)(1000000000 / newRate)) << 10;
    696     newLatency = newLatency > samplingPeriod ? newLatency : samplingPeriod;
    697 
    698     /* record the request */
    699     if (!sensorAmendRequestor(sensorHandle, clientTid, newRate, newLatency))
    700         return false;
    701 
    702     /* update actual sensor if needed */
    703     sensorReconfig(s, newSensorRate, sensorCalcHwLatency(s));
    704     return true;
    705 }
    706 
    707 bool sensorRelease(uint32_t unusedTid, uint32_t sensorHandle)
    708 {
    709     struct Sensor* s = sensorFindByHandle(sensorHandle);
    710 
    711     (void) unusedTid;
    712 
    713     if (!s)
    714         return false;
    715 
    716     /* record the request */
    717     if (!sensorDeleteRequestor(sensorHandle, osGetCurrentTid()))
    718         return false;
    719 
    720     /* update actual sensor if needed */
    721     sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s));
    722     return true;
    723 }
    724 
    725 uint32_t sensorFreeAll(uint32_t clientTid)
    726 {
    727     int i;
    728     uint16_t count1 = 0, count2 = 0;
    729     struct Sensor *s;
    730 
    731     for (i = 0; i < MAX_REGISTERED_SENSORS; i++) {
    732         if (mSensors[i].handle) {
    733             s = mSensors + i;
    734             if (sensorDeleteRequestor(s->handle, clientTid)) {
    735                 sensorReconfig(s, sensorCalcHwRate(s, 0, 0), sensorCalcHwLatency(s));
    736                 count1 ++;
    737             }
    738             if (HANDLE_TO_TID(s->handle) == clientTid) {
    739                 sensorUnregister(s->handle);
    740                 count2 ++;
    741             }
    742         }
    743     }
    744 
    745     return (count1 << 16) | count2;
    746 }
    747 
    748 bool sensorTriggerOndemand(uint32_t unusedTid, uint32_t sensorHandle)
    749 {
    750     struct Sensor* s = sensorFindByHandle(sensorHandle);
    751 
    752     (void)unusedTid;
    753 
    754     if (!s || !s->hasOndemand)
    755         return false;
    756 
    757     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
    758 
    759     if (req)
    760         return sensorCallFuncTrigger(s);
    761 
    762     // not found -> do not report
    763     return false;
    764 }
    765 
    766 bool sensorFlush(uint32_t sensorHandle)
    767 {
    768     struct Sensor* s = sensorFindByHandle(sensorHandle);
    769 
    770     if (!s)
    771         return false;
    772 
    773     return sensorCallFuncFlush(s);
    774 }
    775 
    776 bool sensorCalibrate(uint32_t sensorHandle)
    777 {
    778     struct Sensor* s = sensorFindByHandle(sensorHandle);
    779 
    780     if (!s)
    781         return false;
    782 
    783     return sensorCallFuncCalibrate(s);
    784 }
    785 
    786 bool sensorSelfTest(uint32_t sensorHandle)
    787 {
    788     struct Sensor* s = sensorFindByHandle(sensorHandle);
    789 
    790     if (!s)
    791         return false;
    792 
    793     return sensorCallFuncSelfTest(s);
    794 }
    795 
    796 bool sensorCfgData(uint32_t sensorHandle, void* cfgData)
    797 {
    798     struct Sensor* s = sensorFindByHandle(sensorHandle);
    799 
    800     if (!s)
    801         return false;
    802 
    803     return sensorCallFuncCfgData(s, cfgData);
    804 }
    805 
    806 uint32_t sensorGetCurRate(uint32_t sensorHandle)
    807 {
    808     struct Sensor* s = sensorFindByHandle(sensorHandle);
    809 
    810     return s ? s->currentRate : SENSOR_RATE_OFF;
    811 }
    812 
    813 uint64_t sensorGetCurLatency(uint32_t sensorHandle)
    814 {
    815     struct Sensor* s = sensorFindByHandle(sensorHandle);
    816 
    817     return s ? s->currentLatency : SENSOR_LATENCY_INVALID;
    818 }
    819 
    820 uint32_t sensorGetReqRate(uint32_t sensorHandle)
    821 {
    822     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
    823 
    824     return req ? req->rate : SENSOR_RATE_OFF;
    825 }
    826 
    827 uint64_t sensorGetReqLatency(uint32_t sensorHandle)
    828 {
    829     struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
    830 
    831     return req ? req->latency : SENSOR_LATENCY_INVALID;
    832 }
    833 
    834 uint64_t sensorGetTime(void)
    835 {
    836     return rtcGetTime();
    837 }
    838 
    839 bool sensorGetInitComplete(uint32_t sensorHandle)
    840 {
    841     struct Sensor* s = sensorFindByHandle(sensorHandle);
    842 
    843     return s ? s->initComplete : false;
    844 }
    845 
    846 bool sensorMarshallEvent(uint32_t sensorHandle, uint32_t evtType, void *evtData, TaggedPtr *evtFreeingInfoP)
    847 {
    848     struct Sensor* s = sensorFindByHandle(sensorHandle);
    849 
    850     if (!s)
    851         return false;
    852 
    853     return sensorCallFuncMarshall(s, evtType, evtData, evtFreeingInfoP);
    854 }
    855