1 /*
2 * Copyright (C) 2017 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 <atomic.h>
18 #include <gpio.h>
19 #include <isr.h>
20 #include <nanohubPacket.h>
21 #include <plat/exti.h>
22 #include <plat/gpio.h>
23 #include <platform.h>
24 #include <plat/syscfg.h>
25 #include <plat/rtc.h>
26 #include <sensors.h>
27 #include <seos.h>
28 #include <slab.h>
29 #include <heap.h>
30 #include <i2c.h>
31 #include <timer.h>
32 #include <variant/sensType.h>
33 #include <cpu/cpuMath.h>
34 #include <floatRt.h>
35
36 #include <stdlib.h>
37 #include <string.h>
38 #include <variant/variant.h>
39
40 #define ST_ACC44_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 7)
41
42 /* Sensor registers */
43 #define ST_ACC44_WAI_REG_ADDR 0x0F
44 #define ST_ACC44_WAI_REG_VAL 0x44
45
46 /*
47 * CTRL1 Register
48 *
49 * CTRL1[7:4] := ODR
50 * CTRL1[3:2] := MODE
51 * CTRL1[1:0] := LP_MODE
52 */
53 #define ST_ACC44_CTRL1_REG 0x20
54 #define ST_ACC44_ODR_POWER_DOWN 0x00
55 #define ST_ACC44_ODR_12_5_HZ 0x20
56 #define ST_ACC44_ODR_25_HZ 0x30
57 #define ST_ACC44_ODR_50_HZ 0x40
58 #define ST_ACC44_ODR_100_HZ 0x50
59 #define ST_ACC44_ODR_200_HZ 0x60
60 #define ST_ACC44_ODR_400_HZ 0x70
61 #define ST_ACC44_ODR_800_HZ 0x80
62 #define ST_ACC44_ODR_1600_HZ 0x90
63 #define ST_ACC44_HIPERF_MODE 0x04
64 #define ST_ACC44_CTRL1_DEFVAL (ST_ACC44_HIPERF_MODE)
65
66 /*
67 * CTRL2 Register
68 *
69 * CTRL2[7] := BOOT
70 * CTRL2[6] := SOFT_RESET
71 * CTRL2[3] := BDU
72 */
73 #define ST_ACC44_CTRL2_REG 0x21
74 #define ST_ACC44_CTRL2_BOOT 0x80
75 #define ST_ACC44_CTRL2_SW_RST 0x40
76 #define ST_ACC44_CTRL2_BDU 0x08
77 #define ST_ACC44_CTRL2_IF_ADD_INC 0x04
78 #define ST_ACC44_CTRL2_DEFVAL (ST_ACC44_CTRL2_BDU | ST_ACC44_CTRL2_IF_ADD_INC)
79
80 /*
81 * CTRL3 Register
82 */
83 #define ST_ACC44_CTRL3_REG 0x22
84 #define ST_ACC44_CTRL3_LIR 0x10
85
86 /*
87 * CTRL4 Register
88 *
89 * CTRL4[7] := INT1_6D
90 * CTRL4[6] := INT1_SINGLE_TAP
91 * CTRL4[5] := INT1_WU
92 * CTRL4[4] := INT1_FF
93 * CTRL4[3] := INT1_TAP
94 * CTRL4[2] := INT1_DIFF5
95 * CTRL4[1] := INT1_FTH
96 * CTRL4[0] := INT1_DRDY
97 */
98 #define ST_ACC44_CTRL4_REG 0x23
99 #define ST_ACC44_CTRL4_INT1_6D 0x80
100 #define ST_ACC44_CTRL4_INT1_STAP 0x40
101 #define ST_ACC44_CTRL4_INT1_WU 0x20
102 #define ST_ACC44_CTRL4_INT1_FF 0x10
103 #define ST_ACC44_CTRL4_INT1_DTAP 0x08
104 #define ST_ACC44_CTRL4_INT1_DIFF5 0x04
105 #define ST_ACC44_CTRL4_INT1_FTH 0x02
106 #define ST_ACC44_CTRL4_INT1_DRDY 0x01
107
108 /*
109 * CTRL5 Register
110 */
111 #define ST_ACC44_CTRL5_REG 0x24
112
113 /*
114 * CTRL6 Register
115 *
116 * CTRL6[5:4] := FS
117 */
118 #define ST_ACC44_CTRL6_REG 0x25
119 #define ST_ACC44_CTRL6_FS_2G 0x00
120 #define ST_ACC44_CTRL6_FS_4G 0x10
121 #define ST_ACC44_CTRL6_FS_8G 0x20
122 #define ST_ACC44_CTRL6_FS_16G 0x30
123
124 /*
125 * STATUS Register
126 */
127 #define ST_ACC44_STATUS_REG_ADDR 0x27
128 #define ST_ACC44_STATUS_REG_FTH 0x80
129 #define ST_ACC44_STATUS_REG_DRDY 0x01
130
131 /*
132 * OUTXL Register
133 */
134 #define ST_ACC44_OUTXL_REG_ADDR 0x28
135
136 /*
137 * value in m/s2 per LSB (in high-resolution mode @8g)
138 * Since samples are 14-bit left aligned, the value
139 * must also be right-shifted by 2.
140 *
141 * (9.80665 * 0.976) / (4 * 1000)
142 */
143 #define ST_ACC44_KSCALE 0.0023928226
144
145
146 /* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */
147 #define ST_ACC44_I2C_AUTO_INCR 0x80
148
149 #define INFO_PRINT(fmt, ...) \
150 do { \
151 osLog(LOG_INFO, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
152 } while (0);
153
154 #define DEBUG_PRINT(fmt, ...) \
155 do { \
156 if (ST_ACC44_DBG_ENABLED) { \
157 osLog(LOG_DEBUG, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
158 } \
159 } while (0);
160
161 #define ERROR_PRINT(fmt, ...) \
162 do { \
163 osLog(LOG_ERROR, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
164 } while (0);
165
166 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
167 #ifndef ST_ACC44_DBG_ENABLED
168 #define ST_ACC44_DBG_ENABLED 0
169 #endif /* ST_ACC44_DBG_ENABLED */
170
171 enum st_acc44_SensorEvents
172 {
173 EVT_COMM_DONE = EVT_APP_START + 1,
174 EVT_SENSOR_INTERRUPT,
175 };
176
177 enum st_acc44_SensorState {
178 SENSOR_BOOT,
179 SENSOR_VERIFY_ID,
180 SENSOR_INIT,
181 SENSOR_IDLE,
182 SENSOR_ACCEL_POWER_UP,
183 SENSOR_ACCEL_POWER_DOWN,
184 SENSOR_CHANGE_RATE,
185 SENSOR_READ_SAMPLES,
186 };
187
188 #ifndef ST_ACC44_I2C_BUS_ID
189 #error "ST_ACC44_I2C_BUS_ID is not defined; please define in variant.h"
190 #endif
191
192 #ifndef ST_ACC44_I2C_SPEED
193 #error "ST_ACC44_I2C_SPEED is not defined; please define in variant.h"
194 #endif
195
196 #ifndef ST_ACC44_I2C_ADDR
197 #error "ST_ACC44_I2C_ADDR is not defined; please define in variant.h"
198 #endif
199
200 #ifndef ST_ACC44_INT_PIN
201 #error "ST_ACC44_INT_PIN is not defined; please define in variant.h"
202 #endif
203
204 #ifndef ST_ACC44_INT_IRQ
205 #error "ST_ACC44_INT_IRQ is not defined; please define in variant.h"
206 #endif
207
208 #ifndef ST_ACC44_TO_ANDROID_COORDINATE
209 #error "ST_ACC44_TO_ANDROID_COORDINATE is not defined; please define in variant.h"
210 #endif
211
212 #define RAW_TO_MS2(raw_axis) ((float)raw_axis * ST_ACC44_KSCALE)
213
214 #define ST_ACC44_MAX_PENDING_I2C_REQUESTS 10
215 #define ST_ACC44_MAX_I2C_TRANSFER_SIZE 6
216 #define ST_ACC44_MAX_ACC_EVENTS 50
217
218 struct I2cTransfer
219 {
220 size_t tx;
221 size_t rx;
222 int err;
223 uint8_t txrxBuf[ST_ACC44_MAX_I2C_TRANSFER_SIZE];
224 bool last;
225 bool inUse;
226 uint32_t delay;
227 };
228
229 /* Task structure */
230 struct st_acc44_Task {
231 uint32_t tid;
232
233 struct SlabAllocator *accDataSlab;
234
235 volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly
236 bool accOn;
237 uint32_t sample_rate_ns;
238 uint32_t irq_rate_ns;
239 uint32_t rate;
240 uint32_t latency;
241 uint8_t currentODR;
242 uint8_t samplesToDiscard;
243 uint64_t Timestamp;
244 uint64_t lastTime;
245
246 bool pendingInt;
247 bool pendingSetPower;
248 bool pendingSetRate;
249 uint32_t pendingRate;
250 uint32_t pendingLatency;
251 bool pendingPower;
252
253 struct I2cTransfer transfers[ST_ACC44_MAX_PENDING_I2C_REQUESTS];
254
255 /* Communication functions */
256 bool (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last);
257 bool (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last);
258
259 /* irq */
260 struct Gpio *Int1;
261 struct ChainedIsr Isr1;
262 uint32_t int_num;
263
264 /* sensors */
265 uint32_t accHandle;
266 };
267
268 static struct st_acc44_Task mTask;
269
270 #if DBG_STATE
271 #define PRI_STATE PRIi32
getStateName(int32_t s)272 static int32_t getStateName(int32_t s) {
273 return s;
274 }
275 #endif
276
277 // Atomic get state
278 #define GET_STATE() (atomicReadByte(&mTask.state))
279
280 // Atomic set state, this set the state to arbitrary value, use with caution
281 #define SET_STATE(s) do{\
282 atomicWriteByte(&mTask.state, (s));\
283 }while(0)
284
285 // Atomic switch state from IDLE to desired state.
trySwitchState(enum st_acc44_SensorState newState)286 static bool trySwitchState(enum st_acc44_SensorState newState) {
287 #if DBG_STATE
288 bool ret = atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
289 uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
290 DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
291 getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
292 return ret;
293 #else
294 return atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
295 #endif
296 }
297
298 #define DEC_INFO(name, type, axis, inter, samples, rates, raw, scale) \
299 .sensorName = name, \
300 .sensorType = type, \
301 .numAxis = axis, \
302 .interrupt = inter, \
303 .minSamples = samples, \
304 .supportedRates = rates, \
305 .rawType = raw, \
306 .rawScale = scale,
307
308 static uint32_t st_acc44_Rates[] = {
309 SENSOR_HZ(25.0f/2.0f),
310 SENSOR_HZ(25.0f),
311 SENSOR_HZ(50.0f),
312 SENSOR_HZ(100.0f),
313 SENSOR_HZ(200.0f),
314 SENSOR_HZ(400.0f),
315 SENSOR_HZ(800.0f),
316 0
317 };
318
319 static uint32_t st_acc44_Rates_in_ns[] = {
320 80000000, /* 12.5 Hz */
321 40000000, /* 25 Hz */
322 20000000, /* 50 Hz */
323 10000000, /* 100 Hz */
324 5000000, /* 200 Hz */
325 2500000, /* 400 Hz */
326 1250000, /* 800 Hz */
327 0
328 };
329
330 static uint32_t st_acc44_regVal[] = {
331 ST_ACC44_ODR_12_5_HZ,
332 ST_ACC44_ODR_25_HZ,
333 ST_ACC44_ODR_50_HZ,
334 ST_ACC44_ODR_100_HZ,
335 ST_ACC44_ODR_200_HZ,
336 ST_ACC44_ODR_400_HZ,
337 ST_ACC44_ODR_800_HZ,
338 ST_ACC44_ODR_1600_HZ,
339 };
340
st_acc44_computeOdr(uint32_t rate)341 static uint8_t st_acc44_computeOdr(uint32_t rate)
342 {
343 int i;
344
345 for (i = 0; i < (ARRAY_SIZE(st_acc44_Rates) - 1); i++) {
346 if (st_acc44_Rates[i] == rate)
347 break;
348 }
349 if (i == (ARRAY_SIZE(st_acc44_Rates) -1 )) {
350 ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n");
351 i = 0;
352 }
353
354 return i;
355 }
356
st_acc44_Rate_hz_to_ns(uint32_t rate)357 static uint32_t st_acc44_Rate_hz_to_ns(uint32_t rate)
358 {
359 int i;
360
361 if ((i = st_acc44_computeOdr(rate)) >= 0)
362 return st_acc44_Rates_in_ns[i];
363
364 return 0;
365 }
366
367 static const struct SensorInfo st_acc44_SensorInfo =
368 {
369 DEC_INFO("Accelerometer", SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
370 600, st_acc44_Rates, SENS_TYPE_ACCEL_RAW, 1.0f / ST_ACC44_KSCALE)
371 };
372
st_acc44_Power(bool on,void * cookie)373 static bool st_acc44_Power(bool on, void *cookie)
374 {
375 bool oldMode = mTask.accOn;
376 bool newMode = on;
377 uint32_t state = on ? SENSOR_ACCEL_POWER_UP : SENSOR_ACCEL_POWER_DOWN;
378 bool ret = true;
379
380 INFO_PRINT("Power %s\n", on ? "on" : "off");
381
382 if (trySwitchState(state)) {
383 if (oldMode != newMode) {
384 if (on) {
385 ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_12_5_HZ |
386 ST_ACC44_CTRL1_DEFVAL, 0, true);
387 } else {
388 ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN |
389 ST_ACC44_CTRL1_DEFVAL, 0, true);
390 }
391 } else
392 sensorSignalInternalEvt(mTask.accHandle,
393 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
394 } else {
395 mTask.pendingSetPower = true;
396 mTask.pendingPower = on;
397 }
398
399 return ret;
400 }
401
st_acc44_FwUpload(void * cookie)402 static bool st_acc44_FwUpload(void *cookie)
403 {
404 INFO_PRINT("FwUpload\n");
405 return sensorSignalInternalEvt(mTask.accHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
406 }
407
st_acc44_SetRate(uint32_t rate,uint64_t latency,void * cookie)408 static bool st_acc44_SetRate(uint32_t rate, uint64_t latency, void *cookie)
409 {
410 uint8_t num = 0;
411
412 INFO_PRINT("SetRate %lu Hz - %llu ns\n", rate, latency);
413
414 if (trySwitchState(SENSOR_CHANGE_RATE)) {
415 num = st_acc44_computeOdr(rate);
416 mTask.currentODR = st_acc44_regVal[num];
417 mTask.latency = latency;
418 mTask.rate = rate;
419 mTask.sample_rate_ns = st_acc44_Rate_hz_to_ns(rate);
420 mTask.samplesToDiscard = 2;
421 mTask.lastTime = 0;
422
423 /* one interrupt every sample */
424 mTask.irq_rate_ns = mTask.sample_rate_ns;
425
426 mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, false);
427 mTask.comm_tx(ST_ACC44_CTRL4_REG, ST_ACC44_CTRL4_INT1_DRDY, 0, false);
428 mTask.comm_tx(ST_ACC44_CTRL1_REG, mTask.currentODR | ST_ACC44_CTRL1_DEFVAL, 0, true);
429 } else {
430 mTask.pendingSetRate = true;
431 mTask.pendingRate = rate;
432 mTask.pendingLatency = latency;
433 }
434
435 return true;
436 }
437
st_acc44_Flush(void * cookie)438 static bool st_acc44_Flush(void *cookie)
439 {
440 INFO_PRINT("Flush\n");
441 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
442 }
443
st_acc44_SelfTest(void * cookie)444 static bool st_acc44_SelfTest(void *cookie)
445 {
446 INFO_PRINT("SelfTest\n");
447 return true;
448 }
449
450 #define DEC_OPS(power, firmware, rate, flush, test, cal, cfg) \
451 .sensorPower = power, \
452 .sensorFirmwareUpload = firmware, \
453 .sensorSetRate = rate, \
454 .sensorFlush = flush, \
455 .sensorCalibrate = cal, \
456 .sensorSelfTest = test, \
457 .sensorCfgData = cfg
458
459 static const struct SensorOps st_acc44_SensorOps =
460 {
461 DEC_OPS(st_acc44_Power, st_acc44_FwUpload, st_acc44_SetRate, st_acc44_Flush, st_acc44_SelfTest, NULL, NULL),
462 };
463
enableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)464 static void inline enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
465 {
466 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
467 syscfgSetExtiPort(pin);
468 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
469 extiChainIsr(ST_ACC44_INT_IRQ, isr);
470 }
471
disableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)472 static void inline disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
473 {
474 extiUnchainIsr(ST_ACC44_INT_IRQ, isr);
475 extiDisableIntGpio(pin);
476 }
477
st_acc44_calc_timestamp(void)478 static void st_acc44_calc_timestamp(void)
479 {
480 if (mTask.lastTime == 0) {
481 mTask.Timestamp = sensorGetTime();
482 } else {
483 uint64_t currTime = sensorGetTime();
484 uint64_t deltaTime = currTime - mTask.lastTime;
485
486 deltaTime = (deltaTime + 7*mTask.irq_rate_ns)/8;
487 mTask.Timestamp = mTask.lastTime + deltaTime;
488 }
489 mTask.lastTime = mTask.Timestamp;
490 }
491
st_acc44_int1_isr(struct ChainedIsr * isr)492 static bool st_acc44_int1_isr(struct ChainedIsr *isr)
493 {
494 if (!extiIsPendingGpio(mTask.Int1))
495 return false;
496
497 /* Start sampling for a value */
498 if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT, NULL, NULL, mTask.tid))
499 ERROR_PRINT("st_acc44_int1_isr: osEnqueuePrivateEvt() failed\n");
500
501 mTask.int_num++;
502 extiClearPendingGpio(mTask.Int1);
503 return true;
504 }
505
int2Evt(void)506 static void int2Evt(void)
507 {
508 if (trySwitchState(SENSOR_READ_SAMPLES)) {
509 mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, true);
510 } else {
511 mTask.pendingInt = true;
512 }
513 }
514
processPendingEvt(void)515 static void processPendingEvt(void)
516 {
517 if (mTask.pendingInt) {
518 mTask.pendingInt = false;
519 int2Evt();
520 return;
521 }
522
523 if (mTask.pendingSetPower) {
524 mTask.pendingSetPower = false;
525 st_acc44_Power(mTask.pendingPower, NULL);
526 }
527
528 if (mTask.pendingSetRate) {
529 mTask.pendingSetRate = false;
530 st_acc44_SetRate(mTask.pendingRate, mTask.pendingLatency, NULL);
531 }
532 }
533
accAllocateEvt(struct TripleAxisDataEvent ** evPtr)534 static bool accAllocateEvt(struct TripleAxisDataEvent **evPtr)
535 {
536 struct TripleAxisDataEvent *ev;
537
538 ev = *evPtr = slabAllocatorAlloc(mTask.accDataSlab);
539 if (!ev) {
540 ERROR_PRINT("Failed to allocate acc event memory");
541 return false;
542 }
543
544 memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
545 return true;
546 }
547
accFreeEvt(void * ptr)548 static void accFreeEvt(void *ptr)
549 {
550 slabAllocatorFree(mTask.accDataSlab, ptr);
551 }
552
553 // Allocate a buffer and mark it as in use with the given state, or return NULL
554 // if no buffers available. Must *not* be called from interrupt context.
allocXfer(void)555 static struct I2cTransfer *allocXfer(void)
556 {
557 size_t i;
558
559 for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) {
560 if (!mTask.transfers[i].inUse) {
561 mTask.transfers[i].inUse = true;
562 return &mTask.transfers[i];
563 }
564 }
565
566 ERROR_PRINT("Ran out of i2c buffers!");
567 return NULL;
568 }
569
releaseXfer(struct I2cTransfer * xfer)570 static inline void releaseXfer(struct I2cTransfer *xfer)
571 {
572 xfer->inUse = false;
573 }
574
st_acc44_i2cCallback(void * cookie,size_t tx,size_t rx,int err)575 static void st_acc44_i2cCallback(void *cookie, size_t tx, size_t rx, int err)
576 {
577 struct I2cTransfer *xfer = cookie;
578
579 /* Do not run callback if not the last one in a set of i2c transfers */
580 if (xfer && !xfer->last) {
581 releaseXfer(xfer);
582 return;
583 }
584
585 xfer->tx = tx;
586 xfer->rx = rx;
587 xfer->err = err;
588
589 osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid);
590 if (err != 0)
591 ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
592 }
593
st_acc44_i2c_read(uint8_t addr,uint16_t len,uint32_t delay,bool last)594 static bool st_acc44_i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last)
595 {
596 struct I2cTransfer *xfer = allocXfer();
597 int ret = -1;
598
599 if (xfer != NULL) {
600 xfer->delay = delay;
601 xfer->last = last;
602 xfer->txrxBuf[0] = ST_ACC44_I2C_AUTO_INCR | addr;
603 if ((ret = i2cMasterTxRx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, st_acc44_i2cCallback, xfer)) < 0) {
604 releaseXfer(xfer);
605 DEBUG_PRINT("st_acc44_i2c_read: i2cMasterTxRx operation failed (ret: %d)\n", ret);
606 return false;
607 }
608 }
609
610 return (ret == -1) ? false : true;
611 }
612
st_acc44_i2c_write(uint8_t addr,uint8_t data,uint32_t delay,bool last)613 static bool st_acc44_i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last)
614 {
615 struct I2cTransfer *xfer = allocXfer();
616 int ret = -1;
617
618 if (xfer != NULL) {
619 xfer->delay = delay;
620 xfer->last = last;
621 xfer->txrxBuf[0] = addr;
622 xfer->txrxBuf[1] = data;
623 if ((ret = i2cMasterTx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 2, st_acc44_i2cCallback, xfer)) < 0) {
624 releaseXfer(xfer);
625 DEBUG_PRINT("st_acc44_i2c_write: i2cMasterTx operation failed (ret: %d)\n", ret);
626 return false;
627 }
628 }
629
630 return (ret == -1) ? false : true;
631 }
632
parseRawData(uint8_t * raw,uint8_t num_of_smpl,uint64_t sensor_time)633 static void parseRawData(uint8_t *raw, uint8_t num_of_smpl, uint64_t sensor_time)
634 {
635 uint8_t i;
636 struct TripleAxisDataEvent *accSample;
637 float x, y, z;
638 int32_t raw_x;
639 int32_t raw_y;
640 int32_t raw_z;
641
642 /* Discard samples generated during sensor turn-on time */
643 if (mTask.samplesToDiscard > 0) {
644 if (num_of_smpl > mTask.samplesToDiscard) {
645 num_of_smpl -= mTask.samplesToDiscard;
646 mTask.samplesToDiscard = 0;
647 } else{
648 mTask.samplesToDiscard -= num_of_smpl;
649 return;
650 }
651 }
652
653 if (accAllocateEvt(&accSample) == false)
654 return;
655
656 accSample->referenceTime = sensor_time;
657
658 accSample->samples[0].deltaTime = 0;
659 accSample->samples[0].firstSample.numSamples = num_of_smpl;
660
661 for (i = 0; i < num_of_smpl; i++) {
662 raw_x = (*(int16_t *)&raw[6*i + 0]);
663 raw_y = (*(int16_t *)&raw[6*i + 2]);
664 raw_z = (*(int16_t *)&raw[6*i + 4]);
665
666 /* convert raw data in m/s2 */
667 x = RAW_TO_MS2(raw_x);
668 y = RAW_TO_MS2(raw_y);
669 z = RAW_TO_MS2(raw_z);
670
671 /* rotate axis */
672 ST_ACC44_TO_ANDROID_COORDINATE(x, y, z);
673
674 accSample->samples[i].x = x;
675 accSample->samples[i].y = y;
676 accSample->samples[i].z = z;
677
678 if (i > 0)
679 accSample->samples[i].deltaTime = mTask.sample_rate_ns;
680 }
681
682 osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_ACCEL), accSample, accFreeEvt);
683 }
684
st_acc44_handleCommDoneEvt(const void * evtData)685 static int st_acc44_handleCommDoneEvt(const void* evtData)
686 {
687 bool returnIdle = false;
688 struct I2cTransfer *xfer = (struct I2cTransfer *)evtData;
689
690 switch (GET_STATE()) {
691 case SENSOR_BOOT:
692 SET_STATE(SENSOR_VERIFY_ID);
693 if (!mTask.comm_rx(ST_ACC44_WAI_REG_ADDR, 1, 0, true)) {
694 DEBUG_PRINT("Not able to read WAI\n");
695 return -1;
696 }
697 break;
698
699 case SENSOR_VERIFY_ID:
700 /* Check the sensor ID */
701 if (xfer->err != 0 || xfer->txrxBuf[0] != ST_ACC44_WAI_REG_VAL) {
702 DEBUG_PRINT("WAI returned is: %02x\n", xfer->txrxBuf[0]);
703 break;
704 }
705
706 INFO_PRINT("Device ID is correct! (%02x)\n", xfer->txrxBuf[0]);
707
708 SET_STATE(SENSOR_INIT);
709 mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN | ST_ACC44_CTRL1_DEFVAL, 0, false);
710 mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_DEFVAL, 0, false);
711 mTask.comm_tx(ST_ACC44_CTRL3_REG, ST_ACC44_CTRL3_LIR, 0, false);
712 mTask.comm_tx(ST_ACC44_CTRL6_REG, ST_ACC44_CTRL6_FS_8G, 0, false);
713 mTask.comm_tx(ST_ACC44_CTRL4_REG, 0, 0, true);
714 break;
715
716 case SENSOR_INIT:
717 DEBUG_PRINT("SENSOR INIT\n");
718 returnIdle = true;
719 sensorRegisterInitComplete(mTask.accHandle);
720 break;
721
722 case SENSOR_ACCEL_POWER_UP:
723 DEBUG_PRINT("POWER UP\n");
724 returnIdle = true;
725 mTask.accOn = true;
726 sensorSignalInternalEvt(mTask.accHandle,
727 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
728 break;
729
730 case SENSOR_ACCEL_POWER_DOWN:
731 DEBUG_PRINT("POWER DWN\n");
732 returnIdle = true;
733 mTask.accOn = false;
734 sensorSignalInternalEvt(mTask.accHandle,
735 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
736 break;
737
738 case SENSOR_CHANGE_RATE:
739 DEBUG_PRINT("CHANGE RATE\n");
740 returnIdle = true;
741 DEBUG_PRINT("int_num %ld\n", mTask.int_num);
742 mTask.int_num = 0;
743 sensorSignalInternalEvt(mTask.accHandle,
744 SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency);
745 break;
746
747 case SENSOR_READ_SAMPLES:
748 returnIdle = true;
749
750 parseRawData(&xfer->txrxBuf[0], 1, mTask.Timestamp);
751 break;
752
753 case SENSOR_IDLE:
754 default:
755 break;
756 }
757
758 releaseXfer(xfer);
759
760 if (returnIdle) {
761 SET_STATE(SENSOR_IDLE);
762 processPendingEvt();
763 }
764
765 return (0);
766 }
767
st_acc44_handleEvent(uint32_t evtType,const void * evtData)768 static void st_acc44_handleEvent(uint32_t evtType, const void* evtData)
769 {
770 switch (evtType) {
771 case EVT_APP_START:
772 INFO_PRINT("EVT_APP_START\n");
773 osEventUnsubscribe(mTask.tid, EVT_APP_START);
774
775 SET_STATE(SENSOR_BOOT);
776 mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_SW_RST, 0, true);
777 break;
778
779 case EVT_COMM_DONE:
780 st_acc44_handleCommDoneEvt(evtData);
781 break;
782
783 case EVT_SENSOR_INTERRUPT:
784 st_acc44_calc_timestamp();
785 int2Evt();
786 break;
787
788 default:
789 break;
790 }
791 }
792
st_acc44_startTask(uint32_t task_id)793 static bool st_acc44_startTask(uint32_t task_id)
794 {
795 size_t slabSize;
796
797 mTask.tid = task_id;
798
799 INFO_PRINT("start driver\n");
800
801 mTask.accOn = false;
802 mTask.pendingInt = false;
803 mTask.pendingSetPower = false;
804 mTask.pendingSetRate = false;
805
806 mTask.currentODR = ST_ACC44_ODR_POWER_DOWN;
807
808 slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
809
810 mTask.accDataSlab = slabAllocatorNew(slabSize, 4, ST_ACC44_MAX_ACC_EVENTS);
811 if (!mTask.accDataSlab) {
812 ERROR_PRINT("Failed to allocate accDataSlab memory\n");
813 return false;
814 }
815
816 /* Init the communication part */
817 i2cMasterRequest(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_SPEED);
818
819 mTask.comm_tx = st_acc44_i2c_write;
820 mTask.comm_rx = st_acc44_i2c_read;
821
822 /* irq */
823 mTask.int_num = 0;
824 mTask.Int1 = gpioRequest(ST_ACC44_INT_PIN);
825 gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE);
826 mTask.Isr1.func = st_acc44_int1_isr;
827 enableInterrupt(mTask.Int1, &mTask.Isr1);
828
829 mTask.accHandle = sensorRegister(&st_acc44_SensorInfo, &st_acc44_SensorOps, NULL, false);
830
831 osEventSubscribe(mTask.tid, EVT_APP_START);
832
833 return true;
834 }
835
st_acc44_endTask(void)836 static void st_acc44_endTask(void)
837 {
838 INFO_PRINT("ended\n");
839 slabAllocatorDestroy(mTask.accDataSlab);
840 disableInterrupt(mTask.Int1, &mTask.Isr1);
841 }
842
843 INTERNAL_APP_INIT(ST_ACC44_APP_ID, 0, st_acc44_startTask, st_acc44_endTask, st_acc44_handleEvent);
844