refactor: use shared packet slots

Author: Jiu-xiao

README.md

@@ -13,16 +13,28 @@ 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 `WritePort`：
+
+- 所有 Topic 共用同一组固定 packet 槽位。
+- 空槽位用 `LockFreeQueue<uint32_t>` 管理；TX 消费完成后把槽位还回队列。
+- 待发 packet 用 `LockFreePool` 管理；message callback 打包完成后放入 pool。
+- packet 总数、空槽队列深度、待发 pool 深度相同。
+- 空槽申请失败时丢弃当前新 packet；这是全局背压，不是同 Topic 覆盖。
+
+`TxService()` 每次只尝试交付一个待发 packet，不单独维护发送锁；并发提交与写队列容量
+由 libxr `WritePort` 负责。如果 `WritePort` 暂时忙或写队列已满，当前待发 packet
+会放回待发 pool，等待写完成回调或下一次 Topic callback 再推进。这样不会为转发链路
+额外引入发送线程，也不会重复实现 `WritePort` 已经具备的互斥语义。
 
 ## 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"]
@@ -19,17 +19,28 @@ depends: []
 #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,64 +53,121 @@ 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);
+      : uart_(hw.template FindOrExit<LibXR::UART>({uart_name})) {
     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) {
       auto domain = LibXR::Topic::Domain(config.domain);
-      auto ans = LibXR::Topic::Find(config.name, &domain);
-      if (ans == nullptr) {
+      auto topic = LibXR::Topic::Find(config.name, &domain);
+      if (topic == nullptr) {
         XR_LOG_ERROR("Topic not found: %s/%s", config.domain, config.name);
         ASSERT(false);
       }
       const size_t packet_size =
-          ans->data_.max_length + LibXR::Topic::PACK_BASE_SIZE;
-      ASSERT(packet_size <= buffer_size);
+          topic->data_.max_length + LibXR::Topic::PACK_BASE_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 + 1);
+    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);
+      ASSERT(free_slots_->Push(i) == 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 topic_handle = LibXR::Topic::Find(config.name, &domain);
+      ASSERT(topic_handle != 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, topic_handle->data_.crc32});
 
-      LibXR::Topic topic(ans);
+      LibXR::Topic topic(topic_handle);
 
       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) {
+      return;
+    }
+
+    auto& slot = packets_[slot_index];
+    ASSERT(packet_size <= slot.buffer.size_);
+    LibXR::Topic::PackData(info.topic_crc32, slot.buffer, timestamp, data);
+
+    ASSERT(ready_packets_->Put(ReadyPacket{slot_index, packet_size}) ==
+           LibXR::ErrorCode::OK);
+    TxService(in_isr);
+  }
+
+  void TxService(bool in_isr) {
+    ReadyPacket packet;
+    if (ready_packets_->Get(packet) != LibXR::ErrorCode::OK) {
+      return;
+    }
+
+    auto& slot = packets_[packet.slot_index];
+    auto write_status = uart_->Write(
+        LibXR::ConstRawData{slot.buffer.addr_, packet.packet_size}, tx_op_,
+        in_isr);
+    if (static_cast<int8_t>(write_status) < 0) {
+      ASSERT(ready_packets_->Put(packet) == LibXR::ErrorCode::OK);
+      return;
+    }
+
+    ReturnFreeSlot(packet.slot_index);
+  }
+
+  void OnWriteDone(bool in_isr, LibXR::ErrorCode) {
+    TxService(in_isr);
+  }
+
+  void ReturnFreeSlot(uint32_t slot_index) {
+    ASSERT(free_slots_->Push(slot_index) == 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;
+  LibXR::Callback<LibXR::ErrorCode> tx_callback_;
+  LibXR::WriteOperation tx_op_;
 };