Skip to content

Commit 4b5d628

Browse files
serges147serges147
committed
implemented client side Route-s
1 parent 3814a48 commit 4b5d628

File tree

14 files changed

+580
-50
lines changed

14 files changed

+580
-50
lines changed

‎src/common/dsdl/ocvsmd/common/ipc/Route.1.0.dsdl‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
@union
2+
3+
uavcan.primitive.Empty.1.0 empty
4+
RouteConnect.1.0 connect
5+
RouteChannelMsg.1.0 channel_msg
6+
7+
@sealed

‎src/common/dsdl/ocvsmd/common/ipc/RouteChannelMsg.1.0.dsdl‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
uint64 tag
2+
uint64 type_id
3+
4+
# reserve twice as much as we need.
5+
@extent _offset_.max * 2

‎src/common/dsdl/ocvsmd/common/ipc/RouteConnect.1.0.dsdl‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
uavcan.node.Version.1.0 version
2+
3+
# reserve twice as much as we need.
4+
@extent _offset_.max * 2

src/common/dsdl/ocvsmd/common/dsdl/Foo.1.0.dsdl renamed to src/common/dsdl/ocvsmd/common/node_command/ExecCmd.1.0.dsdl

File renamed without changes.

‎src/common/dsdl_helpers.hpp‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,25 @@ static auto tryDeserializePayload(const cetl::span<const std::uint8_t> payload,
2626
return deserialize(out_message, {payload.data(), payload.size()});
2727
}
2828

29+
template <typename Message, typename Action>
30+
static int tryPerformOnSerialized(const Message& message, Action&& action)
31+
{
32+
// Try to serialize the message to raw payload buffer.
33+
//
34+
// Next nolint b/c we use a buffer to serialize the message, so no need to zero it (and performance better).
35+
// NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init,hicpp-member-init)
36+
std::array<std::uint8_t, Message::_traits_::SerializationBufferSizeBytes> buffer;
37+
//
38+
const auto result_size = serialize(message, {buffer.data(), buffer.size()});
39+
if (!result_size)
40+
{
41+
return EINVAL;
42+
}
43+
44+
const cetl::span<const std::uint8_t> bytes{buffer.data(), result_size.value()};
45+
return std::forward<Action>(action)(bytes);
46+
}
47+
2948
template <typename Message, typename Result, std::size_t BufferSize, bool IsOnStack, typename Action>
3049
static auto tryPerformOnSerialized(const Message& message, Action&& action) -> std::enable_if_t<IsOnStack, Result>
3150
{
@@ -38,7 +57,7 @@ static auto tryPerformOnSerialized(const Message& message, Action&& action) -> s
3857
const auto result_size = serialize(message, {buffer.data(), buffer.size()});
3958
if (!result_size)
4059
{
41-
return result_size.error();
60+
return Result{result_size.error()};
4261
}
4362

4463
const cetl::span<const std::uint8_t> bytes{buffer.data(), result_size.value()};
@@ -56,7 +75,7 @@ static auto tryPerformOnSerialized(const Message& message, Action&& action) -> s
5675
const auto result_size = serialize(message, {buffer->data(), buffer->size()});
5776
if (!result_size)
5877
{
59-
return result_size.error();
78+
return Result{result_size.error()};
6079
}
6180

6281
const cetl::span<const std::uint8_t> bytes{buffer->data(), result_size.value()};

‎src/common/ipc/channel.hpp‎

Lines changed: 52 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99
#include "dsdl_helpers.hpp"
1010
#include "gateway.hpp"
1111

12+
#include <nunavut/support/serialization.hpp>
13+
1214
#include <cetl/pf17/cetlpf.hpp>
1315

1416
#include <cstddef>
@@ -42,55 +44,56 @@ class AnyChannel
4244

4345
}; // AnyChannel
4446

45-
template <typename Input, typename Output>
47+
template <typename Input_, typename Output_>
4648
class Channel final : public AnyChannel
4749
{
4850
public:
51+
using Input = Input_;
52+
using Output = Output_;
53+
using EventVar = EventVar<Input>;
54+
using EventHandler = EventHandler<Input>;
55+
4956
Channel(Channel&& other) noexcept
50-
: gateway_{std::move(other.gateway_)}
57+
: memory_{other.memory_}
58+
, gateway_{std::move(other.gateway_)}
5159
, event_handler_{std::move(other.event_handler_)}
5260
{
5361
setupEventHandler();
5462
}
5563

56-
Channel& operator=(Channel&& other) noexcept
57-
{
58-
if (this != &other)
59-
{
60-
gateway_ = std::move(other.gateway_);
61-
event_handler_ = std::move(other.event_handler_);
62-
setupEventHandler();
63-
}
64-
return *this;
65-
}
66-
67-
Channel(const Channel&) = delete;
68-
Channel& operator=(const Channel&) = delete;
64+
Channel(const Channel&) = delete;
65+
Channel& operator=(const Channel&) = delete;
66+
Channel& operator=(Channel&& other) noexcept = delete;
6967

7068
~Channel()
7169
{
7270
gateway_->setEventHandler(nullptr);
7371
event_handler_ = nullptr;
7472
}
7573

76-
void send(const Output& output)
74+
using SendFailure = nunavut::support::Error;
75+
using SendResult = cetl::optional<SendFailure>;
76+
77+
SendResult send(const Output& output)
7778
{
78-
constexpr std::size_t BufferSize = Output::;
79+
constexpr std::size_t BufferSize = Output::_traits_::SerializationBufferSizeBytes;
7980
constexpr bool IsOnStack = BufferSize <= MsgSmallPayloadSize;
8081

81-
return tryPerformOnSerialized<Output, Result, BufferSize, IsOnStack>( //
82+
return tryPerformOnSerialized<Output, SendResult, BufferSize, IsOnStack>( //
8283
output,
8384
[this](const auto payload) {
8485
//
8586
gateway_->send(payload);
87+
return cetl::nullopt;
8688
});
8789
}
8890

8991
private:
9092
friend class ClientRouter;
9193

92-
Channel(detail::Gateway::Ptr gateway, EventHandler<Input> event_handler)
93-
: gateway_{std::move(gateway)}
94+
Channel(cetl::pmr::memory_resource& memory, detail::Gateway::Ptr gateway, EventHandler event_handler)
95+
: memory_{memory}
96+
, gateway_{std::move(gateway)}
9497
, event_handler_{std::move(event_handler)}
9598
{
9699
CETL_DEBUG_ASSERT(gateway_, "");
@@ -101,16 +104,41 @@ class Channel final : public AnyChannel
101104

102105
void setupEventHandler()
103106
{
104-
gateway_->setEventHandler([this](const auto& event) {
107+
gateway_->setEventHandler([this](const detail::Gateway::Event::Var& gateway_event_var) {
105108
//
106-
event_handler_(event);
109+
cetl::visit(
110+
[this](const auto& gateway_event) {
111+
//
112+
handleGatewayEvent(gateway_event);
113+
},
114+
gateway_event_var);
107115
});
108116
}
109117

118+
void handleGatewayEvent(const detail::Gateway::Event::Message& gateway_message)
119+
{
120+
Input input{&memory_};
121+
if (tryDeserializePayload(gateway_message.payload, input))
122+
{
123+
event_handler_(input);
124+
}
125+
}
126+
127+
void handleGatewayEvent(const detail::Gateway::Event::Connected)
128+
{
129+
event_handler_(Connected{});
130+
}
131+
132+
void handleGatewayEvent(const detail::Gateway::Event::Disconnected)
133+
{
134+
event_handler_(Disconnected{});
135+
}
136+
110137
static constexpr std::size_t MsgSmallPayloadSize = 256;
111138

112-
detail::Gateway::Ptr gateway_;
113-
EventHandler<Input> event_handler_;
139+
cetl::pmr::memory_resource& memory_;
140+
detail::Gateway::Ptr gateway_;
141+
EventHandler event_handler_;
114142

115143
}; // Channel
116144

‎src/common/ipc/client_router.cpp‎

Lines changed: 99 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,20 @@
55

66
#include "client_router.hpp"
77

8+
#include "dsdl_helpers.hpp"
89
#include "gateway.hpp"
910
#include "pipe/client_pipe.hpp"
1011

12+
#include "ocvsmd/common/ipc/RouteChannelMsg_1_0.hpp"
13+
#include "ocvsmd/common/ipc/RouteConnect_1_0.hpp"
14+
#include "ocvsmd/common/ipc/Route_1_0.hpp"
15+
#include "uavcan/primitive/Empty_1_0.hpp"
16+
1117
#include <cetl/cetl.hpp>
18+
#include <cetl/pf17/cetlpf.hpp>
19+
#include <cetl/visit_helpers.hpp>
1220

21+
#include <cerrno>
1322
#include <cstdint>
1423
#include <memory>
1524
#include <unordered_map>
@@ -27,15 +36,34 @@ namespace
2736
class ClientRouterImpl final : public ClientRouter
2837
{
2938
public:
30-
explicit ClientRouterImpl(pipe::ClientPipe::Ptr client_pipe)
31-
: client_pipe_{std::move(client_pipe)}
39+
ClientRouterImpl(cetl::pmr::memory_resource& memory, pipe::ClientPipe::Ptr client_pipe)
40+
: memory_{memory}
41+
, client_pipe_{std::move(client_pipe)}
3242
, next_tag_{0}
3343
{
3444
CETL_DEBUG_ASSERT(client_pipe_, "");
3545
}
3646

3747
// ClientRouter
3848

49+
cetl::pmr::memory_resource& memory() override
50+
{
51+
return memory_;
52+
}
53+
54+
void start() override
55+
{
56+
client_pipe_->start([this](const auto& pipe_event_var) {
57+
//
58+
return cetl::visit(
59+
[this](const auto& pipe_event) {
60+
//
61+
return handlePipeEvent(pipe_event);
62+
},
63+
pipe_event_var);
64+
});
65+
}
66+
3967
CETL_NODISCARD detail::Gateway::Ptr makeGateway() override
4068
{
4169
const Tag new_tag = ++next_tag_;
@@ -109,6 +137,73 @@ class ClientRouterImpl final : public ClientRouter
109137

110138
}; // GatewayImpl
111139

140+
int handlePipeEvent(const pipe::ClientPipe::Event::Connected)
141+
{
142+
Route_1_0 route{&memory_};
143+
auto& route_conn = route.set_connect();
144+
route_conn.version.major = VERSION_MAJOR;
145+
route_conn.version.minor = VERSION_MINOR;
146+
147+
return tryPerformOnSerialized<Route_1_0>(route, [this](const auto payload) {
148+
//
149+
return client_pipe_->sendMessage(payload);
150+
});
151+
}
152+
153+
int handlePipeEvent(const pipe::ClientPipe::Event::Message& msg)
154+
{
155+
Route_1_0 route_msg{&memory_};
156+
const auto result_size = tryDeserializePayload(msg.payload, route_msg);
157+
if (!result_size.has_value())
158+
{
159+
return EINVAL;
160+
}
161+
162+
const auto remaining_payload = msg.payload.subspan(result_size.value());
163+
164+
cetl::visit(cetl::make_overloaded(
165+
//
166+
[this](const uavcan::primitive::Empty_1_0&) {},
167+
[this](const RouteConnect_1_0& route_conn) {
168+
//
169+
handleRouteConnect(route_conn);
170+
},
171+
[this, remaining_payload](const RouteChannelMsg_1_0& route_channel) {
172+
//
173+
handleRouteChannelMsg(route_channel, remaining_payload);
174+
}),
175+
route_msg.union_value);
176+
177+
return 0;
178+
}
179+
180+
int handlePipeEvent(const pipe::ClientPipe::Event::Disconnected)
181+
{
182+
for (auto& pair : tag_to_gateway_)
183+
{
184+
pair.second->event(detail::Gateway::Event::Disconnected{});
185+
}
186+
return 0;
187+
}
188+
189+
void handleRouteConnect(const RouteConnect_1_0&)
190+
{
191+
for (auto& pair : tag_to_gateway_)
192+
{
193+
pair.second->event(detail::Gateway::Event::Connected{});
194+
}
195+
}
196+
197+
void handleRouteChannelMsg(const RouteChannelMsg_1_0& route_channel_msg, pipe::ClientPipe::Payload payload)
198+
{
199+
const auto it = tag_to_gateway_.find(route_channel_msg.tag);
200+
if (it != tag_to_gateway_.end())
201+
{
202+
it->second->event(detail::Gateway::Event::Message{payload});
203+
}
204+
}
205+
206+
cetl::pmr::memory_resource& memory_;
112207
pipe::ClientPipe::Ptr client_pipe_;
113208
Tag next_tag_;
114209
std::unordered_map<Tag, detail::Gateway::Ptr> tag_to_gateway_;
@@ -117,9 +212,9 @@ class ClientRouterImpl final : public ClientRouter
117212

118213
} // namespace
119214

120-
ClientRouter::Ptr ClientRouter::make(pipe::ClientPipe::Ptr client_pipe)
215+
ClientRouter::Ptr ClientRouter::make(cetl::pmr::memory_resource& memory, pipe::ClientPipe::Ptr client_pipe)
121216
{
122-
return std::make_unique<ClientRouterImpl>(std::move(client_pipe));
217+
return std::make_unique<ClientRouterImpl>(memory, std::move(client_pipe));
123218
}
124219

125220
} // namespace ipc

‎src/common/ipc/client_router.hpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include "pipe/client_pipe.hpp"
1212

1313
#include <cetl/cetl.hpp>
14+
#include <cetl/pf17/cetlpf.hpp>
1415

1516
#include <memory>
1617

@@ -26,7 +27,7 @@ class ClientRouter
2627
public:
2728
using Ptr = std::unique_ptr<ClientRouter>;
2829

29-
static Ptr make(pipe::ClientPipe::Ptr client_pipe);
30+
static Ptr make(cetl::pmr::memory_resource& memory, pipe::ClientPipe::Ptr client_pipe);
3031

3132
ClientRouter(const ClientRouter&) = delete;
3233
ClientRouter(ClientRouter&&) noexcept = delete;
@@ -35,10 +36,13 @@ class ClientRouter
3536

3637
virtual ~ClientRouter() = default;
3738

39+
virtual void start() = 0;
40+
virtual cetl::pmr::memory_resource& memory() = 0;
41+
3842
template <typename Input, typename Output>
3943
CETL_NODISCARD Channel<Input, Output> makeChannel(AnyChannel::EventHandler<Input> event_handler)
4044
{
41-
return Channel<Input, Output>{makeGateway(), event_handler};
45+
return Channel<Input, Output>{memory(), makeGateway(), event_handler};
4246
}
4347

4448
protected:

‎src/daemon/engine/application.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55

66
#include "application.hpp"
77

8+
#include "ipc/pipe/server_pipe.hpp"
9+
810
#include <cetl/pf17/cetlpf.hpp>
911
#include <cetl/visit_helpers.hpp>
1012
#include <libcyphal/application/node.hpp>

‎src/sdk/daemon.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
#include <ocvsmd/sdk/daemon.hpp>
77

8+
#include "ipc/pipe/client_pipe.hpp"
89
#include "ipc/pipe/unix_socket_client.hpp"
910

1011
#include <cetl/pf17/cetlpf.hpp>

0 commit comments

Comments
 (0)