From d2b3832860b63313e76ee5d62ce9942cea32affe Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 31 Mar 2014 00:32:52 +0400
Subject: [PATCH] Proper priority comparison for CAN frames of different types

---
 libuavcan/include/uavcan/driver/can.hpp     | 17 +++---
 libuavcan/src/driver/can.cpp                | 39 ++++++++++++++
 libuavcan/test/transport/can/can_driver.cpp | 57 +++++++++++++++++++++
 3 files changed, 103 insertions(+), 10 deletions(-)

diff --git a/libuavcan/include/uavcan/driver/can.hpp b/libuavcan/include/uavcan/driver/can.hpp
index 0c9236e8a8..cecc56afc2 100644
--- a/libuavcan/include/uavcan/driver/can.hpp
+++ b/libuavcan/include/uavcan/driver/can.hpp
@@ -61,16 +61,13 @@ struct CanFrame
     enum StringRepresentation { StrTight, StrAligned };
     std::string toString(StringRepresentation mode = StrTight) const;
 
-    // TODO: priority comparison for EXT vs STD frames
-    bool priorityHigherThan(const CanFrame& rhs) const
-    {
-        return (id & CanFrame::MaskExtID) < (rhs.id & CanFrame::MaskExtID);
-    }
-
-    bool priorityLowerThan(const CanFrame& rhs) const
-    {
-        return (id & CanFrame::MaskExtID) > (rhs.id & CanFrame::MaskExtID);
-    }
+    /**
+     * CAN frames arbitration rules, particularly STD vs EXT:
+     *     Marco Di Natale - "Understanding and using the Controller Area Network"
+     *     http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
+     */
+    bool priorityHigherThan(const CanFrame& rhs) const;
+    bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); }
 };
 UAVCAN_PACKED_END
 
diff --git a/libuavcan/src/driver/can.cpp b/libuavcan/src/driver/can.cpp
index a9be9108f1..75d97c2cc3 100644
--- a/libuavcan/src/driver/can.cpp
+++ b/libuavcan/src/driver/can.cpp
@@ -17,6 +17,45 @@ const uint32_t CanFrame::FlagRTR;
 const uint32_t CanFrame::FlagERR;
 const uint8_t CanFrame::MaxDataLen;
 
+bool CanFrame::priorityHigherThan(const CanFrame& rhs) const
+{
+    const uint32_t clean_id     = id     & MaskExtID;
+    const uint32_t rhs_clean_id = rhs.id & MaskExtID;
+
+    /*
+     * STD vs EXT - if 11 most significant bits are the same, EXT loses.
+     */
+    const bool ext     = id     & FlagEFF;
+    const bool rhs_ext = rhs.id & FlagEFF;
+    if (ext != rhs_ext)
+    {
+        const uint32_t arb11     = ext     ? (clean_id >> 18)     : clean_id;
+        const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
+        if (arb11 != rhs_arb11)
+        {
+            return arb11 < rhs_arb11;
+        }
+        else
+        {
+            return rhs_ext;
+        }
+    }
+
+    /*
+     * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
+     */
+    const bool rtr     = id     & FlagRTR;
+    const bool rhs_rtr = rhs.id & FlagRTR;
+    if (clean_id == rhs_clean_id && rtr != rhs_rtr)
+    {
+        return rhs_rtr;
+    }
+
+    /*
+     * Plain ID arbitration - greater value loses.
+     */
+    return clean_id < rhs_clean_id;
+}
 
 std::string CanFrame::toString(StringRepresentation mode) const
 {
diff --git a/libuavcan/test/transport/can/can_driver.cpp b/libuavcan/test/transport/can/can_driver.cpp
index c76036f46f..c6fcfccfeb 100644
--- a/libuavcan/test/transport/can/can_driver.cpp
+++ b/libuavcan/test/transport/can/can_driver.cpp
@@ -21,6 +21,63 @@ TEST(CanFrame, FrameProperties)
     EXPECT_TRUE(frame.isErrorFrame());
 }
 
+TEST(CanFrame, Arbitration)
+{
+    using uavcan::CanFrame;
+
+    CanFrame a;
+    CanFrame b;
+
+    /*
+     * Simple
+     */
+    a.id = 123;
+    b.id = 122;
+    ASSERT_TRUE(a.priorityLowerThan(b));
+    ASSERT_TRUE(b.priorityHigherThan(a));
+
+    a.id = 123 | CanFrame::FlagEFF;
+    b.id = 122 | CanFrame::FlagEFF;
+    ASSERT_TRUE(a.priorityLowerThan(b));
+    ASSERT_TRUE(b.priorityHigherThan(a));
+
+    a.id = 8;
+    b.id = 8;
+    ASSERT_FALSE(a.priorityLowerThan(b));
+    ASSERT_FALSE(b.priorityHigherThan(a));
+
+    /*
+     * EXT vs STD
+     */
+    a.id = 1000;                      // 1000
+    b.id = 2000 | CanFrame::FlagEFF;  // 2000 >> 18, wins
+    ASSERT_TRUE(a.priorityLowerThan(b));
+    ASSERT_TRUE(b.priorityHigherThan(a));
+
+    a.id = 0x400;
+    b.id = 0x10000000 | CanFrame::FlagEFF; // (0x400 << 18), 11 most significant bits are the same --> EFF loses
+    ASSERT_TRUE(a.priorityHigherThan(b));
+    ASSERT_TRUE(b.priorityLowerThan(a));
+
+    /*
+     * RTR vs Data
+     */
+    a.id = 123 | CanFrame::FlagRTR;      // On the same ID, RTR loses
+    b.id = 123;
+    ASSERT_TRUE(a.priorityLowerThan(b));
+    ASSERT_TRUE(b.priorityHigherThan(a));
+
+    a.id = CanFrame::MaskStdID | CanFrame::FlagRTR;  // RTR is STD, so it wins
+    b.id = CanFrame::MaskExtID | CanFrame::FlagEFF;  // Lowest possible priority for data frame
+    ASSERT_TRUE(a.priorityHigherThan(b));
+    ASSERT_TRUE(b.priorityLowerThan(a));
+
+    a.id = 123 | CanFrame::FlagRTR;      // Both RTR arbitrate as usually
+    b.id = 122 | CanFrame::FlagRTR;
+    ASSERT_TRUE(a.priorityLowerThan(b));
+    ASSERT_TRUE(b.priorityHigherThan(a));
+}
+
 TEST(CanFrame, ToString)
 {
     uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04" "1234", EXT);