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 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
sensorSignalInternalEvt(uint32_t handle,uint32_t intEvtNum,uint32_t value1,uint64_t value2)536 bool 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
sensorFind(uint32_t sensorType,uint32_t idx,uint32_t * handleP)559 const 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
sensorAddRequestor(uint32_t sensorHandle,uint32_t clientTid,uint32_t rate,uint64_t latency)574 static 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
sensorGetCurRequestorRate(uint32_t sensorHandle,uint32_t clientTid,uint32_t * rateP)590 static 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
sensorAmendRequestor(uint32_t sensorHandle,uint32_t clientTid,uint32_t newRate,uint64_t newLatency)603 static 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
sensorDeleteRequestor(uint32_t sensorHandle,uint32_t clientTid)616 static 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
sensorRequest(uint32_t unusedTid,uint32_t sensorHandle,uint32_t rate,uint64_t latency)633 bool 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
sensorRequestRateChange(uint32_t unusedTid,uint32_t sensorHandle,uint32_t newRate,uint64_t newLatency)670 bool 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
sensorRelease(uint32_t unusedTid,uint32_t sensorHandle)705 bool 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
sensorFreeAll(uint32_t clientTid)723 uint32_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
sensorTriggerOndemand(uint32_t unusedTid,uint32_t sensorHandle)746 bool 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
sensorFlush(uint32_t sensorHandle)764 bool 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
sensorCalibrate(uint32_t sensorHandle)774 bool 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
sensorSelfTest(uint32_t sensorHandle)784 bool 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
sensorCfgData(uint32_t sensorHandle,void * cfgData)794 bool 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
sensorGetCurRate(uint32_t sensorHandle)804 uint32_t sensorGetCurRate(uint32_t sensorHandle)
805 {
806 struct Sensor* s = sensorFindByHandle(sensorHandle);
807
808 return s ? s->currentRate : SENSOR_RATE_OFF;
809 }
810
sensorGetCurLatency(uint32_t sensorHandle)811 uint64_t sensorGetCurLatency(uint32_t sensorHandle)
812 {
813 struct Sensor* s = sensorFindByHandle(sensorHandle);
814
815 return s ? s->currentLatency : SENSOR_LATENCY_INVALID;
816 }
817
sensorGetHwRate(uint32_t sensorHandle)818 uint32_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
sensorGetHwLatency(uint32_t sensorHandle)825 uint64_t sensorGetHwLatency(uint32_t sensorHandle)
826 {
827 struct Sensor* s = sensorFindByHandle(sensorHandle);
828
829 return s ? sensorCalcHwLatency(s) : SENSOR_LATENCY_INVALID;
830 }
831
sensorGetReqRate(uint32_t sensorHandle)832 uint32_t sensorGetReqRate(uint32_t sensorHandle)
833 {
834 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
835
836 return req ? req->rate : SENSOR_RATE_OFF;
837 }
838
sensorGetReqLatency(uint32_t sensorHandle)839 uint64_t sensorGetReqLatency(uint32_t sensorHandle)
840 {
841 struct SensorsClientRequest *req = sensorClientRequestFind(sensorHandle, osGetCurrentTid());
842
843 return req ? req->latency : SENSOR_LATENCY_INVALID;
844 }
845
sensorGetTime(void)846 uint64_t sensorGetTime(void)
847 {
848 return rtcGetTime();
849 }
850
sensorGetInitComplete(uint32_t sensorHandle)851 bool sensorGetInitComplete(uint32_t sensorHandle)
852 {
853 struct Sensor* s = sensorFindByHandle(sensorHandle);
854
855 return s ? s->initComplete : false;
856 }
857
sensorMarshallEvent(uint32_t sensorHandle,uint32_t evtType,void * evtData,TaggedPtr * evtFreeingInfoP)858 bool 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