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