split each i2c to different thread and replace polling with signaling

sw/airborne/arch/linux/mcu_periph/i2c_arch.c

@@ -1,6 +1,7 @@
 /*
  *
  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               2018 Kirk Scheper <kirkscheper@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -40,23 +41,71 @@
 #define I2C_THREAD_PRIO 10
 #endif
 
-static void *i2c_thread(void *data __attribute__((unused)));
-static pthread_mutex_t i2c_mutex = PTHREAD_MUTEX_INITIALIZER;
-static bool thread_initialized = false;
+static void *i2c_thread(void *data);
+static bool i2c_threads_initialized = false;
 
-void i2c_complete_trans(struct i2c_periph *p);
+struct i2c_thread_data_t
+{
+  struct i2c_periph *p;
+  pthread_mutex_t mutex;
+  pthread_cond_t condition;
+};
+
+#define N_I2C 4
+struct i2c_tread_data_t i2c_threads_data[N_I2C];
 
 static void i2c_arch_init(void)
 {
-  pthread_mutex_init(&i2c_mutex, NULL);
+  if (i2c_threads_initialized){
+    return;
+  }
 
   pthread_t tid;
-  if (pthread_create(&tid, NULL, i2c_thread, NULL) != 0) {
+
+  /* initialise thread data array*/
+  for (int i = 0; i < N_I2C; i++){
+    i2c_threads_data[i].p = NULL;
+    pthread_mutex_init(i2c_threads_data[i].mutex, NULL);
+    pthread_cond_init(i2c_threads_data[i].condition, NULL);
+  }
+
+#if USE_I2C0
+  i2c_threads_data[0].p = &i2c0;
+  if (pthread_create(&tid, NULL, i2c_thread, (void*)(&i2c_threads_data[0])) != 0) {
+    fprintf(stderr, "i2c_arch_init: Could not create I2C thread.\n");
+    return;
+  }
+  pthread_setname_np(tid, "pprz_i2c0_thread");
+#endif
+
+#if USE_I2C1
+  i2c_threads_data[1].p = &i2c1;
+  if (pthread_create(&tid, NULL, i2c_thread, (void*)(&i2c_threads_data[1])) != 0) {
+    fprintf(stderr, "i2c_arch_init: Could not create I2C thread.\n");
+    return;
+  }
+  pthread_setname_np(tid, "pprz_i2c1_thread");
+#endif
+
+#if USE_I2C2
+  i2c_threads_data[2].p = &i2c2;
+  if (pthread_create(&tid, NULL, i2c_thread, (void*)(&i2c_threads_data[2])) != 0) {
     fprintf(stderr, "i2c_arch_init: Could not create I2C thread.\n");
     return;
   }
-  pthread_setname_np(tid, "pprz_i2c_thread");
-  thread_initialized = true;
+  pthread_setname_np(tid, "pprz_i2c2_thread");
+#endif
+
+#if USE_I2C3
+  i2c_threads_data[3].p = &i2c3;
+  if (pthread_create(&tid, NULL, i2c_thread, (void*)(&i2c_threads_data[3])) != 0) {
+    fprintf(stderr, "i2c_arch_init: Could not create I2C thread.\n");
+    return;
+  }
+  pthread_setname_np(tid, "pprz_i2c3_thread");
+#endif
+
+  i2c_threads_initialized = true;
 }
 
 void i2c_event(void)
@@ -74,89 +123,83 @@ bool i2c_idle(struct i2c_periph *p __attribute__((unused)))
 
 bool i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
 {
-  uint8_t temp;
+  static uint8_t temp;
+  static pthread_mutex_t *i2c_mutex;
+  static pthread_cond_t *condition;
+
+  // find mutex lock and condition for the appropriate thread
+  for (int i = 0; i < N_I2C; i++){
+    if (i2c_threads_data.p == p)
+    {
+      i2c_mutex = &(i2c_threads_data.mutex);
+      condition = &(i2c_threads_data.condition);
+    }
+  }
+
+  pthread_mutex_lock(i2c_mutex);
   temp = (p->trans_insert_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
   if (temp == p->trans_extract_idx) {
     // queue full
-    pthread_mutex_lock(&i2c_mutex);
     p->errors->queue_full_cnt++;
-    pthread_mutex_unlock(&i2c_mutex);
     t->status = I2CTransFailed;
+    pthread_mutex_unlock(i2c_mutex);
     return false;
   }
 
   t->status = I2CTransPending;
 
-  pthread_mutex_lock(&i2c_mutex);
-
   /* put transaction in queue */
   p->trans[p->trans_insert_idx] = t;
   p->trans_insert_idx = temp;
 
-  pthread_mutex_unlock(&i2c_mutex);
+  /* wake handler thread */
+  pthread_cond_signal(&condition);
+  pthread_mutex_unlock(i2c_mutex);
 
   return true;
 }
 
-/**
- * check for new i2c transactions.
- */
-static void *i2c_thread(void *data __attribute__((unused)))
-{
-  get_rt_prio(I2C_THREAD_PRIO);
-
-  while (1)
-  {
-#if USE_I2C0
-    i2c_complete_trans(&i2c0);
-#endif
-
-#if USE_I2C1
-    i2c_complete_trans(&i2c1);
-#endif
-
-#if USE_I2C2
-    i2c_complete_trans(&i2c2);
-#endif
-
-#if USE_I2C3
-    i2c_complete_trans(&i2c3)
-#endif
-    usleep(250);
-  }
-  return 0;
-}
-
 /*
- * Complete requested transactions
+ * Transactions handler thread
  */
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcast-qual"
-void i2c_complete_trans(struct i2c_periph *p)
+static void *i2c_thread(void *thread_data)
 {
-  static struct i2c_msg trx_msgs[2];
-  static struct i2c_rdwr_ioctl_data trx_data = {
+  struct i2c_msg trx_msgs[2];
+  struct i2c_rdwr_ioctl_data trx_data = {
     .msgs = trx_msgs,
     .nmsgs = 2
   };
 
-  if (p->reg_addr == NULL) {return;}
-  if (p->trans_insert_idx == p->trans_extract_idx) {return;}
+  get_rt_prio(I2C_THREAD_PRIO);
 
-  int fd = (int)p->reg_addr;
+  struct i2c_thread_data_t *data = (struct i2c_thread_data_t*)tread_data;
 
-  struct i2c_transaction *t = p->trans[p->trans_extract_idx];
-  // Switch the different transaction types
-  switch (t->type) {
+  while (1)
+  {
+    /* wait for data to transfer */
+    pthread_mutex_lock(&data->mutex);
+    if (data->p->trans_insert_idx == data->p->trans_extract_idx) {
+      pthread_cond_wait(&data->condition, &data->mutex);
+    }
+
+    int fd = (int)data->p->reg_addr;
+
+    struct i2c_transaction *t = data->p->trans[data->p->trans_extract_idx];
+    pthread_mutex_unlock(&data->mutex);
+
+    // Switch the different transaction types
+    switch (t->type) {
       // Just transmitting
     case I2CTransTx:
       // Set the slave address, converted to 7 bit
       ioctl(fd, I2C_SLAVE, t->slave_addr >> 1);
       if (write(fd, (uint8_t *)t->buf, t->len_w) < 0) {
         /* if write failed, increment error counter ack_fail_cnt */
-        pthread_mutex_lock(&i2c_mutex);
-        p->errors->ack_fail_cnt++;
-        pthread_mutex_unlock(&i2c_mutex);
+        pthread_mutex_lock(&data->mutex);
+        data->p->errors->ack_fail_cnt++;
+        pthread_mutex_unlock(&data->mutex);
         t->status = I2CTransFailed;
         return;
       }
@@ -167,9 +210,9 @@ void i2c_complete_trans(struct i2c_periph *p)
       ioctl(fd, I2C_SLAVE, t->slave_addr >> 1);
       if (read(fd, (uint8_t *)t->buf, t->len_r) < 0) {
         /* if read failed, increment error counter arb_lost_cnt */
-        pthread_mutex_lock(&i2c_mutex);
-        p->errors->arb_lost_cnt++;
-        pthread_mutex_unlock(&i2c_mutex);
+        pthread_mutex_lock(&data->mutex);
+        data->p->errors->arb_lost_cnt++;
+        pthread_mutex_unlock(&data->mutex);
         t->status = I2CTransFailed;
         return;
       }
@@ -186,23 +229,26 @@ void i2c_complete_trans(struct i2c_periph *p)
       trx_msgs[1].buf = (void*) t->buf;
       if (ioctl(fd, I2C_RDWR, &trx_data) < 0) {
         /* if write/read failed, increment error counter miss_start_stop_cnt */
-        pthread_mutex_lock(&i2c_mutex);
-        p->errors->miss_start_stop_cnt++;
-        pthread_mutex_unlock(&i2c_mutex);
+        pthread_mutex_lock(&data->mutex);
+        data->p->errors->miss_start_stop_cnt++;
+        pthread_mutex_unlock(&data->mutex);
         t->status = I2CTransFailed;
         return;
       }
       break;
     default:
       break;
-  }
+    }
 
-  // Successful transfer
-  t->status = I2CTransSuccess;
+    // Successful transfer
+    t->status = I2CTransSuccess;
 
-  pthread_mutex_lock(&i2c_mutex);
-  p->trans_extract_idx = (p->trans_extract_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
-  pthread_mutex_unlock(&i2c_mutex);
+    pthread_mutex_lock(&data->mutex);
+    data->p->trans_extract_idx = (data->p->trans_extract_idx + 1) % I2C_TRANSACTION_QUEUE_LEN;
+    pthread_mutex_unlock(&data->mutex);
+  }
+
+  return 0;
 }
 #pragma GCC diagnostic pop
 
@@ -217,10 +263,7 @@ void i2c0_hw_init(void)
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c0_errors);
 
-  if (!thread_initialized)
-  {
-    i2c_arch_init();
-  }
+  i2c_arch_init();
 }
 #endif
 
@@ -235,10 +278,7 @@ void i2c1_hw_init(void)
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c1_errors);
 
-  if (!thread_initialized)
-  {
-    i2c_arch_init();
-  }
+  i2c_arch_init();
 }
 #endif
 
@@ -253,10 +293,7 @@ void i2c2_hw_init(void)
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c2_errors);
 
-  if (!thread_initialized)
-  {
-    i2c_arch_init();
-  }
+  i2c_arch_init();
 }
 #endif
 
@@ -271,9 +308,7 @@ void i2c3_hw_init(void)
   /* zeros error counter */
   ZEROS_ERR_COUNTER(i2c3_errors);
 
-  if (!thread_initialized)
-  {
-    i2c_arch_init();
-  }
+  i2c_arch_init();
 }
 #endif
+