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
56 struct state_callback_obj {
57 int_fast8_t numStateChangeCallbacks;
58 HANDLE mutex;
59 state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES];
60 };
61
62 static struct state_callback_obj sStateChangeCallbacks = { 0 };
63
64 /* --------------- */
65 /* - Functions. - */
66 /* --------------- */
67
inv_init_state_callbacks(void)68 static inv_error_t inv_init_state_callbacks(void)
69 {
70 memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
71 return inv_create_mutex(&sStateChangeCallbacks.mutex);
72 }
73
MLStateCloseCallbacks(void)74 static 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 **/
inv_state_name(unsigned char state)88 char *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 **/
inv_state_transition(unsigned char newState)118 inv_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 **/
inv_get_state(void)159 unsigned 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
inv_register_state_callback(state_change_callback_t callback)175 inv_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 */
inv_unregister_state_callback(state_change_callback_t callback)218 inv_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
inv_run_state_callbacks(unsigned char newState)247 inv_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