1 /*
2 $License:
3 Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16 $
17 */
18 /*******************************************************************************
19 *
20 * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
21 *
22 *******************************************************************************/
23
24 /**
25 * @defgroup MLFIFO_HW
26 * @brief Motion Library - FIFO HW Driver.
27 * Provides facilities to interact with the FIFO.
28 *
29 * @{
30 * @file mlFIFOHW.c
31 * @brief The Motion Library Fifo Hardware Layer.
32 *
33 */
34
35 #include <string.h>
36
37 #include "mpu.h"
38 #if defined CONFIG_MPU_SENSORS_MPU6050A2
39 # include "mpu6050a2.h"
40 #elif defined CONFIG_MPU_SENSORS_MPU6050B1
41 # include "mpu6050b1.h"
42 #elif defined CONFIG_MPU_SENSORS_MPU3050
43 # include "mpu3050.h"
44 #else
45 #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
46 #endif
47 #include "mlFIFOHW.h"
48 #include "ml.h"
49 #include "mldl.h"
50 #include "mldl_cfg.h"
51
52 #include "mlsl.h"
53
54 #include "log.h"
55 #undef MPL_LOG_TAG
56 #define MPL_LOG_TAG "MPL-fifo"
57
58 /*
59 Defines
60 */
61
62 #define _fifoDebug(x) //{x}
63
64 /*
65 Typedefs
66 */
67
68 struct fifo_hw_obj {
69 short fifoCount;
70 inv_error_t fifoError;
71 unsigned char fifoOverflow;
72 unsigned char fifoResetOnOverflow;
73 };
74
75 /*
76 Global variables
77 */
78 const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
79
80 /*
81 Static variables
82 */
83 static struct fifo_hw_obj fifo_objHW;
84
85 /*
86 Definitions
87 */
88
89 /**
90 * @brief Initializes the internal FIFO data structure.
91 */
inv_init_fifo_hardare(void)92 void inv_init_fifo_hardare(void)
93 {
94 memset(&fifo_objHW, 0, sizeof(fifo_objHW));
95 fifo_objHW.fifoResetOnOverflow = TRUE;
96 }
97
98 /**
99 * @internal
100 * @brief used to get the FIFO data.
101 * @param length
102 * Number of bytes to read from the FIFO.
103 * @param buffer
104 * the bytes of FIFO data.
105 * Note that this buffer <b>must</b> be large enough
106 * to store and additional trailing FIFO footer when
107 * expected. The callers must make sure enough space
108 * is allocated.
109 * @return number of valid bytes of data.
110 **/
inv_get_fifo(uint_fast16_t length,unsigned char * buffer)111 uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
112 {
113 INVENSENSE_FUNC_START;
114 inv_error_t result;
115 uint_fast16_t inFifo;
116 uint_fast16_t toRead;
117 int_fast8_t kk;
118
119 toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
120 /*---- make sure length is correct ----*/
121 if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
122 fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
123 return 0;
124 }
125
126 result = inv_get_fifo_length(&inFifo);
127 if (INV_SUCCESS != result) {
128 fifo_objHW.fifoError = result;
129 return 0;
130 }
131 // fifo_objHW.fifoCount is the footer size left in the buffer, or
132 // 0 if this is the first time reading the fifo since it was reset
133 if (inFifo < length + fifo_objHW.fifoCount) {
134 fifo_objHW.fifoError = INV_SUCCESS;
135 return 0;
136 }
137 // if a trailing fifo count is expected - start storing data 2 bytes before
138 result =
139 inv_read_fifo(fifo_objHW.fifoCount >
140 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
141 if (INV_SUCCESS != result) {
142 fifo_objHW.fifoError = result;
143 return 0;
144 }
145 // Make sure the fifo didn't overflow before or during the read
146 result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
147 MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
148 if (INV_SUCCESS != result) {
149 fifo_objHW.fifoError = result;
150 return 0;
151 }
152
153 if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
154 MPL_LOGV("Resetting Fifo : Overflow\n");
155 inv_reset_fifo();
156 fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
157 return 0;
158 }
159
160 /* Check the Footer value to give us a chance at making sure data
161 * didn't get corrupted */
162 for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
163 if (buffer[kk] != gFifoFooter[kk]) {
164 MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
165 buffer[0], buffer[1]);
166 _fifoDebug(char out[200];
167 MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
168 sprintf(out, "0x");
169 for (kk = 0; kk < (int)toRead; kk++) {
170 sprintf(out, "%s%02X", out, buffer[kk]);}
171 MPL_LOGW("%s\n", out);)
172 inv_reset_fifo();
173 fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
174 return 0;
175 }
176 }
177
178 if (fifo_objHW.fifoCount == 0) {
179 fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
180 }
181
182 return length - FIFO_FOOTER_SIZE;
183 }
184
185 /**
186 * @brief Used to query the status of the FIFO.
187 * @return INV_SUCCESS if the fifo is OK. An error code otherwise.
188 **/
inv_get_fifo_status(void)189 inv_error_t inv_get_fifo_status(void)
190 {
191 inv_error_t fifoError = fifo_objHW.fifoError;
192 fifo_objHW.fifoError = 0;
193 return fifoError;
194 }
195
196 /**
197 * @internal
198 * @brief Get the length from the fifo
199 *
200 * @param[out] len amount of data currently stored in the fifo.
201 *
202 * @return INV_SUCCESS or non-zero error code.
203 **/
inv_get_fifo_length(uint_fast16_t * len)204 inv_error_t inv_get_fifo_length(uint_fast16_t * len)
205 {
206 INVENSENSE_FUNC_START;
207 unsigned char fifoBuf[2];
208 inv_error_t result;
209
210 if (NULL == len)
211 return INV_ERROR_INVALID_PARAMETER;
212
213 /*---- read the 2 'count' registers and
214 burst read the data from the FIFO ----*/
215 result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
216 MPUREG_FIFO_COUNTH, 2, fifoBuf);
217 if (INV_SUCCESS != result) {
218 MPL_LOGE("ReadBurst failed %d\n", result);
219 inv_reset_fifo();
220 fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
221 *len = 0;
222 return result;
223 }
224
225 *len = (uint_fast16_t) (fifoBuf[0] << 8);
226 *len += (uint_fast16_t) (fifoBuf[1]);
227 return result;
228 }
229
230 /**
231 * @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO.
232 * This function returns the stored value and does not access the hardware.
233 * See inv_get_fifo_length().
234 * @return the number of bytes left in the FIFO
235 **/
inv_get_fifo_count(void)236 short inv_get_fifo_count(void)
237 {
238 return fifo_objHW.fifoCount;
239 }
240
241 /**
242 * @internal
243 * @brief Read data from the fifo
244 *
245 * @param[out] data Location to store the date read from the fifo
246 * @param[in] len Amount of data to read out of the fifo
247 *
248 * @return INV_SUCCESS or non-zero error code
249 **/
inv_read_fifo(unsigned char * data,uint_fast16_t len)250 inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
251 {
252 INVENSENSE_FUNC_START;
253 inv_error_t result;
254 result = inv_serial_read_fifo(inv_get_serial_handle(),
255 inv_get_mpu_slave_addr(),
256 (unsigned short)len, data);
257 if (INV_SUCCESS != result) {
258 MPL_LOGE("inv_serial_readBurst failed %d\n", result);
259 inv_reset_fifo();
260 fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
261 return result;
262 }
263 return result;
264 }
265
266 /**
267 * @brief Clears the FIFO status and its content.
268 * @note Halt the DMP writing into the FIFO for the time
269 * needed to reset the FIFO.
270 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
271 */
inv_reset_fifo(void)272 inv_error_t inv_reset_fifo(void)
273 {
274 INVENSENSE_FUNC_START;
275 int len = FIFO_HW_SIZE;
276 unsigned char fifoBuf[2];
277 unsigned char tries = 0;
278 unsigned char userCtrlReg;
279 inv_error_t result;
280 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
281
282 fifo_objHW.fifoCount = 0;
283 if (mldl_cfg->gyro_is_suspended)
284 return INV_SUCCESS;
285
286 result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
287 MPUREG_USER_CTRL, 1, &userCtrlReg);
288 if (result) {
289 LOG_RESULT_LOCATION(result);
290 return result;
291 }
292
293 while (len != 0 && tries < 6) {
294 result =
295 inv_serial_single_write(inv_get_serial_handle(),
296 inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
297 ((userCtrlReg & (~BIT_FIFO_EN)) |
298 BIT_FIFO_RST));
299 if (result) {
300 LOG_RESULT_LOCATION(result);
301 return result;
302 }
303 result =
304 inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
305 MPUREG_FIFO_COUNTH, 2, fifoBuf);
306 if (result) {
307 LOG_RESULT_LOCATION(result);
308 return result;
309 }
310 len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
311 tries++;
312 }
313
314 result =
315 inv_serial_single_write(inv_get_serial_handle(),
316 inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
317 userCtrlReg);
318 if (result) {
319 LOG_RESULT_LOCATION(result);
320 return result;
321 }
322
323 return INV_SUCCESS;
324 }
325
326 /**
327 * @}
328 **/
329