diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/mlstates.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/mlstates.c | 269 |
1 files changed, 269 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/mlstates.c b/libsensors/mlsdk/mllite/mlstates.c new file mode 100644 index 0000000..e44f100 --- /dev/null +++ b/libsensors/mlsdk/mllite/mlstates.c @@ -0,0 +1,269 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLSTATES + * @brief Basic state machine definition and support for the Motion Library. + * + * @{ + * @file mlstates.c + * @brief The Motion Library state machine definition. + */ + +#define ML_C + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include <stdio.h> +#include <string.h> + +#include "mlstates.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "ml.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlstates" + +#define _stateDebug(x) //{x} + +#define MAX_STATE_CHANGE_PROCESSES (8) + +struct state_callback_obj { + int_fast8_t numStateChangeCallbacks; + HANDLE mutex; + state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES]; +}; + +static struct state_callback_obj sStateChangeCallbacks = { 0, 0, { NULL } }; + +/* --------------- */ +/* - Functions. - */ +/* --------------- */ + +static inv_error_t inv_init_state_callbacks(void) +{ + memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); + return inv_create_mutex(&sStateChangeCallbacks.mutex); +} + +static inv_error_t MLStateCloseCallbacks(void) +{ + inv_error_t result; + result = inv_destroy_mutex(sStateChangeCallbacks.mutex); + memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); + return result; +} + +/** + * @internal + * @brief return a string containing the label assigned to the given state. + * @param state The state of which the label has to be returned. + * @return A string containing the state label. +**/ +char *inv_state_name(unsigned char state) +{ + switch (state) { + case INV_STATE_SERIAL_CLOSED: + return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED); + break; + case INV_STATE_SERIAL_OPENED: + return INV_STATE_NAME(INV_STATE_SERIAL_OPENED); + break; + case INV_STATE_DMP_OPENED: + return INV_STATE_NAME(INV_STATE_DMP_OPENED); + break; + case INV_STATE_DMP_STARTED: + return INV_STATE_NAME(INV_STATE_DMP_STARTED); + break; + default: + return NULL; + } +} + +/** + * @internal + * @brief Perform a transition from the current state to newState. + * Check for the correctness of the transition. + * Print out an error message if the transition is illegal . + * This routine is also called if a certain normally constant parameters + * are changed such as the FIFO Rate. + * @param newState state we are transitioning to. + * @return +**/ +inv_error_t inv_state_transition(unsigned char newState) +{ + inv_error_t result = INV_SUCCESS; + + if (newState == INV_STATE_SERIAL_CLOSED) { + // Always allow transition to closed + } else if (newState == INV_STATE_SERIAL_OPENED) { + inv_init_state_callbacks(); // Always allow first transition to start over + } else if (((newState == INV_STATE_DMP_OPENED) && + ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) || + (inv_params_obj.state == INV_STATE_DMP_STARTED))) + || + ((newState == INV_STATE_DMP_STARTED) && + (inv_params_obj.state == INV_STATE_DMP_OPENED))) { + // Valid transitions but no special action required + } else { + // All other combinations are illegal + MPL_LOGE("Error : illegal state transition from %s to %s\n", + inv_state_name(inv_params_obj.state), + inv_state_name(newState)); + result = INV_ERROR_SM_TRANSITION; + } + + if (result == INV_SUCCESS) { + _stateDebug(MPL_LOGV + ("ML State transition from %s to %s\n", + inv_state_name(inv_params_obj.state), + inv_state_name(newState))); + result = inv_run_state_callbacks(newState); + if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) { + MLStateCloseCallbacks(); + } + inv_params_obj.state = newState; + } + return result; +} + +/** + * @internal + * @brief To be moved in mlstates.c +**/ +unsigned char inv_get_state(void) +{ + return (inv_params_obj.state); +} + +/** + * @internal + * @brief This registers a function to be called each time the state + * changes. It may also be called when the FIFO Rate is changed. + * It will be called at the start of a state change before the + * state change has taken place. See Also inv_unregister_state_callback() + * The FIFO does not have to be on for this callback. + * @param func Function to be called when a DMP interrupt occurs. + * @return INV_SUCCESS or non-zero error code. + */ + +inv_error_t inv_register_state_callback(state_change_callback_t callback) +{ + INVENSENSE_FUNC_START; + int kk; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we have not filled up our number of allowable callbacks + if (sStateChangeCallbacks.numStateChangeCallbacks < + MAX_STATE_CHANGE_PROCESSES) { + // Make sure we haven't registered this function already + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { + result = INV_ERROR_INVALID_PARAMETER; + break; + } + } + + if (INV_SUCCESS == result) { + // Add new callback + sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks. + numStateChangeCallbacks] + = callback; + sStateChangeCallbacks.numStateChangeCallbacks++; + } + } else { + result = INV_ERROR_MEMORY_EXAUSTED; + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} + +/** + * @internal + * @brief This unregisters a function to be called each time the state + * changes. See Also inv_register_state_callback() + * The FIFO does not have to be on for this callback. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_unregister_state_callback(state_change_callback_t callback) +{ + INVENSENSE_FUNC_START; + int kk, jj; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we haven't registered this function already + result = INV_ERROR_INVALID_PARAMETER; + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { + for (jj = kk + 1; + jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) { + sStateChangeCallbacks.stateChangeCallbacks[jj - 1] = + sStateChangeCallbacks.stateChangeCallbacks[jj]; + } + sStateChangeCallbacks.numStateChangeCallbacks--; + result = INV_SUCCESS; + break; + } + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} + +inv_error_t inv_run_state_callbacks(unsigned char newState) +{ + int kk; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + MPL_LOGE("MLOsLockMutex returned %d\n", result); + return result; + } + + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk]) { + result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState); + if (INV_SUCCESS != result) { + break; // Can't return, must release mutex + } + } + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} |