Positioning Poc: fix mutex handling in register functions. Thanks, Marco for the review

6 files changed, 166 insertions, 134 deletions

diff --git a/gnss-service/src/gnss-impl.c b/gnss-service/src/gnss-impl.c
index 5f183b5..2fbb74b 100644
--- a/gnss-service/src/gnss-impl.c
+++ b/gnss-service/src/gnss-impl.c
@@ -22,13 +22,13 @@
 static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
-static TGNSSSatelliteDetail gSatelliteDetail; //TODO: buffer full set of satellite details for one point in time
+static TGNSSSatelliteDetail gSatelliteDetail = {0}; //TODO: buffer full set of satellite details for one point in time
 static GNSSSatelliteDetailCallback cbSatelliteDetail = 0;
 
-static TGNSSPosition gPosition;
+static TGNSSPosition gPosition = {0};
 static volatile GNSSPositionCallback cbPosition = 0;
 
-static TGNSSTime gTime;
+static TGNSSTime gTime = {0};
 static volatile GNSSTimeCallback cbTime = 0;
 
 
@@ -36,131 +36,140 @@ static volatile GNSSTimeCallback cbTime = 0;
 
 bool gnssRegisterSatelliteDetailCallback(GNSSSatelliteDetailCallback callback)
 {
-    if(cbSatelliteDetail != 0) 
-    {
-        return false; //if already registered
-    }
+    bool retval = false;
 
     pthread_mutex_lock(&mutexCb);
-    cbSatelliteDetail = callback;
+    //only if valid callback and not already registered
+    if(callback && !cbSatelliteDetail)
+    {
+        cbSatelliteDetail = callback;
+        retval = true;
+    }
     pthread_mutex_unlock(&mutexCb);
 
-    return true;
+    return retval;
 }
 
 bool gnssDeregisterSatelliteDetailCallback(GNSSSatelliteDetailCallback callback)
 {
-    if(cbSatelliteDetail == callback && callback != 0)
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbSatelliteDetail == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbSatelliteDetail = 0;
-        pthread_mutex_unlock(&mutexCb);
-
-        return true;
+        retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
-    return false;
+    return retval;
 }
 
 bool gnssGetSatelliteDetails(TGNSSSatelliteDetail* satelliteDetails, uint16_t count, uint16_t* numSatelliteDetails)
 {
-    if(!satelliteDetails || !count)
-    {
-        return false;
-    }
+    bool retval = false;
 
+    if(satelliteDetails && count)
+    {
 //TODO: return full set of satellite details for one point in time
-    pthread_mutex_lock(&mutexData);
-    *satelliteDetails = gSatelliteDetail;
-    *numSatelliteDetails = 1; 
-    pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexData);
+        *satelliteDetails = gSatelliteDetail;
+        *numSatelliteDetails = 1;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
 
-    return true;
+    return retval;
 }
 
 bool gnssRegisterPositionCallback(GNSSPositionCallback callback)
 {
-    if(cbPosition != 0) 
-    {
-        return false; //if already registered
-    }
+    bool retval = false;
 
     pthread_mutex_lock(&mutexCb);
-    cbPosition = callback;
+    //only if valid callback and not already registered
+    if(callback && !cbPosition)
+    {
+        cbPosition = callback;
+        retval = true;
+    }
     pthread_mutex_unlock(&mutexCb);
 
-    return true;
+    return retval;
 }
 
 bool gnssDeregisterPositionCallback(GNSSPositionCallback callback)
 {
-    if(cbPosition == callback && callback != 0)
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbPosition == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbPosition = 0;
-        pthread_mutex_unlock(&mutexCb);
-
-        return true;
+        retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
-    return false;
+    return retval;
 }
 
 bool gnssGetPosition(TGNSSPosition* position)
 {
-    if(!position)
+    bool retval = false;
+    if(position)
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *position = gPosition;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *position = gPosition;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;
 }
 
 
 bool gnssRegisterTimeCallback(GNSSTimeCallback callback)
 {
-    if(cbTime != 0) 
-    {
-        return false; //if already registered
-    }
+    bool retval = false;
 
     pthread_mutex_lock(&mutexCb);
-    cbTime = callback;
+    //only if valid callback and not already registered
+    if(callback && !cbTime)
+    {
+        cbTime = callback;
+        retval = true;
+    }
     pthread_mutex_unlock(&mutexCb);
 
-    return true;
+    return retval;
 }
 
+
 bool gnssDeregisterTimeCallback(GNSSTimeCallback callback)
 {
-    if(cbTime == callback && callback != 0)
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbTime == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbTime = 0;
-        pthread_mutex_unlock(&mutexCb);
-
-        return true;
+        retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
-    return false;
+    return retval;
 }
 
 bool gnssGetTime(TGNSSTime* time)
 {
-    if(!time)
+    bool retval = false;
+    if(time)
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *time = gTime;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *time = gTime;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;
 }
 
 
diff --git a/gnss-service/src/gnss-use-replayer.c b/gnss-service/src/gnss-use-replayer.c
index 336a55d..cd6d244 100644
--- a/gnss-service/src/gnss-use-replayer.c
+++ b/gnss-service/src/gnss-use-replayer.c
@@ -472,44 +472,61 @@ void *listenForMessages( void *ptr )
     }
 
     while(isRunning == true)
-    { 
-        readBytes = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *)&si_other, (socklen_t *)&slen);
-
-        if(readBytes < 0)
+    {
+        //use select to introduce a timeout - alloy shutdown even when no data are received
+        fd_set readfs;    /* file descriptor set */
+        int    maxfd;     /* maximum file desciptor used */        
+        int res;
+        struct timeval Timeout;
+        /* set timeout value within input loop */
+        Timeout.tv_usec = 0;  /* milliseconds */
+        Timeout.tv_sec  = 1;  /* seconds */
+        FD_SET(s, &readfs);
+        maxfd = s+1;
+        /* block until input becomes available */
+        res = select(maxfd, &readfs, NULL, NULL, &Timeout);
+
+        if (res > 0)
         {
-            LOG_ERROR_MSG(gContext,"recvfrom() failed!");
-            exit(EXIT_FAILURE);
-        }
-        buf[readBytes] = '\0';
-
-        LOG_DEBUG_MSG(gContext,"------------------------------------------------");
-
-        LOG_DEBUG(gContext,"Received Packet from %s:%d", 
-                  inet_ntoa(si_other.sin_addr), ntohs(si_other.sin_port));
-
-        sscanf(buf, "%*[^'$']$%[^',']", msgId);
+                    
+            readBytes = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *)&si_other, (socklen_t *)&slen);
+    
+            if(readBytes < 0)
+            {
+                LOG_ERROR_MSG(gContext,"recvfrom() failed!");
+                exit(EXIT_FAILURE);
+            }
+            buf[readBytes] = '\0';
 
-        LOG_DEBUG(gContext,"MsgID:%s",msgId);
-        LOG_DEBUG(gContext,"Len:%d",strlen(buf));
-        LOG_DEBUG(gContext,"Data:%s",buf);
+            LOG_DEBUG_MSG(gContext,"------------------------------------------------");
 
-        LOG_DEBUG_MSG(gContext,"------------------------------------------------");
+            LOG_DEBUG(gContext,"Received Packet from %s:%d", 
+                      inet_ntoa(si_other.sin_addr), ntohs(si_other.sin_port));
 
-        if(strcmp("GVGNSP", msgId) == 0)
-        {
-             processGVGNSP(buf);
-        }
-        else if(strcmp("GVGNSC", msgId) == 0)
-        {
-             processGVGNSC(buf);
-        }
-        else if(strcmp("GVGNSAC", msgId) == 0)
-        {
-             processGVGNSAC(buf);
-        }
-        else if(strcmp("GVGNSSAT", msgId) == 0)
-        {
-             processGVGNSSAT(buf);
+            sscanf(buf, "%*[^'$']$%[^',']", msgId);
+    
+            LOG_DEBUG(gContext,"MsgID:%s",msgId);
+            LOG_DEBUG(gContext,"Len:%d",strlen(buf));
+            LOG_DEBUG(gContext,"Data:%s",buf);
+
+            LOG_DEBUG_MSG(gContext,"------------------------------------------------");
+
+            if(strcmp("GVGNSP", msgId) == 0)
+            {
+                 processGVGNSP(buf);
+            }
+            else if(strcmp("GVGNSC", msgId) == 0)
+            {
+                 processGVGNSC(buf);
+            }
+            else if(strcmp("GVGNSAC", msgId) == 0)
+            {
+                 processGVGNSAC(buf);
+            }
+            else if(strcmp("GVGNSSAT", msgId) == 0)
+            {
+                 processGVGNSSAT(buf);
+            }
         }
 
     }
@@ -519,6 +536,3 @@ void *listenForMessages( void *ptr )
     return EXIT_SUCCESS;
 }
 
-
-
-
diff --git a/sensors-service/src/acceleration.c b/sensors-service/src/acceleration.c
index 1ecc7b2..8506b8d 100644
--- a/sensors-service/src/acceleration.c
+++ b/sensors-service/src/acceleration.c
@@ -24,7 +24,7 @@ static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the ca
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
 static volatile AccelerationCallback cbAcceleration = 0;
-static TAccelerationData gAccelerationData;
+static TAccelerationData gAccelerationData = {0};
 TAccelerationConfiguration gAccelerationConfiguration;
 
 bool iAccelerationInit()
@@ -34,10 +34,11 @@ bool iAccelerationInit()
     pthread_mutex_unlock(&mutexCb);
     
     pthread_mutex_lock(&mutexData);
+    gAccelerationData.validityBits = 0;
     //example accelerometer configuration for a 3-axis accelerometer
     gAccelerationConfiguration.dist2RefPointX = 0;
     gAccelerationConfiguration.dist2RefPointY = 0;
-    gAccelerationConfiguration.dist2RefPointZ = 0;    
+    gAccelerationConfiguration.dist2RefPointZ = 0;
     gAccelerationConfiguration.angleYaw = 0;
     gAccelerationConfiguration.anglePitch = 0;
     gAccelerationConfiguration.angleRoll = 0;
@@ -84,14 +85,15 @@ bool snsAccelerationRegisterCallback(AccelerationCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     //only if valid callback and not already registered
     if(callback && !cbAcceleration)
     {
-        pthread_mutex_lock(&mutexCb);
         cbAcceleration = callback;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
+
     return retval;
 }
 
@@ -99,13 +101,13 @@ bool snsAccelerationDeregisterCallback(AccelerationCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     if((cbAcceleration == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbAcceleration = 0;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
     return retval;
 }
diff --git a/sensors-service/src/gyroscope.c b/sensors-service/src/gyroscope.c
index fa1b2b1..69ad81d 100644
--- a/sensors-service/src/gyroscope.c
+++ b/sensors-service/src/gyroscope.c
@@ -24,7 +24,7 @@ static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the ca
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
 static volatile GyroscopeCallback cbGyroscope = 0;
-static TGyroscopeData gGyroscopeData;
+static TGyroscopeData gGyroscopeData = {0};
 TGyroscopeConfiguration gGyroscopeConfiguration;
 
 bool iGyroscopeInit()
@@ -34,6 +34,7 @@ bool iGyroscopeInit()
     pthread_mutex_unlock(&mutexCb);
 
     pthread_mutex_lock(&mutexData);
+    gGyroscopeData.validityBits = 0;
     //example gyroscope configuration for a 3-axis gyro
     gGyroscopeConfiguration.angleYaw = 0;
     gGyroscopeConfiguration.anglePitch = 0;
@@ -73,21 +74,22 @@ bool snsGyroscopeGetGyroscopeData(TGyroscopeData * gyroData)
         pthread_mutex_unlock(&mutexData);
         retval = true;
     }
-    return retval;    
+    return retval;
 }
 
 bool snsGyroscopeRegisterCallback(GyroscopeCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     //only if valid callback and not already registered
     if(callback && !cbGyroscope)
     {
-        pthread_mutex_lock(&mutexCb);
         cbGyroscope = callback;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
+
     return retval;
 }
 
@@ -95,13 +97,13 @@ bool snsGyroscopeDeregisterCallback(GyroscopeCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     if((cbGyroscope == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbGyroscope = 0;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
     return retval;
 }
diff --git a/sensors-service/src/vehicle-speed.c b/sensors-service/src/vehicle-speed.c
index c4379f4..0825f4c 100644
--- a/sensors-service/src/vehicle-speed.c
+++ b/sensors-service/src/vehicle-speed.c
@@ -19,7 +19,7 @@ static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the ca
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
 static volatile VehicleSpeedCallback cbVehicleSpeed = 0;
-static TVehicleSpeedData gVehicleSpeedData;
+static TVehicleSpeedData gVehicleSpeedData = {0};
 
 bool iVehicleSpeedInit()
 {
@@ -27,6 +27,10 @@ bool iVehicleSpeedInit()
     cbVehicleSpeed = 0;
     pthread_mutex_unlock(&mutexCb);
 
+    pthread_mutex_lock(&mutexData);
+    gVehicleSpeedData.validityBits = 0;
+    pthread_mutex_unlock(&mutexData);
+
     return true;
 }
 
@@ -41,44 +45,44 @@ bool iVehicleSpeedDestroy()
 
 bool snsVehicleSpeedGetVehicleSpeedData(TVehicleSpeedData* vehicleSpeed)
 {
-    if(!vehicleSpeed)
+    bool retval = false;
+    if(vehicleSpeed)
     {
-    	return false;
+        pthread_mutex_lock(&mutexData);
+        *vehicleSpeed = gVehicleSpeedData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *vehicleSpeed = gVehicleSpeedData;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;
 }
 
 bool snsVehicleSpeedRegisterCallback(VehicleSpeedCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     //only if valid callback and not already registered
     if(callback && !cbVehicleSpeed)
     {
-        pthread_mutex_lock(&mutexCb);
         cbVehicleSpeed = callback;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
-    return retval;    
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
 }
 
 bool snsVehicleSpeedDeregisterCallback(VehicleSpeedCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     if((cbVehicleSpeed == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbVehicleSpeed = 0;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
     return retval;
 }
diff --git a/sensors-service/src/wheeltick.c b/sensors-service/src/wheeltick.c
index 972b80f..9944a0c 100644
--- a/sensors-service/src/wheeltick.c
+++ b/sensors-service/src/wheeltick.c
@@ -23,7 +23,7 @@ static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the ca
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
 static volatile WheeltickCallback cbWheelticks = 0;
-static TWheelticks gWheelticks;
+static TWheelticks gWheelticks = {0};
 
 bool iWheeltickInit()
 {
@@ -63,21 +63,22 @@ bool snsWheeltickGetWheelticks(TWheelticks * ticks)
         pthread_mutex_unlock(&mutexData);
         retval = true;
     }
-    return retval;    
+    return retval;
 }
 
 bool snsWheeltickRegisterCallback(WheeltickCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     //only if valid callback and not already registered
     if(callback && !cbWheelticks)
     {
-        pthread_mutex_lock(&mutexCb);
         cbWheelticks = callback;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
+
     return retval;
 }
 
@@ -85,15 +86,15 @@ bool snsWheeltickDeregisterCallback(WheeltickCallback callback)
 {
     bool retval = false;
 
+    pthread_mutex_lock(&mutexCb);
     if((cbWheelticks == callback) && callback)
     {
-        pthread_mutex_lock(&mutexCb);
         cbWheelticks = 0;
-        pthread_mutex_unlock(&mutexCb);
         retval = true;
     }
+    pthread_mutex_unlock(&mutexCb);
 
-    return retval;    
+    return retval;
 }
 
 void updateWheelticks(const TWheelticks ticks[], uint16_t numElements)