From c9c9a16e94e0922f431f144c8bbbe76b841823d9 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 12 Jul 2014 16:52:17 +0400
Subject: [PATCH] libuavcan passive mode support

---
 libuavcan/include/uavcan/error.hpp            |  1 +
 .../include/uavcan/node/abstract_node.hpp     |  2 ++
 .../include/uavcan/transport/dispatcher.hpp   |  2 ++
 .../src/protocol/uc_node_status_provider.cpp  | 16 +++++++++---
 .../src/transport/uc_transfer_sender.cpp      |  5 ++++
 libuavcan/test/transport/transfer_sender.cpp  | 25 +++++++++++++++++++
 6 files changed, 47 insertions(+), 4 deletions(-)

diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp
index 9969135dea..5cf9129684 100644
--- a/libuavcan/include/uavcan/error.hpp
+++ b/libuavcan/include/uavcan/error.hpp
@@ -26,6 +26,7 @@ const int16_t ErrInvalidTransferListener = 7;
 const int16_t ErrNotInited               = 8;
 const int16_t ErrRecursiveCall           = 9;
 const int16_t ErrLogic                   = 10;
+const int16_t ErrPassiveMode             = 11;
 
 }
 
diff --git a/libuavcan/include/uavcan/node/abstract_node.hpp b/libuavcan/include/uavcan/node/abstract_node.hpp
index 6056cb7ed2..2c57d4244b 100644
--- a/libuavcan/include/uavcan/node/abstract_node.hpp
+++ b/libuavcan/include/uavcan/node/abstract_node.hpp
@@ -34,6 +34,8 @@ public:
         return getScheduler().getDispatcher().setNodeID(nid);
     }
 
+    bool isPassiveMode() const { return getScheduler().getDispatcher().isPassiveMode(); }
+
     int spin(MonotonicTime deadline)
     {
         return getScheduler().spin(deadline);
diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp
index 48b73b6744..a36700c30c 100644
--- a/libuavcan/include/uavcan/transport/dispatcher.hpp
+++ b/libuavcan/include/uavcan/transport/dispatcher.hpp
@@ -141,6 +141,8 @@ public:
     NodeID getNodeID() const { return self_node_id_; }
     bool setNodeID(NodeID nid);
 
+    bool isPassiveMode() const { return !getNodeID().isUnicast(); }
+
     const ISystemClock& getSystemClock() const { return sysclock_; }
     ISystemClock& getSystemClock() { return sysclock_; }
 
diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp
index cc76c08c89..c8d6b04816 100644
--- a/libuavcan/src/protocol/uc_node_status_provider.cpp
+++ b/libuavcan/src/protocol/uc_node_status_provider.cpp
@@ -62,10 +62,15 @@ int NodeStatusProvider::startAndPublish()
         return -ErrNotInited;
     }
 
-    int res = publish(); // Initial broadcast
-    if (res < 0)
+    int res = -1;
+
+    if (!getNode().isPassiveMode())
     {
-        goto fail;
+        res = publish(); // Initial broadcast
+        if (res < 0)
+        {
+            goto fail;
+        }
     }
 
     res = gdr_sub_.start(GlobalDiscoveryRequestCallback(this, &NodeStatusProvider::handleGlobalDiscoveryRequest));
@@ -80,7 +85,10 @@ int NodeStatusProvider::startAndPublish()
         goto fail;
     }
 
-    TimerBase::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
+    if (!getNode().isPassiveMode())
+    {
+        TimerBase::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
+    }
 
     return res;
 
diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp
index caf6791dad..712e8202bd 100644
--- a/libuavcan/src/transport/uc_transfer_sender.cpp
+++ b/libuavcan/src/transport/uc_transfer_sender.cpp
@@ -19,6 +19,11 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
                          MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
                          TransferID tid)
 {
+    if (dispatcher_.isPassiveMode())
+    {
+        return -ErrPassiveMode;
+    }
+
     dispatcher_.getTransferPerfCounter().addTxTransfer();
 
     Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid);
diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp
index 46898fc313..c940e6fb07 100644
--- a/libuavcan/test/transport/transfer_sender.cpp
+++ b/libuavcan/test/transport/transfer_sender.cpp
@@ -218,3 +218,28 @@ TEST(TransferSender, Loopback)
     EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount());
     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
 }
+
+TEST(TransferSender, PassiveMode)
+{
+    uavcan::PoolManager<1> poolmgr;
+
+    SystemClockMock clockmock(100);
+    CanDriverMock driver(2, clockmock);
+
+    uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
+
+    uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock, out_trans_reg);
+
+    uavcan::TransferSender sender(dispatcher, makeDataType(uavcan::DataTypeKindMessage, 123),
+                                  uavcan::CanTxQueue::Volatile);
+
+    static const uint8_t Payload[] = {1, 2, 3, 4, 5};
+
+    ASSERT_EQ(-uavcan::ErrPassiveMode,
+              sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+                          uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast));
+
+    EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
+    EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
+    EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
+}