refactor: use shared packet slots

Author: Jiu-xiao

README.md

@@ -13,16 +13,26 @@ SharedTopicClient is a client module for multi-topic data sharing and transparen
 ## 构造参数 / Constructor Arguments
 
 - uart_name: 串口设备名 / UART device name (e.g., "uart_cdc")
-- buffer_size: 单个 Topic 串口包最大字节数 / Maximum bytes of one forwarded Topic packet (e.g., 256)
+- slot_count: 共享待发槽位数量。每个槽位的字节数由订阅 Topic 中最大的打包后长度自动计算。
+  / Number of shared pending slots. Each slot size is derived from the largest packed subscribed Topic.
 - topic_configs: 需要订阅并转发的 Topic 配置列表。每项可以只写 topic 名，也可以写
   `[topic, domain]`。/ Topic configs to subscribe and forward. Each item may be a
   topic name or `[topic, domain]`.
 
 ## 运行方式
 
 `SharedTopicClient` 不创建发送线程。模块注册 Topic callback；每次 Topic 发布时，
-callback 内直接完成打包并把当前包写入 UART `write_port`。UART 写入仍走 libxr
-非阻塞写队列，包内容在 `Write()` 返回前已经复制到 UART 队列。
+callback 先从空槽位队列申请一个 packet 槽，完成打包后把 `{槽位, 长度}` 放入待发
+pool，然后尝试抢占当前 UART 发送服务：
+
+- 所有 Topic 共用同一组固定 packet 槽位。
+- 空槽位用 `LockFreeQueue<uint32_t>` 管理；TX 消费完成后把槽位还回队列。
+- 待发 packet 用 `LockFreePool` 管理；message callback 打包完成后放入 pool。
+- packet 总数、空槽队列深度、待发 pool 深度相同。
+- 空槽申请失败时丢弃当前新 packet，并记录丢包计数；这是全局背压，不是同 Topic 覆盖。
+
+写完成回调会继续调用 `TxService()`，直到 UART 写队列再次满或共享槽位池为空。
+这样不会让多个 Topic callback 同时推进 UART 服务，也不会为转发链路额外引入发送线程。
 
 ## Timestamp
 

SharedTopicClient.hpp

@@ -5,7 +5,7 @@
 module_description: SharedTopicClient 是一个多 Topic 数据共享与串口转发客户端模块。它用于通过 UART 将多个 Topic 的数据统一打包、发送，实现消息流的串口透明同步转发，适用于分布式系统的多主题数据同步或边缘数据采集。 / SharedTopicClient is a client module for multi-topic data sharing and transparent UART forwarding. It subscribes to multiple Topics, packs their updates, and transmits them via UART, enabling efficient and reliable message synchronization over serial connections—ideal for distributed systems or edge data acquisition.
 constructor_args:
   - uart_name: "uart_cdc"
-  - buffer_size: 256
+  - slot_count: 16
   - topic_configs:
     - "topic1"
     - ["topic2", "libxr_def_domain"]
@@ -15,21 +15,33 @@ depends: []
 === END MANIFEST === */
 // clang-format on
 
+#include <atomic>
 #include <cstddef>
 #include <cstdint>
 
 #include "app_framework.hpp"
+#include "lockfree_pool.hpp"
+#include "lockfree_queue.hpp"
 #include "message.hpp"
 #include "uart.hpp"
 
 class SharedTopicClient : public LibXR::Application {
- public:
+ private:
   struct CallbackInfo {
     SharedTopicClient* client;
     uint32_t topic_crc32;
-    uint32_t index;
   };
 
+  struct PacketSlot {
+    LibXR::RawData buffer;
+  };
+
+  struct ReadyPacket {
+    uint32_t slot_index = 0;
+    size_t packet_size = 0;
+  };
+
+ public:
   struct TopicConfig {
     const char* name;
     const char* domain = "libxr_def_domain";
@@ -42,15 +54,16 @@ class SharedTopicClient : public LibXR::Application {
 
   SharedTopicClient(LibXR::HardwareContainer& hw,
                     LibXR::ApplicationManager& app, const char* uart_name,
-                    uint32_t buffer_size,
+                    uint32_t slot_count,
                     std::initializer_list<TopicConfig> topic_configs)
       : uart_(hw.template Find<LibXR::UART>(uart_name)) {
     ASSERT(uart_ != nullptr);
     ASSERT(uart_->write_port_ != nullptr);
     ASSERT(uart_->write_port_->queue_data_ != nullptr);
+    ASSERT(uart_->write_port_->Writable());
+    ASSERT(topic_configs.size() > 0);
+    ASSERT(slot_count > 0);
 
-    topics_pack_buffer_ = new LibXR::RawData[topic_configs.size()];
-    uint32_t i = 0;
     size_t max_packet_size = 0;
 
     for (auto config : topic_configs) {
@@ -62,44 +75,146 @@ class SharedTopicClient : public LibXR::Application {
       }
       const size_t packet_size =
           ans->data_.max_length + LibXR::Topic::PACK_BASE_SIZE;
-      ASSERT(packet_size <= buffer_size);
       max_packet_size = LibXR::max(max_packet_size, packet_size);
-      topics_pack_buffer_[i] = LibXR::RawData(
-          new uint8_t[packet_size], packet_size);
+    }
+
+    ASSERT(max_packet_size <= uart_->write_port_->queue_data_->MaxSize());
 
+    packets_ = new PacketSlot[slot_count];
+    free_slots_ = new LibXR::LockFreeQueue<uint32_t>(slot_count);
+    ready_packets_ = new LibXR::LockFreePool<ReadyPacket>(slot_count);
+    for (uint32_t i = 0; i < slot_count; i++) {
+      packets_[i].buffer =
+          LibXR::RawData(new uint8_t[max_packet_size], max_packet_size);
+      auto ans = free_slots_->Push(i);
+      UNUSED(ans);
+      ASSERT(ans == LibXR::ErrorCode::OK);
+    }
+
+    tx_callback_ = LibXR::Callback<LibXR::ErrorCode>::CreateGuarded(
+        [](bool in_isr, SharedTopicClient* self, LibXR::ErrorCode status) {
+          self->OnWriteDone(in_isr, status);
+        },
+        this);
+    tx_op_ = LibXR::WriteOperation(tx_callback_);
+
+    for (auto config : topic_configs) {
+      auto domain = LibXR::Topic::Domain(config.domain);
+      auto ans = LibXR::Topic::Find(config.name, &domain);
+      ASSERT(ans != nullptr);
       void (*func)(bool, CallbackInfo, LibXR::MicrosecondTimestamp,
                    LibXR::ConstRawData&) =
           [](bool in_isr, CallbackInfo info,
              LibXR::MicrosecondTimestamp timestamp, LibXR::ConstRawData& data) {
-            auto& buffer = info.client->topics_pack_buffer_[info.index];
-            ASSERT(data.size_ + LibXR::Topic::PACK_BASE_SIZE <= buffer.size_);
-            LibXR::Topic::PackData(info.topic_crc32, buffer, timestamp, data);
-
-            LibXR::WriteOperation op;
-            auto ans = info.client->uart_->Write(
-                {buffer.addr_, data.size_ + LibXR::Topic::PACK_BASE_SIZE}, op,
-                in_isr);
-            UNUSED(ans);
+            info.client->OnTopic(in_isr, info, timestamp, data);
           };
 
       auto msg_cb = LibXR::Topic::Callback::Create(
-          func, CallbackInfo{this, ans->data_.crc32, i});
+          func, CallbackInfo{this, ans->data_.crc32});
 
       LibXR::Topic topic(ans);
 
       topic.RegisterCallback(msg_cb);
-
-      i++;
     }
 
-    ASSERT(max_packet_size <= uart_->write_port_->queue_data_->MaxSize());
-
     app.Register(*this);
   }
 
   void OnMonitor() override {}
 
  private:
+  void OnTopic(bool in_isr, CallbackInfo info,
+               LibXR::MicrosecondTimestamp timestamp,
+               LibXR::ConstRawData& data) {
+    const size_t packet_size = data.size_ + LibXR::Topic::PACK_BASE_SIZE;
+    uint32_t slot_index = 0;
+
+    if (free_slots_->Pop(slot_index) != LibXR::ErrorCode::OK) {
+      dropped_packets_.fetch_add(1U, std::memory_order_relaxed);
+      return;
+    }
+
+    auto& slot = packets_[slot_index];
+    ASSERT(packet_size <= slot.buffer.size_);
+    LibXR::Topic::PackData(info.topic_crc32, slot.buffer, timestamp, data);
+
+    auto ans = ready_packets_->Put(ReadyPacket{slot_index, packet_size});
+    UNUSED(ans);
+    ASSERT(ans == LibXR::ErrorCode::OK);
+    TxService(in_isr);
+  }
+
+  void TxService(bool in_isr) {
+    tx_pend_.store(1U, std::memory_order_release);
+
+    uint32_t expected = 0U;
+    if (!tx_lock_.compare_exchange_strong(expected, 1U,
+                                          std::memory_order_acquire,
+                                          std::memory_order_relaxed)) {
+      return;
+    }
+
+    while (true) {
+      tx_pend_.store(0U, std::memory_order_release);
+      bool blocked = false;
+
+      while (true) {
+        ReadyPacket packet = retry_packet_;
+        if (!retry_pending_) {
+          if (ready_packets_->Get(packet) != LibXR::ErrorCode::OK) {
+            break;
+          }
+        }
+
+        auto& slot = packets_[packet.slot_index];
+        auto ans = uart_->Write(
+            LibXR::ConstRawData{slot.buffer.addr_, packet.packet_size}, tx_op_,
+            in_isr);
+        if (static_cast<int8_t>(ans) < 0) {
+          retry_packet_ = packet;
+          retry_pending_ = true;
+          blocked = true;
+          break;
+        }
+
+        retry_pending_ = false;
+        ReturnFreeSlot(packet.slot_index);
+      }
+
+      tx_lock_.store(0U, std::memory_order_release);
+      if (blocked || tx_pend_.load(std::memory_order_acquire) == 0U) {
+        return;
+      }
+
+      expected = 0U;
+      if (!tx_lock_.compare_exchange_strong(expected, 1U,
+                                            std::memory_order_acquire,
+                                            std::memory_order_relaxed)) {
+        return;
+      }
+    }
+  }
+
+  void OnWriteDone(bool in_isr, LibXR::ErrorCode status) {
+    UNUSED(status);
+    TxService(in_isr);
+  }
+
+  void ReturnFreeSlot(uint32_t slot_index) {
+    auto ans = free_slots_->Push(slot_index);
+    UNUSED(ans);
+    ASSERT(ans == LibXR::ErrorCode::OK);
+  }
+
   LibXR::UART* uart_;
-  LibXR::RawData* topics_pack_buffer_;
+  PacketSlot* packets_ = nullptr;
+  LibXR::LockFreeQueue<uint32_t>* free_slots_ = nullptr;
+  LibXR::LockFreePool<ReadyPacket>* ready_packets_ = nullptr;
+  ReadyPacket retry_packet_;
+  bool retry_pending_ = false;
+  std::atomic<uint32_t> tx_lock_{0};
+  std::atomic<uint32_t> tx_pend_{0};
+  std::atomic<uint32_t> dropped_packets_{0};
+  LibXR::Callback<LibXR::ErrorCode> tx_callback_;
+  LibXR::WriteOperation tx_op_;
 };