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