DataFlash: DataFlash_MAVLink: avoid races by taking semaphore

libraries/DataFlash/DataFlash_MAVLink.cpp

@@ -277,11 +277,15 @@ void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
 {
     mavlink_remote_log_block_status_t packet;
     mavlink_msg_remote_log_block_status_decode(msg, &packet);
+    if (!semaphore->take_nonblocking()) {
+        return;
+    }
     if(packet.status == 0){
         handle_retry(packet.seqno);
     } else{
         handle_ack(chan, msg, packet.seqno);
     }
+    semaphore->give();
 }
 
 void DataFlash_MAVLink::handle_retry(uint32_t seqno)
@@ -399,6 +403,9 @@ void DataFlash_MAVLink::stats_collect()
     if (!_initialised || !_logging_started) {
         return;
     }
+    if (!semaphore->take_nonblocking()) {
+        return;
+    }
     uint8_t pending = queue_size(_blocks_pending);
     uint8_t sent = queue_size(_blocks_sent);
     uint8_t retry = queue_size(_blocks_retry);
@@ -407,6 +414,8 @@ void DataFlash_MAVLink::stats_collect()
     if (sfree != _blockcount_free) {
         internal_error();
     }
+    semaphore->give();
+
     stats.state_pending += pending;
     stats.state_sent += sent;
     stats.state_free += sfree;
@@ -500,16 +509,21 @@ void DataFlash_MAVLink::do_resends(uint32_t now)
     }
     uint32_t oldest = now - 100; // 100 milliseconds before resend.  Hmm.
     while (count_to_send-- > 0) {
+        if (!semaphore->take_nonblocking()) {
+            return;
+        }
         for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) {
             // only want to send blocks every now-and-then:
             if (block->last_sent < oldest) {
                 if (! send_log_block(*block)) {
                     // failed to send the block; try again later....
+                    semaphore->give();
                     return;
                 }
                 stats.resends++;
             }
         }
+        semaphore->give();
     }
 }