CAN: locking on shared buffer

ThingSet · Nov 15, 2023 · 3434490 · 3434490
1 parent df1bc8f
commit 3434490
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 3 deletions.
diff --git a/src/can.c b/src/can.c
@@ -454,12 +454,19 @@ void isotp_fast_recv_callback(struct net_buf *buffer, int rem_len, isotp_fast_ca
         }
         else {
             struct shared_buffer *sbuf = thingset_sdk_shared_buffer();
+            k_sem_take(&sbuf->lock, K_FOREVER);
             int tx_len =
                 thingset_process_message(&ts, ts_can->rx_buffer, len, sbuf->data, sbuf->size);
             if (tx_len > 0) {
                 isotp_fast_node_id target_id = (uint8_t)(can_id & 0xFF);
-                thingset_can_send_inst(ts_can, sbuf->data, tx_len, target_id, NULL, NULL,
-                                       K_NO_WAIT);
+                int err = thingset_can_send_inst(ts_can, sbuf->data, tx_len, target_id, NULL, NULL,
+                                                 K_NO_WAIT);
+                if (err != 0) {
+                    k_sem_give(&sbuf->lock);
+                }
+            }
+            else {
+                k_sem_give(&sbuf->lock);
             }
         }
     }
@@ -477,6 +484,10 @@ void isotp_fast_sent_callback(int result, void *arg)
         ts_can->request_response.callback(NULL, 0, result, 0, ts_can->request_response.cb_arg);
         thingset_can_reset_request_response(&ts_can->request_response);
     }
+    else {
+        struct shared_buffer *sbuf = thingset_sdk_shared_buffer();
+        k_sem_give(&sbuf->lock);
+    }
 }
 #else
 int thingset_can_send_inst(struct thingset_can *ts_can, uint8_t *tx_buf, size_t tx_len,

diff --git a/src/isotp_fast.c b/src/isotp_fast.c
@@ -1015,7 +1015,7 @@ int isotp_fast_send(struct isotp_fast_ctx *ctx, const uint8_t *data, size_t len,
         memcpy(&frame.data[index], data, len);
         int ret = can_send(ctx->can_dev, &frame, K_MSEC(ISOTP_A_TIMEOUT_MS), NULL, NULL);
         ctx->sent_callback(ret, cb_arg);
-        return ISOTP_N_OK;
+        return ret;
     }
     else {
         if (len > ISOTP_FAST_MAX_LEN) {