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