Home | History | Annotate | Download | only in mllite 
      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 
     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 
     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 **/
     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 **/
    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 **/
    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 
    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  */
    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 
    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