camerad: protect concurrent access to shared variables and avoid race conditions (#1966)

* Protect concurrent access for shared resources

* fix
old-commit-hash: f3fb406eee

selfdrive/camerad/cameras/camera_frame_stream.cc

@@ -94,8 +94,6 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
 };
 
 void cameras_init(DualCameraState *s) {
-  memset(s, 0, sizeof(*s));
-
   camera_init(&s->rear, CAMERA_ID_IMX298, 20);
   s->rear.transform = (mat3){{
     1.0,  0.0, 0.0,

selfdrive/camerad/cameras/camera_qcom.cc

@@ -107,8 +107,6 @@ static void camera_release_buffer(void* cookie, int buf_idx) {
 static void camera_init(CameraState *s, int camera_id, int camera_num,
                         uint32_t pixel_clock, uint32_t line_length_pclk,
                         unsigned int max_gain, unsigned int fps) {
-  memset(s, 0, sizeof(*s));
-
   s->camera_num = camera_num;
   s->camera_id = camera_id;
 
@@ -261,8 +259,6 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li
 }
 
 void cameras_init(DualCameraState *s) {
-  memset(s, 0, sizeof(*s));
-
   char project_name[1024] = {0};
   property_get("ro.boot.project_name", project_name, "");
 
@@ -396,7 +392,9 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
 
   if (err == 0) {
     s->cur_exposure_frac = exposure_frac;
+    pthread_mutex_lock(&s->frame_info_lock);
     s->cur_gain_frac = gain_frac;
+    pthread_mutex_unlock(&s->frame_info_lock);
   }
 
   //LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err);
@@ -413,16 +411,20 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
     const unsigned int exposure_time_min = 16;
     const unsigned int exposure_time_max = frame_length - 11; // copied from set_exposure()
 
+    float cur_gain_frac = s->cur_gain_frac;
     float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05);
-    if (s->cur_gain_frac > 0.125 && exposure_factor < 1) {
-      s->cur_gain_frac *= exposure_factor;
+    if (cur_gain_frac > 0.125 && exposure_factor < 1) {
+      cur_gain_frac *= exposure_factor;
     } else if (s->cur_integ_lines * exposure_factor <= exposure_time_max && s->cur_integ_lines * exposure_factor >= exposure_time_min) { // adjust exposure time first
       s->cur_exposure_frac *= exposure_factor;
-    } else if (s->cur_gain_frac * exposure_factor <= gain_frac_max && s->cur_gain_frac * exposure_factor >= gain_frac_min) {
-      s->cur_gain_frac *= exposure_factor;
+    } else if (cur_gain_frac * exposure_factor <= gain_frac_max && cur_gain_frac * exposure_factor >= gain_frac_min) {
+      cur_gain_frac *= exposure_factor;
     }
+    pthread_mutex_lock(&s->frame_info_lock);
+    s->cur_gain_frac = cur_gain_frac;
+    pthread_mutex_unlock(&s->frame_info_lock);
 
-    set_exposure(s, s->cur_exposure_frac, s->cur_gain_frac);
+    set_exposure(s, s->cur_exposure_frac, cur_gain_frac);
 
   } else { // keep the old for others
     float new_exposure = s->cur_exposure_frac;
@@ -1794,15 +1796,16 @@ static void do_autofocus(CameraState *s) {
   const int dac_up = s->device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP;
   const int dac_down = s->device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN;
 
+  float lens_true_pos = s->lens_true_pos;
   if (!isnan(err))  {
     // learn lens_true_pos
-    s->lens_true_pos -= err*focus_kp;
+    lens_true_pos -= err*focus_kp;
   }
 
   // stay off the walls
-  s->lens_true_pos = clamp(s->lens_true_pos, dac_down, dac_up);
-
-  int target = clamp(s->lens_true_pos - sag, dac_down, dac_up);
+  lens_true_pos = clamp(lens_true_pos, dac_down, dac_up);
+  int target = clamp(lens_true_pos - sag, dac_down, dac_up);
+  s->lens_true_pos = lens_true_pos;
 
   /*char debug[4096];
   char *pdebug = debug;

selfdrive/camerad/cameras/camera_qcom.h

@@ -1,10 +1,10 @@
-#ifndef CAMERA_H
-#define CAMERA_H
+#pragma once
 
 #include <stdint.h>
 #include <stdbool.h>
 #include <pthread.h>
 #include <czmq.h>
+#include <atomic>
 #include "messaging.hpp"
 
 #include "msmb_isp.h"
@@ -96,7 +96,7 @@ typedef struct CameraState {
   int cur_frame_length;
   int cur_integ_lines;
 
-  float digital_gain;
+  std::atomic<float> digital_gain;
 
   StreamState ss[3];
 
@@ -113,9 +113,9 @@ typedef struct CameraState {
   uint16_t cur_lens_pos;
   uint64_t last_sag_ts;
   float last_sag_acc_z;
-  float lens_true_pos;
+  std::atomic<float> lens_true_pos;
 
-  int self_recover; // af recovery counter, neg is patience, pos is active
+  std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
 
   int fps;
 
@@ -144,5 +144,3 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size
 #ifdef __cplusplus
 }  // extern "C"
 #endif
-
-#endif

selfdrive/camerad/cameras/camera_webcam.cc

@@ -222,7 +222,6 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
 };
 
 void cameras_init(DualCameraState *s) {
-  memset(s, 0, sizeof(*s));
 
   camera_init(&s->rear, CAMERA_ID_LGC920, 20);
   s->rear.transform = (mat3){{

selfdrive/camerad/main.cc

@@ -477,25 +477,26 @@ void* processing_thread(void *arg) {
     t10 = millis_since_boot();*/
 
     // setup self recover
+    const float lens_true_pos = s->cameras.rear.lens_true_pos;
     if (is_blur(&s->lapres[0]) &&
-       (s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 ||
-        s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) &&
+       (lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 ||
+        lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) &&
        s->cameras.rear.self_recover < 2) {
       // truly stuck, needs help
       s->cameras.rear.self_recover -= 1;
       if (s->cameras.rear.self_recover < -FOCUS_RECOVER_PATIENCE) {
         LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d",
-                                      s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover);
-        s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at
+                                      lens_true_pos, s->cameras.rear.self_recover.load());
+        s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at
       }
-    } else if ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) ||
-               s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) &&
+    } else if ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) ||
+               lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) &&
               s->cameras.rear.self_recover < 2) {
       // in suboptimal position with high prob, but may still recover by itself
       s->cameras.rear.self_recover -= 1;
       if (s->cameras.rear.self_recover < -(FOCUS_RECOVER_PATIENCE*3)) {
-        LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover);
-        s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0);
+        LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load());
+        s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0);
       }
     } else if (s->cameras.rear.self_recover < 0) {
       s->cameras.rear.self_recover += 1; // reset if fine
@@ -1267,7 +1268,7 @@ int main(int argc, char *argv[]) {
   signal(SIGINT, (sighandler_t)set_do_exit);
   signal(SIGTERM, (sighandler_t)set_do_exit);
 
-  VisionState state = {0};
+  VisionState state = {};
   VisionState *s = &state;
 
   clu_init();

selfdrive/camerad/test/camera/test.c

@@ -34,7 +34,7 @@ void tbuffer_stop(TBuffer *tb) {
 }
 
 int main() {
-  DualCameraState s;
+  DualCameraState s={};
   cameras_init(&s);
   VisionBuf camera_bufs_rear[0x10] = {0};
   VisionBuf camera_bufs_focus[0x10] = {0};