libxr  1.0
Want to be the best embedded framework
Loading...
Searching...
No Matches
linux_shared_topic_impl.hpp
1#pragma once
2
3#if defined(LIBXR_SYSTEM_POSIX_HOST)
4
5#include <fcntl.h>
6#include <linux/futex.h>
7#include <signal.h>
8#include <sys/mman.h>
9#include <sys/stat.h>
10#include <sys/syscall.h>
11#include <time.h>
12#include <unistd.h>
13
14#include <atomic>
15#include <cerrno>
16#include <chrono>
17#include <cinttypes>
18#include <cstddef>
19#include <cstdint>
20#include <cstdio>
21#include <cstring>
22#include <fstream>
23#include <memory>
24#include <sstream>
25#include <string>
26#include <type_traits>
27
28#include "crc.hpp"
29#include "libxr_def.hpp"
30#include "message.hpp"
31
32namespace LibXR
33{
38enum class LinuxSharedSubscriberMode : uint8_t
39{
40 BROADCAST_FULL = 0,
41 BROADCAST_DROP_OLD =
42 1,
43 BALANCE_RR = 2,
44};
45
50struct LinuxSharedTopicConfig
51{
52 uint32_t slot_num = 64;
53 uint32_t subscriber_num = 8;
54 uint32_t queue_num =
55 64;
56};
57
73template <typename TopicData>
74class LinuxSharedTopic : public Topic
75{
76 static_assert(std::is_trivially_copyable<TopicData>::value,
77 "LinuxSharedTopic requires trivially copyable data");
78 static_assert(std::atomic<uint32_t>::is_always_lock_free,
79 "LinuxSharedTopic requires lock-free 32-bit atomics");
80 static_assert(std::atomic<uint64_t>::is_always_lock_free,
81 "LinuxSharedTopic requires lock-free 64-bit atomics");
82
83 enum class SharedDataState : uint8_t
84 {
85 EMPTY = 0,
86 PUBLISHER = 1,
87 SUBSCRIBER = 2,
88 };
89
90 public:
91 class SharedData;
92 using Data = SharedData;
93 static constexpr const char* DEFAULT_DOMAIN_NAME = "libxr_def_domain";
94
101 class Subscriber
102 {
103 public:
104 using Data = SharedData;
105
110 Subscriber() = default;
111
120 explicit Subscriber(const char* name, LinuxSharedSubscriberMode mode =
121 LinuxSharedSubscriberMode::BROADCAST_FULL)
122 : owned_topic_(new LinuxSharedTopic(name))
123 {
124 if (Attach(*owned_topic_, mode) != ErrorCode::OK)
125 {
126 delete owned_topic_;
127 owned_topic_ = nullptr;
128 }
129 }
130
131 Subscriber(const char* name, const char* domain_name,
132 LinuxSharedSubscriberMode mode = LinuxSharedSubscriberMode::BROADCAST_FULL)
133 : owned_topic_(new LinuxSharedTopic(name, domain_name))
134 {
135 if (Attach(*owned_topic_, mode) != ErrorCode::OK)
136 {
137 delete owned_topic_;
138 owned_topic_ = nullptr;
139 }
140 }
141
142 Subscriber(const char* name, Topic::Domain& domain,
143 LinuxSharedSubscriberMode mode = LinuxSharedSubscriberMode::BROADCAST_FULL)
144 : owned_topic_(new LinuxSharedTopic(name, domain))
145 {
146 if (Attach(*owned_topic_, mode) != ErrorCode::OK)
147 {
148 delete owned_topic_;
149 owned_topic_ = nullptr;
150 }
151 }
152
159 explicit Subscriber(
160 LinuxSharedTopic& topic,
161 LinuxSharedSubscriberMode mode = LinuxSharedSubscriberMode::BROADCAST_FULL)
162 {
163 (void)Attach(topic, mode);
164 }
165
169 ~Subscriber() { Reset(); }
170
171 Subscriber(const Subscriber&) = delete;
172 Subscriber& operator=(const Subscriber&) = delete;
173
174 Subscriber(Subscriber&& other) noexcept { *this = std::move(other); }
175
176 Subscriber& operator=(Subscriber&& other) noexcept
177 {
178 if (this == &other)
179 {
180 return *this;
181 }
182
183 Reset();
184
185 topic_ = other.topic_;
186 owned_topic_ = other.owned_topic_;
187 subscriber_index_ = other.subscriber_index_;
188 current_slot_index_ = other.current_slot_index_;
189 current_sequence_ = other.current_sequence_;
190 current_timestamp_ = other.current_timestamp_;
191
192 other.topic_ = nullptr;
193 other.owned_topic_ = nullptr;
194 other.subscriber_index_ = INVALID_INDEX;
195 other.current_slot_index_ = INVALID_INDEX;
196 other.current_sequence_ = 0;
197 other.current_timestamp_ = MicrosecondTimestamp();
198 return *this;
199 }
200
206 bool Valid() const { return topic_ != nullptr && subscriber_index_ != INVALID_INDEX; }
207
215 ErrorCode Wait(uint32_t timeout_ms = UINT32_MAX)
216 {
217 if (!Valid())
218 {
219 return ErrorCode::STATE_ERR;
220 }
221
222 const uint64_t deadline_ms =
223 (timeout_ms == UINT32_MAX) ? 0 : (NowMonotonicMs() + timeout_ms);
224
225 Descriptor desc = {};
226 while (true)
227 {
228 ErrorCode pop_ans = topic_->TryPopDescriptor(subscriber_index_, desc);
229 if (pop_ans == ErrorCode::OK)
230 {
231 Release();
232 topic_->HoldSlot(subscriber_index_, desc.slot_index);
233 current_slot_index_ = desc.slot_index;
234 current_sequence_ = desc.sequence;
235 current_timestamp_ = topic_->SlotTimestamp(desc.slot_index);
236 return ErrorCode::OK;
237 }
238
239 uint32_t wait_ms = UINT32_MAX;
240 if (timeout_ms != UINT32_MAX)
241 {
242 const uint64_t now_ms = NowMonotonicMs();
243 if (now_ms >= deadline_ms)
244 {
245 return ErrorCode::TIMEOUT;
246 }
247 wait_ms = static_cast<uint32_t>(deadline_ms - now_ms);
248 }
249
250 const ErrorCode wait_ans =
251 topic_->WaitReady(topic_->subscribers_[subscriber_index_], wait_ms);
252 if (wait_ans == ErrorCode::OK)
253 {
254 continue;
255 }
256 return wait_ans;
257 }
258 }
259
268 ErrorCode Wait(SharedData& data, uint32_t timeout_ms = UINT32_MAX)
269 {
270 data.Reset();
271
272 if (!Valid())
273 {
274 return ErrorCode::STATE_ERR;
275 }
276
277 const uint64_t deadline_ms =
278 (timeout_ms == UINT32_MAX) ? 0 : (NowMonotonicMs() + timeout_ms);
279
280 Descriptor desc = {};
281 while (true)
282 {
283 ErrorCode pop_ans = topic_->TryPopDescriptor(subscriber_index_, desc);
284 if (pop_ans == ErrorCode::OK)
285 {
286 data.topic_ = topic_;
287 data.slot_index_ = desc.slot_index;
288 data.sequence_ = desc.sequence;
289 data.state_ = SharedDataState::SUBSCRIBER;
290 data.subscriber_index_ = subscriber_index_;
291 topic_->HoldSlot(subscriber_index_, desc.slot_index);
292 return ErrorCode::OK;
293 }
294
295 uint32_t wait_ms = UINT32_MAX;
296 if (timeout_ms != UINT32_MAX)
297 {
298 const uint64_t now_ms = NowMonotonicMs();
299 if (now_ms >= deadline_ms)
300 {
301 return ErrorCode::TIMEOUT;
302 }
303 wait_ms = static_cast<uint32_t>(deadline_ms - now_ms);
304 }
305
306 const ErrorCode wait_ans =
307 topic_->WaitReady(topic_->subscribers_[subscriber_index_], wait_ms);
308 if (wait_ans == ErrorCode::OK)
309 {
310 continue;
311 }
312 return wait_ans;
313 }
314 }
315
321 TopicData* GetData() const
322 {
323 if (!Valid() || current_slot_index_ == INVALID_INDEX)
324 {
325 return nullptr;
326 }
327
328 return &topic_->payloads_[current_slot_index_];
329 }
330
334 uint64_t GetSequence() const { return current_sequence_; }
335
339 MicrosecondTimestamp GetTimestamp() const { return current_timestamp_; }
340
344 uint32_t GetPendingNum() const
345 {
346 if (!Valid())
347 {
348 return 0;
349 }
350
351 const SubscriberControl& control = topic_->subscribers_[subscriber_index_];
352 const uint32_t head = control.queue_head.load(std::memory_order_acquire);
353 const uint32_t tail = control.queue_tail.load(std::memory_order_acquire);
354 if (tail >= head)
355 {
356 return tail - head;
357 }
358 return topic_->queue_capacity_ - (head - tail);
359 }
360
365 uint64_t GetDropNum() const
366 {
367 if (!Valid())
368 {
369 return 0;
370 }
371
372 return topic_->subscribers_[subscriber_index_].dropped_messages.load(
373 std::memory_order_acquire);
374 }
375
379 void Release()
380 {
381 if (!Valid() || current_slot_index_ == INVALID_INDEX)
382 {
383 return;
384 }
385
386 topic_->ClearHeldSlot(subscriber_index_, current_slot_index_);
387 topic_->ReleaseSlot(current_slot_index_);
388 current_slot_index_ = INVALID_INDEX;
389 current_sequence_ = 0;
390 current_timestamp_ = MicrosecondTimestamp();
391 }
392
397 void Reset()
398 {
399 if (!Valid())
400 {
401 return;
402 }
403
404 topic_->UnregisterBalancedSubscriber(subscriber_index_);
405 topic_->subscribers_[subscriber_index_].active.store(0, std::memory_order_release);
406 topic_->subscribers_[subscriber_index_].owner_pid.store(0,
407 std::memory_order_release);
408 topic_->subscribers_[subscriber_index_].owner_starttime.store(
409 0, std::memory_order_release);
410
411 Descriptor desc = {};
412 while (topic_->TryPopDescriptor(subscriber_index_, desc) == ErrorCode::OK)
413 {
414 topic_->ReleaseSlot(desc.slot_index);
415 }
416
417 Release();
418
419 topic_ = nullptr;
420 delete owned_topic_;
421 owned_topic_ = nullptr;
422 subscriber_index_ = INVALID_INDEX;
423 current_slot_index_ = INVALID_INDEX;
424 current_sequence_ = 0;
425 current_timestamp_ = MicrosecondTimestamp();
426 }
427
428 private:
429 ErrorCode Attach(LinuxSharedTopic& topic, LinuxSharedSubscriberMode mode)
430 {
431 Reset();
432
433 if (!topic.Valid())
434 {
435 return ErrorCode::STATE_ERR;
436 }
437
438 if (topic.self_identity_.starttime == 0)
439 {
440 return ErrorCode::STATE_ERR;
441 }
442
443 for (uint32_t i = 0; i < topic.subscriber_capacity_; ++i)
444 {
445 uint32_t expected = 0;
446 auto& active = topic.subscribers_[i].active;
447 if (active.compare_exchange_strong(expected, 1, std::memory_order_acq_rel,
448 std::memory_order_relaxed))
449 {
450 topic.subscribers_[i].queue_head.store(0, std::memory_order_release);
451 topic.subscribers_[i].queue_tail.store(0, std::memory_order_release);
452 topic.subscribers_[i].ready_sem_count.store(0, std::memory_order_release);
453 topic.subscribers_[i].dropped_messages.store(0, std::memory_order_release);
454 topic.subscribers_[i].owner_pid.store(topic.self_identity_.pid,
455 std::memory_order_release);
456 topic.subscribers_[i].owner_starttime.store(topic.self_identity_.starttime,
457 std::memory_order_release);
458 topic.subscribers_[i].held_slot.store(INVALID_INDEX, std::memory_order_release);
459 topic.subscribers_[i].mode.store(static_cast<uint32_t>(mode),
460 std::memory_order_release);
461 if (mode == LinuxSharedSubscriberMode::BALANCE_RR)
462 {
463 const ErrorCode join_ans = topic.RegisterBalancedSubscriber(i);
464 if (join_ans != ErrorCode::OK)
465 {
466 topic.subscribers_[i].active.store(0, std::memory_order_release);
467 topic.subscribers_[i].owner_pid.store(0, std::memory_order_release);
468 topic.subscribers_[i].owner_starttime.store(0, std::memory_order_release);
469 topic.subscribers_[i].mode.store(
470 static_cast<uint32_t>(LinuxSharedSubscriberMode::BROADCAST_FULL),
471 std::memory_order_release);
472 return join_ans;
473 }
474 }
475 topic_ = &topic;
476 subscriber_index_ = i;
477 current_slot_index_ = INVALID_INDEX;
478 current_sequence_ = 0;
479 current_timestamp_ = MicrosecondTimestamp();
480 return ErrorCode::OK;
481 }
482 }
483
484 return ErrorCode::FULL;
485 }
486
487 LinuxSharedTopic* topic_ = nullptr;
488 LinuxSharedTopic* owned_topic_ = nullptr;
489 uint32_t subscriber_index_ = INVALID_INDEX;
490 uint32_t current_slot_index_ = INVALID_INDEX;
491 uint64_t current_sequence_ = 0;
492 MicrosecondTimestamp current_timestamp_;
493 };
494
504 class SharedData
505 {
506 public:
510 SharedData() = default;
511
516 ~SharedData() { Reset(); }
517
518 SharedData(const SharedData&) = delete;
519 SharedData& operator=(const SharedData&) = delete;
520
521 SharedData(SharedData&& other) noexcept { *this = std::move(other); }
522
526 SharedData& operator=(SharedData&& other) noexcept
527 {
528 if (this == &other)
529 {
530 return *this;
531 }
532
533 Reset();
534
535 topic_ = other.topic_;
536 slot_index_ = other.slot_index_;
537 sequence_ = other.sequence_;
538 state_ = other.state_;
539 subscriber_index_ = other.subscriber_index_;
540
541 other.topic_ = nullptr;
542 other.slot_index_ = INVALID_INDEX;
543 other.sequence_ = 0;
544 other.state_ = SharedDataState::EMPTY;
545 other.subscriber_index_ = INVALID_INDEX;
546 return *this;
547 }
548
552 bool Valid() const { return topic_ != nullptr && slot_index_ != INVALID_INDEX; }
553
557 bool Empty() const { return !Valid(); }
558
562 uint64_t GetSequence() const { return sequence_; }
563
567 MicrosecondTimestamp GetTimestamp() const
568 {
569 if (!Valid() || state_ != SharedDataState::SUBSCRIBER)
570 {
571 return MicrosecondTimestamp();
572 }
573 return topic_->SlotTimestamp(slot_index_);
574 }
575
581 TopicData* GetData()
582 {
583 if (!Valid())
584 {
585 return nullptr;
586 }
587 return &topic_->payloads_[slot_index_];
588 }
589
595 TopicData* GetData() const
596 {
597 if (!Valid())
598 {
599 return nullptr;
600 }
601 return &topic_->payloads_[slot_index_];
602 }
603
607 void Reset()
608 {
609 if (!Valid())
610 {
611 return;
612 }
613
614 if (state_ == SharedDataState::PUBLISHER)
615 {
616 topic_->RecycleSlot(slot_index_);
617 }
618 else if (state_ == SharedDataState::SUBSCRIBER)
619 {
620 topic_->ClearHeldSlot(subscriber_index_, slot_index_);
621 topic_->ReleaseSlot(slot_index_);
622 }
623 topic_ = nullptr;
624 slot_index_ = INVALID_INDEX;
625 sequence_ = 0;
626 state_ = SharedDataState::EMPTY;
627 subscriber_index_ = INVALID_INDEX;
628 }
629
630 private:
631 friend class LinuxSharedTopic<TopicData>;
632 friend class Subscriber;
633
634 LinuxSharedTopic* topic_ = nullptr;
635 uint32_t slot_index_ = INVALID_INDEX;
636 uint64_t sequence_ = 0;
637 SharedDataState state_ = SharedDataState::EMPTY;
638 uint32_t subscriber_index_ = INVALID_INDEX;
639 };
640
645 explicit LinuxSharedTopic(const char* topic_name)
646 : LinuxSharedTopic(topic_name, DEFAULT_DOMAIN_NAME)
647 {
648 }
649
650 LinuxSharedTopic(const char* topic_name, const char* domain_name)
651 : create_(false),
652 publisher_(false),
653 config_(),
654 domain_crc32_(ResolveDomainKey(domain_name)),
655 topic_name_(ResolveTopicName(topic_name)),
656 name_key_(BuildNameKey(domain_crc32_, topic_name_)),
657 shm_name_(BuildShmName(name_key_))
658 {
659 (void)ReadProcessIdentity(static_cast<uint32_t>(getpid()), self_identity_);
660 Open();
661 }
662
663 LinuxSharedTopic(const char* topic_name, Topic::Domain& domain)
664 : create_(false),
665 publisher_(false),
666 config_(),
667 domain_crc32_(domain.node_ != nullptr ? domain.node_->key : 0),
668 topic_name_(ResolveTopicName(topic_name)),
669 name_key_(BuildNameKey(domain_crc32_, topic_name_)),
670 shm_name_(BuildShmName(name_key_))
671 {
672 (void)ReadProcessIdentity(static_cast<uint32_t>(getpid()), self_identity_);
673 Open();
674 }
675
682 LinuxSharedTopic(const char* topic_name, const LinuxSharedTopicConfig& config)
683 : LinuxSharedTopic(topic_name, DEFAULT_DOMAIN_NAME, config)
684 {
685 }
686
687 LinuxSharedTopic(const char* topic_name, const char* domain_name,
688 const LinuxSharedTopicConfig& config)
689 : create_(true),
690 publisher_(true),
691 config_(config),
692 domain_crc32_(ResolveDomainKey(domain_name)),
693 topic_name_(ResolveTopicName(topic_name)),
694 name_key_(BuildNameKey(domain_crc32_, topic_name_)),
695 shm_name_(BuildShmName(name_key_))
696 {
697 (void)ReadProcessIdentity(static_cast<uint32_t>(getpid()), self_identity_);
698 Open();
699 }
700
701 LinuxSharedTopic(const char* topic_name, Topic::Domain& domain,
702 const LinuxSharedTopicConfig& config)
703 : create_(true),
704 publisher_(true),
705 config_(config),
706 domain_crc32_(domain.node_ != nullptr ? domain.node_->key : 0),
707 topic_name_(ResolveTopicName(topic_name)),
708 name_key_(BuildNameKey(domain_crc32_, topic_name_)),
709 shm_name_(BuildShmName(name_key_))
710 {
711 (void)ReadProcessIdentity(static_cast<uint32_t>(getpid()), self_identity_);
712 Open();
713 }
714
719 using SyncSubscriber = Subscriber;
720
724 ~LinuxSharedTopic() { Close(); }
725
726 LinuxSharedTopic(const LinuxSharedTopic&) = delete;
727 LinuxSharedTopic& operator=(const LinuxSharedTopic&) = delete;
728
729 LinuxSharedTopic(LinuxSharedTopic&&) = delete;
730 LinuxSharedTopic& operator=(LinuxSharedTopic&&) = delete;
731
736 bool Valid() const { return open_ok_; }
737
741 ErrorCode GetError() const { return open_status_; }
742
746 uint32_t GetSubscriberNum() const
747 {
748 if (!Valid())
749 {
750 return 0;
751 }
752
753 uint32_t count = 0;
754 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
755 {
756 if (subscribers_[i].active.load(std::memory_order_acquire) != 0)
757 {
758 ++count;
759 }
760 }
761 return count;
762 }
763
770 ErrorCode CreateData(SharedData& data)
771 {
772 if (!Valid())
773 {
774 return ErrorCode::STATE_ERR;
775 }
776
777 if (!PublisherValid())
778 {
779 return ErrorCode::STATE_ERR;
780 }
781
782 data.Reset();
783
784 uint32_t slot_index = INVALID_INDEX;
785 ErrorCode pop_ans = PopFreeSlot(slot_index);
786 if (pop_ans != ErrorCode::OK)
787 {
788 ScavengeDeadSubscribers();
789 pop_ans = PopFreeSlot(slot_index);
790 if (pop_ans != ErrorCode::OK)
791 {
792 return pop_ans;
793 }
794 }
795
796 slots_[slot_index].refcount.store(0, std::memory_order_release);
797 slots_[slot_index].sequence.store(0, std::memory_order_release);
798 slots_[slot_index].timestamp_us = 0;
799
800 data.topic_ = this;
801 data.slot_index_ = slot_index;
802 data.sequence_ = 0;
803 data.state_ = SharedDataState::PUBLISHER;
804 data.subscriber_index_ = INVALID_INDEX;
805 return ErrorCode::OK;
806 }
807
813 ErrorCode Publish(const TopicData& data)
814 {
815 SharedData topic_data;
816 const ErrorCode acquire_ans = CreateData(topic_data);
817 if (acquire_ans != ErrorCode::OK)
818 {
819 return acquire_ans;
820 }
821
822 *topic_data.GetData() = data;
823 return Publish(topic_data);
824 }
825
826 ErrorCode Publish(const TopicData& data, MicrosecondTimestamp timestamp)
827 {
828 SharedData topic_data;
829 const ErrorCode acquire_ans = CreateData(topic_data);
830 if (acquire_ans != ErrorCode::OK)
831 {
832 return acquire_ans;
833 }
834
835 *topic_data.GetData() = data;
836 return Publish(topic_data, timestamp);
837 }
838
842 ErrorCode Publish(SharedData&& data) { return PublishData<false>(data); }
843
844 ErrorCode Publish(SharedData&& data, MicrosecondTimestamp timestamp)
845 {
846 return PublishData<true>(data, timestamp);
847 }
848
852 ErrorCode Publish(SharedData& data) { return PublishData<false>(data); }
853
854 ErrorCode Publish(SharedData& data, MicrosecondTimestamp timestamp)
855 {
856 return PublishData<true>(data, timestamp);
857 }
858
862 uint64_t GetPublishFailedNum() const
863 {
864 if (!Valid())
865 {
866 return 0;
867 }
868 return header_->publish_failures.load(std::memory_order_acquire);
869 }
870
876 static ErrorCode Remove(const char* topic_name)
877 {
878 return Remove(topic_name, DEFAULT_DOMAIN_NAME);
879 }
880
881 static ErrorCode Remove(const char* topic_name, const char* domain_name)
882 {
883 const std::string shm_name = BuildShmName(
884 BuildNameKey(ResolveDomainKey(domain_name), ResolveTopicName(topic_name)));
885 if (shm_unlink(shm_name.c_str()) == 0 || errno == ENOENT)
886 {
887 return ErrorCode::OK;
888 }
889 return ErrorCode::FAILED;
890 }
891
892 static ErrorCode Remove(const char* topic_name, Topic::Domain& domain)
893 {
894 const uint32_t domain_crc32 = (domain.node_ != nullptr) ? domain.node_->key : 0;
895 const std::string shm_name =
896 BuildShmName(BuildNameKey(domain_crc32, ResolveTopicName(topic_name)));
897 if (shm_unlink(shm_name.c_str()) == 0 || errno == ENOENT)
898 {
899 return ErrorCode::OK;
900 }
901 return ErrorCode::FAILED;
902 }
903
904 private:
905 struct alignas(LibXR::CONCURRENCY_ALIGNMENT) SharedHeader
906 {
907 uint64_t magic = 0;
908 uint64_t name_key = 0;
909 uint32_t domain_crc32 = 0;
910 uint32_t version = 0;
911 uint32_t data_size = 0;
912 uint32_t slot_count = 0;
913 uint32_t subscriber_capacity = 0;
914 uint32_t queue_capacity = 0;
915 uint32_t topic_name_len = 0;
916 std::atomic<uint32_t> init_state;
917 std::atomic<uint32_t> publisher_pid;
918 std::atomic<uint64_t> publisher_starttime;
919 std::atomic<uint64_t> free_queue_head;
920 std::atomic<uint64_t> free_queue_tail;
921 std::atomic<uint64_t> next_sequence;
922 std::atomic<uint64_t> publish_failures;
923 };
924
925 struct alignas(LibXR::CONCURRENCY_ALIGNMENT) SlotControl
926 {
927 std::atomic<uint32_t> refcount;
928 std::atomic<uint64_t> sequence;
929 uint64_t timestamp_us;
930 };
931
932 struct alignas(16) FreeSlotCell
933 {
934 std::atomic<uint64_t> sequence;
935 uint32_t slot_index = 0;
936 uint32_t reserved = 0;
937 };
938
939 struct Descriptor
940 {
941 uint32_t slot_index = INVALID_INDEX;
942 uint32_t reserved = 0;
943 uint64_t sequence = 0;
944 };
945
946 struct alignas(LibXR::CONCURRENCY_ALIGNMENT) SubscriberControl
947 {
948 std::atomic<uint32_t> active;
949 std::atomic<uint32_t> mode;
950 std::atomic<uint32_t> queue_head;
951 std::atomic<uint32_t> queue_tail;
952 std::atomic<uint32_t> ready_sem_count;
953 std::atomic<uint64_t> dropped_messages;
954 std::atomic<uint32_t> owner_pid;
955 std::atomic<uint64_t> owner_starttime;
956 std::atomic<uint32_t> held_slot;
957 };
958
959 struct alignas(LibXR::CONCURRENCY_ALIGNMENT) BalancedGroupControl
960 {
961 std::atomic<uint64_t> rr_cursor;
962 };
963
964 struct ProcessIdentity
965 {
966 uint32_t pid = 0;
967 uint64_t starttime = 0;
968 };
969
970 static constexpr uint64_t MAGIC = 0x4c58524950435348ULL;
971 static constexpr uint32_t VERSION = 2;
972 static constexpr uint32_t INIT_READY = 1;
973 static constexpr uint32_t INVALID_INDEX = UINT32_MAX;
974
975 static uint32_t ResolveDomainKey(const char* domain_name)
976 {
977 const std::string resolved = (domain_name == nullptr || domain_name[0] == '\0')
978 ? std::string(DEFAULT_DOMAIN_NAME)
979 : std::string(domain_name);
980 return CRC32::Calculate(resolved.data(), resolved.size());
981 }
982
983 static std::string ResolveTopicName(const char* topic_name)
984 {
985 return (topic_name != nullptr) ? std::string(topic_name) : std::string();
986 }
987
988 static uint64_t BuildNameKey(uint32_t domain_crc32, const std::string& topic_name)
989 {
990 const uint32_t topic_len = static_cast<uint32_t>(topic_name.size());
991 std::string key_material;
992 key_material.reserve(sizeof(domain_crc32) + sizeof(topic_len) + topic_len);
993 key_material.append(reinterpret_cast<const char*>(&domain_crc32),
994 sizeof(domain_crc32));
995 key_material.append(reinterpret_cast<const char*>(&topic_len), sizeof(topic_len));
996 key_material.append(topic_name.data(), topic_name.size());
997 return CRC64::Calculate(key_material.data(), key_material.size());
998 }
999
1000 static std::string BuildShmName(uint64_t name_key)
1001 {
1002 char buffer[64] = {};
1003 std::snprintf(buffer, sizeof(buffer), "/libxr_ipc_%016" PRIx64, name_key);
1004 return std::string(buffer);
1005 }
1006
1007 static size_t AlignUp(size_t value, size_t alignment)
1008 {
1009 return (value + alignment - 1U) & ~(alignment - 1U);
1010 }
1011
1012 static uint64_t NowMonotonicMs() { return MonotonicTime::NowMilliseconds(); }
1013
1014 static MicrosecondTimestamp NowMessageTimestamp() { return Topic::NowTimestamp(); }
1015
1016 static uint64_t ToSharedTimestamp(MicrosecondTimestamp timestamp)
1017 {
1018 return MonotonicTime::XrToSharedMicroseconds(static_cast<uint64_t>(timestamp));
1019 }
1020
1021 static MicrosecondTimestamp FromSharedTimestamp(uint64_t timestamp_us)
1022 {
1023 return MicrosecondTimestamp(MonotonicTime::SharedToXrMicroseconds(timestamp_us));
1024 }
1025
1026 static bool ReadProcessIdentity(uint32_t pid, ProcessIdentity& identity)
1027 {
1028 identity = {};
1029 if (pid == 0)
1030 {
1031 return false;
1032 }
1033
1034 char path[64] = {};
1035 std::snprintf(path, sizeof(path), "/proc/%u/stat", pid);
1036
1037 std::ifstream file(path);
1038 if (!file.is_open())
1039 {
1040 return false;
1041 }
1042
1043 std::string line;
1044 std::getline(file, line);
1045 if (line.empty())
1046 {
1047 return false;
1048 }
1049
1050 const size_t rparen = line.rfind(')');
1051 if (rparen == std::string::npos || rparen + 2U >= line.size())
1052 {
1053 return false;
1054 }
1055
1056 std::istringstream iss(line.substr(rparen + 2U));
1057 std::string token;
1058 for (int field = 3; field <= 22; ++field)
1059 {
1060 if (!(iss >> token))
1061 {
1062 return false;
1063 }
1064
1065 if (field == 22)
1066 {
1067 identity.pid = pid;
1068 identity.starttime = std::strtoull(token.c_str(), nullptr, 10);
1069 return identity.starttime != 0;
1070 }
1071 }
1072
1073 return false;
1074 }
1075
1076 static int FutexWait(std::atomic<uint32_t>* word, uint32_t expected,
1077 uint32_t timeout_ms)
1078 {
1079 struct timespec timeout = {};
1080 struct timespec* timeout_ptr = nullptr;
1081 if (timeout_ms != UINT32_MAX)
1082 {
1083 timeout.tv_sec = static_cast<time_t>(timeout_ms / 1000U);
1084 timeout.tv_nsec = static_cast<long>(timeout_ms % 1000U) * 1000000L;
1085 timeout_ptr = &timeout;
1086 }
1087
1088 return static_cast<int>(syscall(SYS_futex, reinterpret_cast<uint32_t*>(word),
1089 FUTEX_WAIT, expected, timeout_ptr, nullptr, 0));
1090 }
1091
1092 static int FutexWake(std::atomic<uint32_t>* word)
1093 {
1094 return static_cast<int>(syscall(SYS_futex, reinterpret_cast<uint32_t*>(word),
1095 FUTEX_WAKE, INT32_MAX, nullptr, nullptr, 0));
1096 }
1097
1098 static size_t ComputeSharedBytes(uint32_t slot_count, uint32_t subscriber_capacity,
1099 uint32_t queue_capacity, uint32_t topic_name_len)
1100 {
1101 size_t offset = 0;
1102 offset = AlignUp(offset, alignof(SharedHeader));
1103 offset += sizeof(SharedHeader);
1104
1105 offset += static_cast<size_t>(topic_name_len) + 1U;
1106
1107 offset = AlignUp(offset, alignof(SlotControl));
1108 offset += sizeof(SlotControl) * slot_count;
1109
1110 offset = AlignUp(offset, alignof(SubscriberControl));
1111 offset += sizeof(SubscriberControl) * subscriber_capacity;
1112
1113 offset = AlignUp(offset, alignof(BalancedGroupControl));
1114 offset += sizeof(BalancedGroupControl);
1115
1116 offset = AlignUp(offset, alignof(std::atomic<uint32_t>));
1117 offset += sizeof(std::atomic<uint32_t>) * subscriber_capacity;
1118
1119 offset = AlignUp(offset, alignof(FreeSlotCell));
1120 offset += sizeof(FreeSlotCell) * slot_count;
1121
1122 offset = AlignUp(offset, alignof(Descriptor));
1123 offset += sizeof(Descriptor) * subscriber_capacity * queue_capacity;
1124
1125 offset = AlignUp(offset, alignof(TopicData));
1126 offset += sizeof(TopicData) * slot_count;
1127 return offset;
1128 }
1129
1130 void SetupPointers()
1131 {
1132 size_t offset = 0;
1133
1134 offset = AlignUp(offset, alignof(SharedHeader));
1135 header_ = reinterpret_cast<SharedHeader*>(base_ + offset);
1136 offset += sizeof(SharedHeader);
1137
1138 topic_name_ptr_ = reinterpret_cast<char*>(base_ + offset);
1139 offset += static_cast<size_t>(header_->topic_name_len) + 1U;
1140
1141 offset = AlignUp(offset, alignof(SlotControl));
1142 slots_ = reinterpret_cast<SlotControl*>(base_ + offset);
1143 offset += sizeof(SlotControl) * slot_count_;
1144
1145 offset = AlignUp(offset, alignof(SubscriberControl));
1146 subscribers_ = reinterpret_cast<SubscriberControl*>(base_ + offset);
1147 offset += sizeof(SubscriberControl) * subscriber_capacity_;
1148
1149 offset = AlignUp(offset, alignof(BalancedGroupControl));
1150 balanced_group_ = reinterpret_cast<BalancedGroupControl*>(base_ + offset);
1151 offset += sizeof(BalancedGroupControl);
1152
1153 offset = AlignUp(offset, alignof(std::atomic<uint32_t>));
1154 balanced_members_ = reinterpret_cast<std::atomic<uint32_t>*>(base_ + offset);
1155 offset += sizeof(std::atomic<uint32_t>) * subscriber_capacity_;
1156
1157 offset = AlignUp(offset, alignof(FreeSlotCell));
1158 free_slots_ = reinterpret_cast<FreeSlotCell*>(base_ + offset);
1159 offset += sizeof(FreeSlotCell) * slot_count_;
1160
1161 offset = AlignUp(offset, alignof(Descriptor));
1162 descriptors_ = reinterpret_cast<Descriptor*>(base_ + offset);
1163 offset += sizeof(Descriptor) * subscriber_capacity_ * queue_capacity_;
1164
1165 offset = AlignUp(offset, alignof(TopicData));
1166 payloads_ = reinterpret_cast<TopicData*>(base_ + offset);
1167 }
1168
1169 bool HeaderMatchesIdentity() const
1170 {
1171 if (header_->name_key != name_key_)
1172 {
1173 return false;
1174 }
1175 if (header_->domain_crc32 != domain_crc32_)
1176 {
1177 return false;
1178 }
1179 if (header_->topic_name_len != topic_name_.size())
1180 {
1181 return false;
1182 }
1183 if (std::memcmp(topic_name_ptr_, topic_name_.c_str(), topic_name_.size() + 1U) != 0)
1184 {
1185 return false;
1186 }
1187 return true;
1188 }
1189
1190 ErrorCode InitializeLayout()
1191 {
1192 if (config_.slot_num == 0 || config_.subscriber_num == 0 || config_.queue_num < 2)
1193 {
1194 return ErrorCode::ARG_ERR;
1195 }
1196
1197 const size_t bytes =
1198 ComputeSharedBytes(config_.slot_num, config_.subscriber_num, config_.queue_num,
1199 static_cast<uint32_t>(topic_name_.size()));
1200
1201 if (ftruncate(fd_, static_cast<off_t>(bytes)) != 0)
1202 {
1203 return ErrorCode::INIT_ERR;
1204 }
1205
1206 const struct stat st = GetStat();
1207 if (st.st_size <= 0)
1208 {
1209 return ErrorCode::INIT_ERR;
1210 }
1211
1212 mapping_size_ = static_cast<size_t>(st.st_size);
1213 mapping_ = mmap(nullptr, mapping_size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
1214 if (mapping_ == MAP_FAILED)
1215 {
1216 mapping_ = nullptr;
1217 return ErrorCode::INIT_ERR;
1218 }
1219
1220 base_ = static_cast<uint8_t*>(mapping_);
1221 slot_count_ = config_.slot_num;
1222 subscriber_capacity_ = config_.subscriber_num;
1223 queue_capacity_ = config_.queue_num;
1224 header_ = reinterpret_cast<SharedHeader*>(base_ + AlignUp(0, alignof(SharedHeader)));
1225 header_->topic_name_len = static_cast<uint32_t>(topic_name_.size());
1226 SetupPointers();
1227
1228 header_->magic = MAGIC;
1229 header_->name_key = name_key_;
1230 header_->domain_crc32 = domain_crc32_;
1231 header_->version = VERSION;
1232 header_->data_size = sizeof(TopicData);
1233 header_->slot_count = slot_count_;
1234 header_->subscriber_capacity = subscriber_capacity_;
1235 header_->queue_capacity = queue_capacity_;
1236 std::memcpy(topic_name_ptr_, topic_name_.c_str(), topic_name_.size() + 1U);
1237 header_->publisher_pid.store(self_identity_.pid, std::memory_order_release);
1238 header_->publisher_starttime.store(self_identity_.starttime,
1239 std::memory_order_release);
1240 header_->free_queue_head.store(0, std::memory_order_release);
1241 header_->free_queue_tail.store(slot_count_, std::memory_order_release);
1242 header_->next_sequence.store(0, std::memory_order_release);
1243 header_->publish_failures.store(0, std::memory_order_release);
1244
1245 for (uint32_t i = 0; i < slot_count_; ++i)
1246 {
1247 slots_[i].refcount.store(0, std::memory_order_release);
1248 slots_[i].sequence.store(0, std::memory_order_release);
1249 slots_[i].timestamp_us = 0;
1250 std::construct_at(&payloads_[i], TopicData{});
1251 free_slots_[i].slot_index = i;
1252 free_slots_[i].sequence.store(static_cast<uint64_t>(i) + 1U,
1253 std::memory_order_release);
1254 }
1255
1256 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1257 {
1258 subscribers_[i].active.store(0, std::memory_order_release);
1259 subscribers_[i].mode.store(
1260 static_cast<uint32_t>(LinuxSharedSubscriberMode::BROADCAST_FULL),
1261 std::memory_order_release);
1262 subscribers_[i].queue_head.store(0, std::memory_order_release);
1263 subscribers_[i].queue_tail.store(0, std::memory_order_release);
1264 subscribers_[i].ready_sem_count.store(0, std::memory_order_release);
1265 subscribers_[i].dropped_messages.store(0, std::memory_order_release);
1266 subscribers_[i].owner_pid.store(0, std::memory_order_release);
1267 subscribers_[i].owner_starttime.store(0, std::memory_order_release);
1268 subscribers_[i].held_slot.store(INVALID_INDEX, std::memory_order_release);
1269 balanced_members_[i].store(INVALID_INDEX, std::memory_order_release);
1270 }
1271
1272 balanced_group_->rr_cursor.store(0, std::memory_order_release);
1273
1274 for (size_t i = 0; i < static_cast<size_t>(subscriber_capacity_) * queue_capacity_;
1275 ++i)
1276 {
1277 descriptors_[i] = Descriptor{};
1278 }
1279
1280 header_->init_state.store(INIT_READY, std::memory_order_release);
1281 return ErrorCode::OK;
1282 }
1283
1284 ErrorCode AttachLayout()
1285 {
1286 const struct stat st = GetStat();
1287 if (st.st_size <= 0)
1288 {
1289 return ErrorCode::NOT_FOUND;
1290 }
1291
1292 mapping_size_ = static_cast<size_t>(st.st_size);
1293 mapping_ = mmap(nullptr, mapping_size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
1294 if (mapping_ == MAP_FAILED)
1295 {
1296 mapping_ = nullptr;
1297 return ErrorCode::INIT_ERR;
1298 }
1299
1300 base_ = static_cast<uint8_t*>(mapping_);
1301 header_ = reinterpret_cast<SharedHeader*>(base_);
1302
1303 while (header_->init_state.load(std::memory_order_acquire) != INIT_READY)
1304 {
1305 usleep(1000);
1306 }
1307
1308 if (header_->magic != MAGIC || header_->version != VERSION ||
1309 header_->data_size != sizeof(TopicData))
1310 {
1311 return ErrorCode::CHECK_ERR;
1312 }
1313
1314 slot_count_ = header_->slot_count;
1315 subscriber_capacity_ = header_->subscriber_capacity;
1316 queue_capacity_ = header_->queue_capacity;
1317 SetupPointers();
1318 if (!HeaderMatchesIdentity())
1319 {
1320 return ErrorCode::CHECK_ERR;
1321 }
1322 return ErrorCode::OK;
1323 }
1324
1325 bool TryReclaimStaleSegment()
1326 {
1327 int stale_fd = shm_open(shm_name_.c_str(), O_RDWR, 0600);
1328 if (stale_fd < 0)
1329 {
1330 return errno == ENOENT;
1331 }
1332
1333 struct stat st = {};
1334 if (fstat(stale_fd, &st) != 0)
1335 {
1336 close(stale_fd);
1337 return false;
1338 }
1339
1340 bool reclaim = false;
1341 if (st.st_size < static_cast<off_t>(sizeof(SharedHeader)))
1342 {
1343 reclaim = true;
1344 }
1345 else
1346 {
1347 void* mapping = mmap(nullptr, static_cast<size_t>(st.st_size),
1348 PROT_READ | PROT_WRITE, MAP_SHARED, stale_fd, 0);
1349 if (mapping != MAP_FAILED)
1350 {
1351 uint8_t* base = static_cast<uint8_t*>(mapping);
1352 auto* header = reinterpret_cast<SharedHeader*>(mapping);
1353 const uint32_t init_state = header->init_state.load(std::memory_order_acquire);
1354 bool identity_match = false;
1355 const size_t mapping_size = static_cast<size_t>(st.st_size);
1356 const size_t topic_name_bytes = topic_name_.size() + 1U;
1357 if (header->magic == MAGIC && header->version == VERSION &&
1358 header->domain_crc32 == domain_crc32_ &&
1359 header->topic_name_len == topic_name_.size())
1360 {
1361 size_t offset = AlignUp(0, alignof(SharedHeader));
1362 offset += sizeof(SharedHeader);
1363 if (offset <= mapping_size && topic_name_bytes <= (mapping_size - offset))
1364 {
1365 const char* mapped_topic = reinterpret_cast<const char*>(base + offset);
1366 identity_match =
1367 (header->name_key == name_key_) &&
1368 (std::memcmp(mapped_topic, topic_name_.c_str(), topic_name_bytes) == 0);
1369 }
1370 }
1371 const ProcessIdentity publisher_identity = {
1372 header->publisher_pid.load(std::memory_order_acquire),
1373 header->publisher_starttime.load(std::memory_order_acquire),
1374 };
1375
1376 if (!identity_match)
1377 {
1378 reclaim = false;
1379 }
1380 else if (init_state != INIT_READY)
1381 {
1382 reclaim = !ProcessAlive(publisher_identity);
1383 }
1384 else if (!ProcessAlive(publisher_identity))
1385 {
1386 reclaim = true;
1387 }
1388
1389 munmap(mapping, static_cast<size_t>(st.st_size));
1390 }
1391 }
1392
1393 close(stale_fd);
1394
1395 if (!reclaim)
1396 {
1397 return false;
1398 }
1399
1400 return shm_unlink(shm_name_.c_str()) == 0 || errno == ENOENT;
1401 }
1402
1403 void Open()
1404 {
1405 open_status_ = ErrorCode::STATE_ERR;
1406 open_ok_ = false;
1407
1408 if (create_)
1409 {
1410 for (int attempt = 0; attempt < 2; ++attempt)
1411 {
1412 fd_ = shm_open(shm_name_.c_str(), O_CREAT | O_EXCL | O_RDWR, 0600);
1413 if (fd_ >= 0)
1414 {
1415 break;
1416 }
1417
1418 if (errno != EEXIST || !TryReclaimStaleSegment())
1419 {
1420 break;
1421 }
1422 }
1423
1424 if (fd_ < 0)
1425 {
1426 open_status_ = (errno == EEXIST) ? ErrorCode::BUSY : ErrorCode::INIT_ERR;
1427 return;
1428 }
1429
1430 open_status_ = InitializeLayout();
1431 }
1432 else
1433 {
1434 fd_ = shm_open(shm_name_.c_str(), O_RDWR, 0600);
1435 if (fd_ < 0)
1436 {
1437 open_status_ = (errno == ENOENT) ? ErrorCode::NOT_FOUND : ErrorCode::INIT_ERR;
1438 return;
1439 }
1440
1441 open_status_ = AttachLayout();
1442 }
1443
1444 if (fd_ >= 0)
1445 {
1446 close(fd_);
1447 fd_ = -1;
1448 }
1449
1450 open_ok_ = (open_status_ == ErrorCode::OK);
1451 }
1452
1453 void Close()
1454 {
1455 if (mapping_ != nullptr)
1456 {
1457 munmap(mapping_, mapping_size_);
1458 }
1459
1460 mapping_ = nullptr;
1461 base_ = nullptr;
1462 header_ = nullptr;
1463 slots_ = nullptr;
1464 subscribers_ = nullptr;
1465 free_slots_ = nullptr;
1466 descriptors_ = nullptr;
1467 payloads_ = nullptr;
1468 mapping_size_ = 0;
1469 open_ok_ = false;
1470 open_status_ = ErrorCode::STATE_ERR;
1471 }
1472
1473 struct stat GetStat() const
1474 {
1475 struct stat st = {};
1476 fstat(fd_, &st);
1477 return st;
1478 }
1479
1480 Descriptor* DescriptorRing(uint32_t subscriber_index) const
1481 {
1482 return descriptors_ + static_cast<size_t>(subscriber_index) * queue_capacity_;
1483 }
1484
1485 static bool ProcessAlive(const ProcessIdentity& identity)
1486 {
1487 ProcessIdentity current = {};
1488 if (!ReadProcessIdentity(identity.pid, current))
1489 {
1490 return false;
1491 }
1492
1493 return current.starttime == identity.starttime;
1494 }
1495
1496 bool PublisherValid() const
1497 {
1498 if (!publisher_ || header_ == nullptr)
1499 {
1500 return false;
1501 }
1502
1503 const ProcessIdentity owner = {
1504 header_->publisher_pid.load(std::memory_order_acquire),
1505 header_->publisher_starttime.load(std::memory_order_acquire),
1506 };
1507 return owner.pid == self_identity_.pid && owner.starttime == self_identity_.starttime;
1508 }
1509
1510 void HoldSlot(uint32_t subscriber_index, uint32_t slot_index)
1511 {
1512 subscribers_[subscriber_index].held_slot.store(slot_index, std::memory_order_release);
1513 }
1514
1515 void ClearHeldSlot(uint32_t subscriber_index, uint32_t slot_index)
1516 {
1517 uint32_t expected = slot_index;
1518 subscribers_[subscriber_index].held_slot.compare_exchange_strong(
1519 expected, INVALID_INDEX, std::memory_order_acq_rel, std::memory_order_relaxed);
1520 }
1521
1522 MicrosecondTimestamp SlotTimestamp(uint32_t slot_index) const
1523 {
1524 return FromSharedTimestamp(slots_[slot_index].timestamp_us);
1525 }
1526
1527 ErrorCode RegisterBalancedSubscriber(uint32_t subscriber_index)
1528 {
1529 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1530 {
1531 uint32_t expected = INVALID_INDEX;
1532 if (balanced_members_[i].compare_exchange_strong(expected, subscriber_index,
1533 std::memory_order_acq_rel,
1534 std::memory_order_relaxed))
1535 {
1536 return ErrorCode::OK;
1537 }
1538 }
1539 return ErrorCode::FULL;
1540 }
1541
1542 void UnregisterBalancedSubscriber(uint32_t subscriber_index)
1543 {
1544 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1545 {
1546 uint32_t expected = subscriber_index;
1547 if (balanced_members_[i].compare_exchange_strong(expected, INVALID_INDEX,
1548 std::memory_order_acq_rel,
1549 std::memory_order_relaxed))
1550 {
1551 return;
1552 }
1553 }
1554 }
1555
1556 bool SelectBalancedSubscriber(uint32_t& subscriber_index)
1557 {
1558 const uint64_t base =
1559 balanced_group_->rr_cursor.fetch_add(1, std::memory_order_acq_rel);
1560 for (uint32_t offset = 0; offset < subscriber_capacity_; ++offset)
1561 {
1562 const uint32_t member_index =
1563 balanced_members_[(base + offset) % subscriber_capacity_].load(
1564 std::memory_order_acquire);
1565 if (member_index == INVALID_INDEX)
1566 {
1567 continue;
1568 }
1569 if (subscribers_[member_index].active.load(std::memory_order_acquire) == 0)
1570 {
1571 continue;
1572 }
1573 if (subscribers_[member_index].mode.load(std::memory_order_acquire) !=
1574 static_cast<uint32_t>(LinuxSharedSubscriberMode::BALANCE_RR))
1575 {
1576 continue;
1577 }
1578 const ProcessIdentity owner_identity = {
1579 subscribers_[member_index].owner_pid.load(std::memory_order_acquire),
1580 subscribers_[member_index].owner_starttime.load(std::memory_order_acquire),
1581 };
1582 if (owner_identity.pid == 0 || owner_identity.starttime == 0)
1583 {
1584 continue;
1585 }
1586 if (!ProcessAlive(owner_identity))
1587 {
1588 ReclaimSubscriber(member_index);
1589 continue;
1590 }
1591 if (!QueueHasSpace(member_index))
1592 {
1593 continue;
1594 }
1595 subscriber_index = member_index;
1596 return true;
1597 }
1598 return false;
1599 }
1600
1601 bool ReclaimSubscriber(uint32_t subscriber_index)
1602 {
1603 uint32_t expected = 1;
1604 if (!subscribers_[subscriber_index].active.compare_exchange_strong(
1605 expected, 0, std::memory_order_acq_rel, std::memory_order_relaxed))
1606 {
1607 return false;
1608 }
1609
1610 if (subscribers_[subscriber_index].mode.load(std::memory_order_acquire) ==
1611 static_cast<uint32_t>(LinuxSharedSubscriberMode::BALANCE_RR))
1612 {
1613 UnregisterBalancedSubscriber(subscriber_index);
1614 }
1615
1616 subscribers_[subscriber_index].owner_pid.store(0, std::memory_order_release);
1617 subscribers_[subscriber_index].owner_starttime.store(0, std::memory_order_release);
1618 subscribers_[subscriber_index].mode.store(
1619 static_cast<uint32_t>(LinuxSharedSubscriberMode::BROADCAST_FULL),
1620 std::memory_order_release);
1621
1622 const uint32_t held_slot = subscribers_[subscriber_index].held_slot.exchange(
1623 INVALID_INDEX, std::memory_order_acq_rel);
1624 if (held_slot != INVALID_INDEX)
1625 {
1626 ReleaseSlot(held_slot);
1627 }
1628
1629 Descriptor desc = {};
1630 while (TryPopDescriptor(subscriber_index, desc) == ErrorCode::OK)
1631 {
1632 ReleaseSlot(desc.slot_index);
1633 }
1634
1635 return true;
1636 }
1637
1638 static void PostReady(SubscriberControl& control)
1639 {
1640 control.ready_sem_count.fetch_add(1, std::memory_order_release);
1641 FutexWake(&control.ready_sem_count);
1642 }
1643
1644 static void ConsumeReady(SubscriberControl& control)
1645 {
1646 const uint32_t prev = control.ready_sem_count.fetch_sub(1, std::memory_order_acq_rel);
1647 ASSERT(prev > 0);
1648 }
1649
1650 static ErrorCode WaitReady(SubscriberControl& control, uint32_t timeout_ms)
1651 {
1652 if (control.ready_sem_count.load(std::memory_order_acquire) != 0)
1653 {
1654 return ErrorCode::OK;
1655 }
1656
1657 const bool infinite_wait = (timeout_ms == UINT32_MAX);
1658 const uint64_t deadline_ms = infinite_wait ? 0 : (NowMonotonicMs() + timeout_ms);
1659
1660 while (true)
1661 {
1662 if (control.ready_sem_count.load(std::memory_order_acquire) != 0)
1663 {
1664 return ErrorCode::OK;
1665 }
1666
1667 uint32_t wait_ms = UINT32_MAX;
1668 if (!infinite_wait)
1669 {
1670 wait_ms = MonotonicTime::RemainingMilliseconds(deadline_ms);
1671 if (wait_ms == 0)
1672 {
1673 return ErrorCode::TIMEOUT;
1674 }
1675 }
1676
1677 wait_ms = MonotonicTime::WaitSliceMilliseconds(wait_ms);
1678
1679 const int futex_ans = FutexWait(&control.ready_sem_count, 0, wait_ms);
1680 if (futex_ans == 0 || errno == EAGAIN || errno == EINTR)
1681 {
1682 continue;
1683 }
1684
1685 if (errno == ETIMEDOUT)
1686 {
1687 if (infinite_wait)
1688 {
1689 continue;
1690 }
1691 if (MonotonicTime::RemainingMilliseconds(deadline_ms) == 0 &&
1692 control.ready_sem_count.load(std::memory_order_acquire) == 0)
1693 {
1694 return ErrorCode::TIMEOUT;
1695 }
1696 continue;
1697 }
1698
1699 return ErrorCode::FAILED;
1700 }
1701 }
1702
1703 void ScavengeDeadSubscribers()
1704 {
1705 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1706 {
1707 if (subscribers_[i].active.load(std::memory_order_acquire) == 0)
1708 {
1709 continue;
1710 }
1711
1712 const ProcessIdentity owner_identity = {
1713 subscribers_[i].owner_pid.load(std::memory_order_acquire),
1714 subscribers_[i].owner_starttime.load(std::memory_order_acquire),
1715 };
1716 if (ProcessAlive(owner_identity))
1717 {
1718 continue;
1719 }
1720
1721 ReclaimSubscriber(i);
1722 }
1723 }
1724
1725 bool QueueHasSpace(uint32_t subscriber_index) const
1726 {
1727 const SubscriberControl& control = subscribers_[subscriber_index];
1728 const uint32_t head = control.queue_head.load(std::memory_order_acquire);
1729 const uint32_t tail = control.queue_tail.load(std::memory_order_relaxed);
1730 const uint32_t next_tail = (tail + 1U) % queue_capacity_;
1731 return next_tail != head;
1732 }
1733
1734 void PushDescriptor(uint32_t subscriber_index, const Descriptor& descriptor)
1735 {
1736 SubscriberControl& control = subscribers_[subscriber_index];
1737 Descriptor* ring = DescriptorRing(subscriber_index);
1738
1739 const uint32_t tail = control.queue_tail.load(std::memory_order_relaxed);
1740 ring[tail] = descriptor;
1741 const uint32_t next_tail = (tail + 1U) % queue_capacity_;
1742 control.queue_tail.store(next_tail, std::memory_order_release);
1743 PostReady(control);
1744 }
1745
1746 ErrorCode TryPopDescriptor(uint32_t subscriber_index, Descriptor& descriptor)
1747 {
1748 SubscriberControl& control = subscribers_[subscriber_index];
1749 Descriptor* ring = DescriptorRing(subscriber_index);
1750
1751 while (true)
1752 {
1753 uint32_t head = control.queue_head.load(std::memory_order_relaxed);
1754 const uint32_t tail = control.queue_tail.load(std::memory_order_acquire);
1755 if (head == tail)
1756 {
1757 return ErrorCode::EMPTY;
1758 }
1759
1760 descriptor = ring[head];
1761 const uint32_t next_head = (head + 1U) % queue_capacity_;
1762 if (control.queue_head.compare_exchange_weak(
1763 head, next_head, std::memory_order_acq_rel, std::memory_order_relaxed))
1764 {
1765 ConsumeReady(control);
1766 return ErrorCode::OK;
1767 }
1768 }
1769 }
1770
1771 ErrorCode DropDescriptor(uint32_t subscriber_index)
1772 {
1773 SubscriberControl& control = subscribers_[subscriber_index];
1774 Descriptor* ring = DescriptorRing(subscriber_index);
1775
1776 while (true)
1777 {
1778 uint32_t head = control.queue_head.load(std::memory_order_relaxed);
1779 const uint32_t tail = control.queue_tail.load(std::memory_order_acquire);
1780 if (head == tail)
1781 {
1782 return ErrorCode::EMPTY;
1783 }
1784
1785 const Descriptor descriptor = ring[head];
1786 const uint32_t next_head = (head + 1U) % queue_capacity_;
1787 if (control.queue_head.compare_exchange_weak(
1788 head, next_head, std::memory_order_acq_rel, std::memory_order_relaxed))
1789 {
1790 control.dropped_messages.fetch_add(1, std::memory_order_relaxed);
1791 ConsumeReady(control);
1792 ReleaseSlot(descriptor.slot_index);
1793 return ErrorCode::OK;
1794 }
1795 }
1796 }
1797
1798 ErrorCode PopFreeSlot(uint32_t& slot_index)
1799 {
1800 while (true)
1801 {
1802 uint64_t head = header_->free_queue_head.load(std::memory_order_relaxed);
1803 FreeSlotCell& cell = free_slots_[head % slot_count_];
1804 const uint64_t seq = cell.sequence.load(std::memory_order_acquire);
1805 const intptr_t diff = static_cast<intptr_t>(seq) - static_cast<intptr_t>(head + 1U);
1806
1807 if (diff == 0)
1808 {
1809 if (header_->free_queue_head.compare_exchange_weak(
1810 head, head + 1U, std::memory_order_acq_rel, std::memory_order_relaxed))
1811 {
1812 slot_index = cell.slot_index;
1813 cell.sequence.store(head + slot_count_, std::memory_order_release);
1814 return ErrorCode::OK;
1815 }
1816 }
1817 else if (diff < 0)
1818 {
1819 return ErrorCode::FULL;
1820 }
1821 }
1822 }
1823
1824 void RecycleSlot(uint32_t slot_index)
1825 {
1826 slots_[slot_index].sequence.store(0, std::memory_order_release);
1827 slots_[slot_index].timestamp_us = 0;
1828
1829 while (true)
1830 {
1831 uint64_t tail = header_->free_queue_tail.load(std::memory_order_relaxed);
1832 FreeSlotCell& cell = free_slots_[tail % slot_count_];
1833 const uint64_t seq = cell.sequence.load(std::memory_order_acquire);
1834 const intptr_t diff = static_cast<intptr_t>(seq) - static_cast<intptr_t>(tail);
1835
1836 if (diff == 0)
1837 {
1838 if (header_->free_queue_tail.compare_exchange_weak(
1839 tail, tail + 1U, std::memory_order_acq_rel, std::memory_order_relaxed))
1840 {
1841 cell.slot_index = slot_index;
1842 cell.sequence.store(tail + 1U, std::memory_order_release);
1843 return;
1844 }
1845 }
1846 }
1847 }
1848
1849 void ReleaseSlot(uint32_t slot_index)
1850 {
1851 const uint32_t prev =
1852 slots_[slot_index].refcount.fetch_sub(1, std::memory_order_acq_rel);
1853 ASSERT(prev > 0);
1854 if (prev == 1)
1855 {
1856 RecycleSlot(slot_index);
1857 }
1858 }
1859
1860 template <bool HAS_TIMESTAMP>
1861 ErrorCode PublishData(SharedData& data,
1862 MicrosecondTimestamp timestamp = MicrosecondTimestamp())
1863 {
1864 if (!data.Valid() || data.topic_ != this)
1865 {
1866 return ErrorCode::STATE_ERR;
1867 }
1868
1869 uint32_t active_count = 0;
1870 uint32_t balanced_target = INVALID_INDEX;
1871 bool has_balanced_subscriber = false;
1872 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1873 {
1874 if (subscribers_[i].active.load(std::memory_order_acquire) == 0)
1875 {
1876 continue;
1877 }
1878
1879 const LinuxSharedSubscriberMode mode = static_cast<LinuxSharedSubscriberMode>(
1880 subscribers_[i].mode.load(std::memory_order_acquire));
1881 if (mode == LinuxSharedSubscriberMode::BALANCE_RR)
1882 {
1883 has_balanced_subscriber = true;
1884 continue;
1885 }
1886
1887 if (!QueueHasSpace(i))
1888 {
1889 ScavengeDeadSubscribers();
1890 if (subscribers_[i].active.load(std::memory_order_acquire) == 0)
1891 {
1892 continue;
1893 }
1894
1895 if (mode == LinuxSharedSubscriberMode::BROADCAST_DROP_OLD)
1896 {
1897 const ErrorCode drop_ans = DropDescriptor(i);
1898 if (drop_ans == ErrorCode::EMPTY && QueueHasSpace(i))
1899 {
1900 // 消费者可能在 QueueHasSpace() 和 DropDescriptor() 之间弹走旧描述符。
1901 // 此时队列已经有空位,发布者继续写入即可,不能把竞态误报成 FULL。
1902 }
1903 else if (drop_ans != ErrorCode::OK)
1904 {
1905 header_->publish_failures.fetch_add(1, std::memory_order_relaxed);
1906 data.Reset();
1907 return ErrorCode::FULL;
1908 }
1909 }
1910 else
1911 {
1912 subscribers_[i].dropped_messages.fetch_add(1, std::memory_order_relaxed);
1913 header_->publish_failures.fetch_add(1, std::memory_order_relaxed);
1914 data.Reset();
1915 return ErrorCode::FULL;
1916 }
1917 }
1918
1919 ++active_count;
1920 }
1921
1922 if (has_balanced_subscriber)
1923 {
1924 if (!SelectBalancedSubscriber(balanced_target))
1925 {
1926 ScavengeDeadSubscribers();
1927 if (!SelectBalancedSubscriber(balanced_target))
1928 {
1929 header_->publish_failures.fetch_add(1, std::memory_order_relaxed);
1930 data.Reset();
1931 return ErrorCode::FULL;
1932 }
1933 }
1934 ++active_count;
1935 }
1936
1937 if (active_count == 0)
1938 {
1939 data.Reset();
1940 return ErrorCode::OK;
1941 }
1942
1943 const uint64_t sequence =
1944 header_->next_sequence.fetch_add(1, std::memory_order_acq_rel) + 1ULL;
1945 if constexpr (!HAS_TIMESTAMP)
1946 {
1947 timestamp = NowMessageTimestamp();
1948 }
1949 SlotControl& slot = slots_[data.slot_index_];
1950 slot.refcount.store(active_count, std::memory_order_release);
1951 slot.timestamp_us = ToSharedTimestamp(timestamp);
1952 slot.sequence.store(sequence, std::memory_order_release);
1953
1954 const Descriptor descriptor = {data.slot_index_, 0U, sequence};
1955 for (uint32_t i = 0; i < subscriber_capacity_; ++i)
1956 {
1957 if (subscribers_[i].active.load(std::memory_order_acquire) == 0)
1958 {
1959 continue;
1960 }
1961 const LinuxSharedSubscriberMode mode = static_cast<LinuxSharedSubscriberMode>(
1962 subscribers_[i].mode.load(std::memory_order_acquire));
1963 if (mode == LinuxSharedSubscriberMode::BALANCE_RR)
1964 {
1965 continue;
1966 }
1967 PushDescriptor(i, descriptor);
1968 }
1969
1970 if (balanced_target != INVALID_INDEX)
1971 {
1972 PushDescriptor(balanced_target, descriptor);
1973 }
1974
1975 data.topic_ = nullptr;
1976 data.slot_index_ = INVALID_INDEX;
1977 return ErrorCode::OK;
1978 }
1979
1980 bool create_ = false;
1981 bool publisher_ = false;
1982 LinuxSharedTopicConfig config_;
1983 uint32_t domain_crc32_ = 0;
1984 std::string topic_name_;
1985 uint64_t name_key_ = 0;
1986 std::string shm_name_;
1987 ProcessIdentity self_identity_ = {};
1988
1989 int fd_ = -1;
1990 void* mapping_ = nullptr;
1991 uint8_t* base_ = nullptr;
1992 size_t mapping_size_ = 0;
1993
1994 SharedHeader* header_ = nullptr;
1995 char* topic_name_ptr_ = nullptr;
1996 SlotControl* slots_ = nullptr;
1997 SubscriberControl* subscribers_ = nullptr;
1998 BalancedGroupControl* balanced_group_ = nullptr;
1999 std::atomic<uint32_t>* balanced_members_ = nullptr;
2000 FreeSlotCell* free_slots_ = nullptr;
2001 Descriptor* descriptors_ = nullptr;
2002 TopicData* payloads_ = nullptr;
2003
2004 uint32_t slot_count_ = 0;
2005 uint32_t subscriber_capacity_ = 0;
2006 uint32_t queue_capacity_ = 0;
2007
2008 bool open_ok_ = false;
2009 ErrorCode open_status_ = ErrorCode::STATE_ERR;
2010};
2011
2012} // namespace LibXR
2013
2014#endif
LibXR 命名空间
Definition ch32_can.hpp:14
ErrorCode
定义错误码枚举
@ INIT_ERR
初始化错误 | Initialization error
@ EMPTY
为空 | Empty