mout: Fix missing unlock + unify the code

diff --git a/rpp/src/rpp/mout.c b/rpp/src/rpp/mout.c
index d8f116b7f0d51623c05ef66deebbdece7f1d8b6b..6f9ce2ede9a3ab2abee7ee50a0c483efecf2c735 100644 (file)
--- a/rpp/src/rpp/mout.c
+++ b/rpp/src/rpp/mout.c
@@ -1,4 +1,4 @@
-/* Copyright (C) 2013 Czech Technical University in Prague
+/* Copyright (C) 2013, 2016 Czech Technical University in Prague
  *
  * Authors:
  *     - Carlos Jenkins <carlos@jenkins.co.cr>
@@ -52,6 +52,7 @@ static uint8_t cache[] = {
 
 int8_t rpp_mout_set(uint8_t pin, uint8_t val)
 {
+       int ret;
        // Check range
        if ((pin < 1) || (pin > 6))
                return -1;
@@ -65,10 +66,12 @@ int8_t rpp_mout_set(uint8_t pin, uint8_t val)
 #ifndef FREERTOS_POSIX
        // Set and store value
        RPP_MUTEX_LOCK(mutex_mout);
-       if (drv_mout_set(idx, val) != SUCCESS)
-               return -3;
-       cache[idx] = val;
+       ret = drv_mout_set(idx, val);
+       if (ret == SUCCESS)
+               cache[idx] = val;
        RPP_MUTEX_UNLOCK(mutex_mout);
+       if (ret != SUCCESS)
+               return -3;
 
        // FIXME Wait some time for synchronization
        // Don't use vTaskDelay() here because the minimum time to wait is one tick,
@@ -81,11 +84,11 @@ int8_t rpp_mout_set(uint8_t pin, uint8_t val)
 
        RPP_MUTEX_LOCK(mutex_mout);
        // Get value back and compare
-       if (drv_mout_diag(idx) == FAILURE) {
-               RPP_MUTEX_UNLOCK(mutex_mout);
+       ret = drv_mout_diag(idx);
+       RPP_MUTEX_UNLOCK(mutex_mout);
+       if (ret < 0) {
                return -4;
        }
-       RPP_MUTEX_UNLOCK(mutex_mout);
 
 #else
        RPP_MUTEX_LOCK(mutex_mout);