1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <plat/inc/taggedPtr.h>
18 #include <plat/inc/rtc.h>
19 #include <cpu/inc/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
44 struct 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
56 struct 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
71 struct SensorsClientRequest {
72 uint32_t handle;
73 uint32_t clientTid;
74 uint64_t latency;
75 uint32_t rate;
76 };
77
78 static struct Sensor mSensors[MAX_REGISTERED_SENSORS];
79 ATOMIC_BITSET_DECL(mSensorsUsed, MAX_REGISTERED_SENSORS, static);
80 static struct SlabAllocator *mInternalEvents;
81 static struct SlabAllocator *mCliSensMatrix;
82 static uint32_t mNextSensorHandle;
83 struct SingleAxisDataEvent singleAxisFlush = { .referenceTime = 0 };
84 struct TripleAxisDataEvent tripleAxisFlush = { .referenceTime = 0 };
85
newSensorHandle()86 static 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
sensorsInit(void)93 bool 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
sensorFindByHandle(uint32_t handle)110 static 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
sensorRegisterEx(const struct SensorInfo * si,TaggedPtr callInfo,void * callData,bool initComplete)121 static 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
sensorRegister(const struct SensorInfo * si,const struct SensorOps * ops,void * callData,bool initComplete)165 uint32_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
sensorRegisterAsApp(const struct SensorInfo * si,uint32_t unusedTid,void * callData,bool initComplete)170 uint32_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
sensorRegisterInitComplete(uint32_t handle)176 bool 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
sensorUnregister(uint32_t handle)189 bool 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
sensorCallFuncPowerEvtFreeF(void * event)206 static 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
sensorCallFuncPower(struct Sensor * s,bool on)221 static 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
sensorCallAsOwner(struct Sensor * s,bool (* callback)(void *))244 static bool sensorCallAsOwner(struct Sensor* s, bool (*callback)(void*))
245 {
246 INVOKE_AS_OWNER_AND_RETURN(callback, s->callData);
247 }
248
sensorCallFuncFwUpld(struct Sensor * s)249 static 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
sensorCallFuncExternalEvtFreeF(void * event)257 static void sensorCallFuncExternalEvtFreeF(void* event)
258 {
259 slabAllocatorFree(mInternalEvents, event);
260 }
261
sensorCallFuncSetRate(struct Sensor * s,uint32_t rate,uint64_t latency)262 static 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
sensorCallFuncCalibrate(struct Sensor * s)284 static 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
sensorCallFuncSelfTest(struct Sensor * s)292 static 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
sensorCallFuncFlush(struct Sensor * s)300 static 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
sensorCallFuncCfgData(struct Sensor * s,void * cfgData)308 static 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
sensorCallFuncMarshall(struct Sensor * s,uint32_t evtType,void * evtData,TaggedPtr * evtFreeingInfoP)329 static 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
sensorCallFuncTrigger(struct Sensor * s)352 static 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
sensorCallFuncSendOneDirectEvt(struct Sensor * s,uint32_t tid)360 static 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
sensorReconfig(struct Sensor * s,uint32_t newHwRate,uint64_t newHwLatency)382 static 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
sensorCalcHwLatency(struct Sensor * s)415 static 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
sensorCalcHwRate(struct Sensor * s,uint32_t extraReqedRate,uint32_t removedRate)434 static 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
sensorInternalFwStateChanged(void * evtP)495 static 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
sensorInternalPowerStateChanged(void * evtP)519 static 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
sensorInternalRateChanged(void * evtP)545 static 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
sensorSignalInternalEvt(uint32_t handle,uint32_t intEvtNum,uint32_t value1,uint64_t value2)558 bool 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
sensorFind(uint32_t sensorType,uint32_t idx,uint32_t * handleP)581 const 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
sensorAddRequestor(uint32_t sensorHandle,uint32_t clientTid,uint32_t rate,uint64_t latency)596 static 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
sensorGetCurRequestorRate(uint32_t sensorHandle,uint32_t clientTid,uint32_t * rateP,uint64_t * latencyP)612 static 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
sensorAmendRequestor(uint32_t sensorHandle,uint32_t clientTid,uint32_t newRate,uint64_t newLatency)631 static 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
sensorDeleteRequestor(uint32_t sensorHandle,uint32_t clientTid)648 static 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
sensorRequest(uint32_t unusedTid,uint32_t sensorHandle,uint32_t rate,uint64_t latency)669 bool 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
sensorRequestRateChange(uint32_t unusedTid,uint32_t sensorHandle,uint32_t newRate,uint64_t newLatency)706 bool 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
sensorRelease(uint32_t unusedTid,uint32_t sensorHandle)741 bool 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
sensorTriggerOndemand(uint32_t unusedTid,uint32_t sensorHandle)759 bool 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
sensorFlush(uint32_t sensorHandle)783 bool 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
sensorCalibrate(uint32_t sensorHandle)793 bool 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
sensorSelfTest(uint32_t sensorHandle)803 bool 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
sensorCfgData(uint32_t sensorHandle,void * cfgData)813 bool 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
sensorGetCurRate(uint32_t sensorHandle)823 uint32_t sensorGetCurRate(uint32_t sensorHandle)
824 {
825 struct Sensor* s = sensorFindByHandle(sensorHandle);
826
827 return s ? s->currentRate : SENSOR_RATE_OFF;
828 }
829
sensorGetCurLatency(uint32_t sensorHandle)830 uint64_t sensorGetCurLatency(uint32_t sensorHandle)
831 {
832 struct Sensor* s = sensorFindByHandle(sensorHandle);
833
834 return s ? s->currentLatency : SENSOR_LATENCY_INVALID;
835 }
836
sensorGetTime(void)837 uint64_t sensorGetTime(void)
838 {
839 return rtcGetTime();
840 }
841
sensorGetInitComplete(uint32_t sensorHandle)842 bool sensorGetInitComplete(uint32_t sensorHandle)
843 {
844 struct Sensor* s = sensorFindByHandle(sensorHandle);
845
846 return s ? s->initComplete : false;
847 }
848
sensorMarshallEvent(uint32_t sensorHandle,uint32_t evtType,void * evtData,TaggedPtr * evtFreeingInfoP)849 bool 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
sensorUnregisterAll(uint32_t tid)859 int 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