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
68struct 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*/
78const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
79
80/*
81    Static variables
82*/
83static struct fifo_hw_obj fifo_objHW;
84
85/*
86    Definitions
87*/
88
89/**
90 *  @brief  Initializes the internal FIFO data structure.
91 */
92void 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**/
111uint_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**/
189inv_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**/
204inv_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**/
236short 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**/
250inv_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 */
272inv_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