mlstates.c revision 42331858975144405f95243be8427084ee7d478d 
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: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $
21 *
22 *******************************************************************************/
23
24/**
25 *  @defgroup MLSTATES
26 *  @brief  Basic state machine definition and support for the Motion Library.
27 *
28 *  @{
29 *      @file mlstates.c
30 *      @brief The Motion Library state machine definition.
31 */
32
33#define ML_C
34
35/* ------------------ */
36/* - Include Files. - */
37/* ------------------ */
38
39#include <stdio.h>
40#include <string.h>
41
42#include "mlstates.h"
43#include "mltypes.h"
44#include "mlinclude.h"
45#include "ml.h"
46#include "mlos.h"
47
48#include <log.h>
49#undef MPL_LOG_TAG
50#define MPL_LOG_TAG "MPL-mlstates"
51
52#define _stateDebug(x)          //{x}
53
54#define MAX_STATE_CHANGE_PROCESSES (8)
55
56struct state_callback_obj {
57    int_fast8_t numStateChangeCallbacks;
58    HANDLE mutex;
59    state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES];
60};
61
62static struct state_callback_obj sStateChangeCallbacks = { 0 };
63
64/* --------------- */
65/* -  Functions. - */
66/* --------------- */
67
68static inv_error_t inv_init_state_callbacks(void)
69{
70    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
71    return inv_create_mutex(&sStateChangeCallbacks.mutex);
72}
73
74static inv_error_t MLStateCloseCallbacks(void)
75{
76    inv_error_t result;
77    result = inv_destroy_mutex(sStateChangeCallbacks.mutex);
78    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
79    return result;
80}
81
82/**
83 *  @internal
84 *  @brief  return a string containing the label assigned to the given state.
85 *  @param  state   The state of which the label has to be returned.
86 *  @return A string containing the state label.
87**/
88char *inv_state_name(unsigned char state)
89{
90    switch (state) {
91    case INV_STATE_SERIAL_CLOSED:
92        return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED);
93        break;
94    case INV_STATE_SERIAL_OPENED:
95        return INV_STATE_NAME(INV_STATE_SERIAL_OPENED);
96        break;
97    case INV_STATE_DMP_OPENED:
98        return INV_STATE_NAME(INV_STATE_DMP_OPENED);
99        break;
100    case INV_STATE_DMP_STARTED:
101        return INV_STATE_NAME(INV_STATE_DMP_STARTED);
102        break;
103    default:
104        return NULL;
105    }
106}
107
108/**
109 *  @internal
110 *  @brief  Perform a transition from the current state to newState.
111 *          Check for the correctness of the transition.
112 *          Print out an error message if the transition is illegal .
113 *          This routine is also called if a certain normally constant parameters
114 *          are changed such as the FIFO Rate.
115 *  @param  newState    state we are transitioning to.
116 *  @return
117**/
118inv_error_t inv_state_transition(unsigned char newState)
119{
120    inv_error_t result = INV_SUCCESS;
121
122    if (newState == INV_STATE_SERIAL_CLOSED) {
123        // Always allow transition to closed
124    } else if (newState == INV_STATE_SERIAL_OPENED) {
125        inv_init_state_callbacks(); // Always allow first transition to start over
126    } else if (((newState == INV_STATE_DMP_OPENED) &&
127                ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) ||
128                 (inv_params_obj.state == INV_STATE_DMP_STARTED)))
129               ||
130               ((newState == INV_STATE_DMP_STARTED) &&
131                (inv_params_obj.state == INV_STATE_DMP_OPENED))) {
132        // Valid transitions but no special action required
133    } else {
134        // All other combinations are illegal
135        MPL_LOGE("Error : illegal state transition from %s to %s\n",
136                 inv_state_name(inv_params_obj.state),
137                 inv_state_name(newState));
138        result = INV_ERROR_SM_TRANSITION;
139    }
140
141    if (result == INV_SUCCESS) {
142        _stateDebug(MPL_LOGV
143                    ("ML State transition from %s to %s\n",
144                     inv_state_name(inv_params_obj.state),
145                     inv_state_name(newState)));
146        result = inv_run_state_callbacks(newState);
147        if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) {
148            MLStateCloseCallbacks();
149        }
150        inv_params_obj.state = newState;
151    }
152    return result;
153}
154
155/**
156 *  @internal
157 *  @brief  To be moved in mlstates.c
158**/
159unsigned char inv_get_state(void)
160{
161    return (inv_params_obj.state);
162}
163
164/**
165 * @internal
166 * @brief   This registers a function to be called each time the state
167 *          changes. It may also be called when the FIFO Rate is changed.
168 *          It will be called at the start of a state change before the
169 *          state change has taken place. See Also inv_unregister_state_callback()
170 *          The FIFO does not have to be on for this callback.
171 * @param func Function to be called when a DMP interrupt occurs.
172 * @return INV_SUCCESS or non-zero error code.
173 */
174
175inv_error_t inv_register_state_callback(state_change_callback_t callback)
176{
177    INVENSENSE_FUNC_START;
178    int kk;
179    inv_error_t result;
180
181    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
182    if (INV_SUCCESS != result) {
183        return result;
184    }
185    // Make sure we have not filled up our number of allowable callbacks
186    if (sStateChangeCallbacks.numStateChangeCallbacks <
187        MAX_STATE_CHANGE_PROCESSES) {
188        // Make sure we haven't registered this function already
189        for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
190            if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
191                result = INV_ERROR_INVALID_PARAMETER;
192                break;
193            }
194        }
195
196        if (INV_SUCCESS == result) {
197            // Add new callback
198            sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks.
199                                                       numStateChangeCallbacks]
200                = callback;
201            sStateChangeCallbacks.numStateChangeCallbacks++;
202        }
203    } else {
204        result = INV_ERROR_MEMORY_EXAUSTED;
205    }
206
207    inv_unlock_mutex(sStateChangeCallbacks.mutex);
208    return result;
209}
210
211/**
212 * @internal
213 * @brief   This unregisters a function to be called each time the state
214 *          changes. See Also inv_register_state_callback()
215 *          The FIFO does not have to be on for this callback.
216 * @return INV_SUCCESS or non-zero error code.
217 */
218inv_error_t inv_unregister_state_callback(state_change_callback_t callback)
219{
220    INVENSENSE_FUNC_START;
221    int kk, jj;
222    inv_error_t result;
223
224    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
225    if (INV_SUCCESS != result) {
226        return result;
227    }
228    // Make sure we haven't registered this function already
229    result = INV_ERROR_INVALID_PARAMETER;
230    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
231        if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
232            for (jj = kk + 1;
233                 jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) {
234                sStateChangeCallbacks.stateChangeCallbacks[jj - 1] =
235                    sStateChangeCallbacks.stateChangeCallbacks[jj];
236            }
237            sStateChangeCallbacks.numStateChangeCallbacks--;
238            result = INV_SUCCESS;
239            break;
240        }
241    }
242
243    inv_unlock_mutex(sStateChangeCallbacks.mutex);
244    return result;
245}
246
247inv_error_t inv_run_state_callbacks(unsigned char newState)
248{
249    int kk;
250    inv_error_t result;
251
252    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
253    if (INV_SUCCESS != result) {
254        MPL_LOGE("MLOsLockMutex returned %d\n", result);
255        return result;
256    }
257
258    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
259        if (sStateChangeCallbacks.stateChangeCallbacks[kk]) {
260            result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState);
261            if (INV_SUCCESS != result) {
262                break;          // Can't return, must release mutex
263            }
264        }
265    }
266
267    inv_unlock_mutex(sStateChangeCallbacks.mutex);
268    return result;
269}
270