robofsm: Added lock checker

diff --git a/build/linux/config.target b/build/linux/config.target
index 2e589a61ca33d69b6725b2370a9ffffa7f5d03be..7a49ddffb3637af36e3acab7ee1abbc9b040fcf9 100644 (file)
--- a/build/linux/config.target
+++ b/build/linux/config.target
@@ -1,4 +1,5 @@
 #CONFIG_OC_ETH_ORTE_IDL=y
+CONFIG_LOCK_CHECKING=y
 LN_HEADERS=y
 OMIT_KERNEL_PASSES=y
 CFLAGS = -O2 -Wall -fno-strict-aliasing

diff --git a/src/robodata/robodata.h b/src/robodata/robodata.h
index fe610d17d0132335d159d4cc522c9029615d49ca..caa717569dabbe36fc40b2e9636ceff9e54c0090 100644 (file)
--- a/src/robodata/robodata.h
+++ b/src/robodata/robodata.h
@@ -23,22 +23,6 @@ enum {
 #define FSM(id) (&robot.fsm[FSM_ID_##id])
 #define FSM_GET_BY_ID(fsm_id) (&robot.fsm[FSM_ID_##fsm_id])
 
-/**
- * LOCKING MANIPULATION 
- */
-
-/** 
- * Locks the robot structure.
- * @param var Field in the structure you are going to work with.
- */
-#define ROBOT_LOCK(var)                (pthread_mutex_lock(&robot.__robot_lock_##var))
-
-/** 
- * Unlocks the robot structure.
- * @param var Field in the structure, which was locked by ROBOT_LOCK.
- */
-#define ROBOT_UNLOCK(var)      (pthread_mutex_unlock(&robot.__robot_lock_##var))
-
 /**     
  * DEBUG MACROS
  */

diff --git a/src/robofsm/eb2008/Makefile.omk b/src/robofsm/eb2008/Makefile.omk
index 3514a9647defe5d59a258f9904c4a1ca74504676..e6f32cbbe90f24f68c1b09021718f0a76345ac92 100644 (file)
--- a/src/robofsm/eb2008/Makefile.omk
+++ b/src/robofsm/eb2008/Makefile.omk
@@ -2,8 +2,10 @@
 
 #SUBDIRS = test
 
-default_CONFIG = CONFIG_OPEN_LOOP=n
-LOCAL_CONFIG_H = robot_config.h
+default_CONFIG = CONFIG_OPEN_LOOP=n CONFIG_LOCK_CHECKING=n
+
+config_include_HEADERS = robot_config.h
+robot_config_DEFINES = CONFIG_OPEN_LOOP CONFIG_LOCK_CHECKING
 
 bin_PROGRAMS += robomain
 robomain_SOURCES = fsmmain.cc

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 70420357b7f467a717b4ddfbeb7144d6f907780d..30f598f11994a1fcd97d9bb8f2a8efefc8cc4d3e 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -41,6 +41,11 @@ static char *fsm_name[] = {
 
 struct robot_eb2008 robot;
 
+#ifdef CONFIG_LOCK_CHECKING
+struct lock_log robot_lock_log;
+#endif
+
+
 static void block_signals()
 {
        sigset_t sigset;

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 0a745fee54a729b3b5212347624e7ce66fd093d2..810494ea316003a0f88c5a48bb362dfb17bb2dd4 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -23,6 +23,7 @@
 #include <roboevent_eb2008.h>
 #include <fsm.h>
 #include <servos_eb2008.h>
+#include <robot_config.h>
 
 /* Select robot color. If we are BLUE, no coordinate transformation will
  * be performed. */
@@ -65,6 +66,48 @@ enum robot_fsm_id {
 #define LAS_MEAS_PERI  1       /* period index */
 #define LAS_MEAS_DATAI 2       /* data index */
 
+/**
+ * LOCKING MANIPULATION 
+ */
+
+#ifdef CONFIG_LOCK_CHECKING
+#define LOCK_CHECK_COUNT 10
+struct lock_log {
+       pthread_mutex_t *locked[LOCK_CHECK_COUNT];
+       int idx;
+};
+
+extern struct lock_log robot_lock_log;
+
+#define __LOCK_CHECK(mutex) robot_lock_log.locked[robot_lock_log.idx++] = mutex;
+#define __UNLOCK_CHECK(mutex)                                          \
+       if (robot_lock_log.locked[--robot_lock_log.idx] != mutex ||     \
+           robot_lock_log.idx < 0)                                     \
+               printf("!!! Locking error %s:%d\n", __FILE__, __LINE__);
+#else
+#define __LOCK_CHECK(mutex)
+#define __UNLOCK_CHECK(mutex)
+#endif
+/** 
+ * Locks the robot structure.
+ * @param var Field in the structure you are going to work with.
+ */
+#define ROBOT_LOCK(var)                                                 \
+       do {                                                     \
+               pthread_mutex_lock(&robot.__robot_lock_##var);   \
+               __LOCK_CHECK(&robot.__robot_lock_##var);         \
+       } while(0)
+
+/** 
+ * Unlocks the robot structure.
+ * @param var Field in the structure, which was locked by ROBOT_LOCK.
+ */
+#define ROBOT_UNLOCK(var)                                              \
+       do {                                                            \
+               __UNLOCK_CHECK(&robot.__robot_lock_##var);              \
+               pthread_mutex_unlock(&robot.__robot_lock_##var);        \
+       } while(0)
+
 /* Mapping of robot structure fields to locks */
 //#define __robot_lock_ lock   /* ROBOT_LOCK() */
 #define __robot_lock_ref_pos           lock_ref_pos