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