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 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
89static 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
133uint32_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
138uint32_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
144bool 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
157bool 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
174static 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
189static 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
212static bool sensorCallAsOwner(struct Sensor* s, bool (*callback)(void*))
213{
214    INVOKE_AS_OWNER_AND_RETURN(callback, s->callData);
215}
216
217static 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
225static void sensorCallFuncExternalEvtFreeF(void* event)
226{
227    slabAllocatorFree(mInternalEvents, event);
228}
229
230static 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
252static 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
260static 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
268static 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
276static 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
297static 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
320static 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
328static 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
350static 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
383static 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
402static 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
463static void sensorInternalEvtFreeF(void *evtP)
464{
465    slabAllocatorFree(mInternalEvents, evtP);
466}
467
468static 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
492static 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
520static 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
536bool 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
559const 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
574static 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
590static 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
603static 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
616static 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
633bool 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
670bool 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
705bool 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
723uint32_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
746bool 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
764bool 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
774bool 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
784bool 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
794bool 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
804uint32_t sensorGetCurRate(uint32_t sensorHandle)
805{
806    struct Sensor* s = sensorFindByHandle(sensorHandle);
807
808    return s ? s->currentRate : SENSOR_RATE_OFF;
809}
810
811uint64_t sensorGetCurLatency(uint32_t sensorHandle)
812{
813    struct Sensor* s = sensorFindByHandle(sensorHandle);
814
815    return s ? s->currentLatency : SENSOR_LATENCY_INVALID;
816}
817
818uint32_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
825uint64_t sensorGetHwLatency(uint32_t sensorHandle)
826{
827    struct Sensor* s = sensorFindByHandle(sensorHandle);
828
829    return s ? sensorCalcHwLatency(s) : SENSOR_LATENCY_INVALID;
830}
831
832uint32_t sensorGetReqRate(uint32_t sensorHandle)
833{
834    struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
835
836    return req ? req->rate : SENSOR_RATE_OFF;
837}
838
839uint64_t sensorGetReqLatency(uint32_t sensorHandle)
840{
841    struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
842
843    return req ? req->latency : SENSOR_LATENCY_INVALID;
844}
845
846uint64_t sensorGetTime(void)
847{
848    return rtcGetTime();
849}
850
851bool sensorGetInitComplete(uint32_t sensorHandle)
852{
853    struct Sensor* s = sensorFindByHandle(sensorHandle);
854
855    return s ? s->initComplete : false;
856}
857
858bool 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