Optimize kernel memory block lookup and consolidate RBTree implementations (#3410)

* Implement intrusive red-black tree, use it for HLE kernel block manager

* Implement TreeDictionary using IntrusiveRedBlackTree

* Implement IntervalTree using IntrusiveRedBlackTree

* Implement IntervalTree (on Ryujinx.Memory) using IntrusiveRedBlackTree

* Make PredecessorOf and SuccessorOf internal, expose Predecessor and Successor properties on the node itself

* Allocation free tree node lookup

1 files changed, 28 insertions, 372 deletions

diff --git a/Ryujinx.Common/Collections/TreeDictionary.cs b/Ryujinx.Common/Collections/TreeDictionary.cs
index 108fa773..a5a3b818 100644
--- a/Ryujinx.Common/Collections/TreeDictionary.cs
+++ b/Ryujinx.Common/Collections/TreeDictionary.cs
@@ -10,14 +10,8 @@ namespace Ryujinx.Common.Collections
     /// </summary>
     /// <typeparam name="K">Key</typeparam>
     /// <typeparam name="V">Value</typeparam>
-    public class TreeDictionary<K, V> : IDictionary<K, V> where K : IComparable<K>
+    public class TreeDictionary<K, V> : IntrusiveRedBlackTreeImpl<Node<K, V>>, IDictionary<K, V> where K : IComparable<K>
     {
-        private const bool Black = true;
-        private const bool Red = false;
-        private Node<K, V> _root = null;
-        private int _count = 0;
-        public TreeDictionary() { }
-
         #region Public Methods
 
         /// <summary>
@@ -57,7 +51,7 @@ namespace Ryujinx.Common.Collections
             {
                 throw new ArgumentNullException(nameof(key));
             }
-            if (null == value)
+            if (value == null)
             {
                 throw new ArgumentNullException(nameof(value));
             }
@@ -78,7 +72,7 @@ namespace Ryujinx.Common.Collections
             }
             if (Delete(key) != null)
             {
-                _count--;
+                Count--;
             }
         }
 
@@ -160,13 +154,12 @@ namespace Ryujinx.Common.Collections
 
             Queue<Node<K, V>> nodes = new Queue<Node<K, V>>();
 
-            if (this._root != null)
+            if (this.Root != null)
             {
-                nodes.Enqueue(this._root);
+                nodes.Enqueue(this.Root);
             }
-            while (nodes.Count > 0)
+            while (nodes.TryDequeue(out Node<K, V> node))
             {
-                Node<K, V> node = nodes.Dequeue();
                 list.Add(new KeyValuePair<K, V>(node.Key, node.Value));
                 if (node.Left != null)
                 {
@@ -188,7 +181,7 @@ namespace Ryujinx.Common.Collections
         {
             List<KeyValuePair<K, V>> list = new List<KeyValuePair<K, V>>();
 
-            AddToList(_root, list);
+            AddToList(Root, list);
 
             return list;
         }
@@ -229,7 +222,7 @@ namespace Ryujinx.Common.Collections
                 throw new ArgumentNullException(nameof(key));
             }
 
-            Node<K, V> node = _root;
+            Node<K, V> node = Root;
             while (node != null)
             {
                 int cmp = key.CompareTo(node.Key);
@@ -275,7 +268,7 @@ namespace Ryujinx.Common.Collections
         private Node<K, V> BSTInsert(K key, V value)
         {
             Node<K, V> parent = null;
-            Node<K, V> node = _root;
+            Node<K, V> node = Root;
 
             while (node != null)
             {
@@ -298,7 +291,7 @@ namespace Ryujinx.Common.Collections
             Node<K, V> newNode = new Node<K, V>(key, value, parent);
             if (newNode.Parent == null)
             {
-                _root = newNode;
+                Root = newNode;
             }
             else if (key.CompareTo(parent.Key) < 0)
             {
@@ -308,7 +301,7 @@ namespace Ryujinx.Common.Collections
             {
                 parent.Right = newNode;
             }
-            _count++;
+            Count++;
             return newNode;
         }
 
@@ -344,9 +337,8 @@ namespace Ryujinx.Common.Collections
 
             if (ParentOf(replacementNode) == null)
             {
-                _root = tmp;
+                Root = tmp;
             }
-
             else if (replacementNode == LeftOf(ParentOf(replacementNode)))
             {
                 ParentOf(replacementNode).Left = tmp;
@@ -371,43 +363,6 @@ namespace Ryujinx.Common.Collections
         }
 
         /// <summary>
-        /// Returns the node with the largest key where <paramref name="node"/> is considered the root node.
-        /// </summary>
-        /// <param name="node">Root Node</param>
-        /// <returns>Node with the maximum key in the tree of <paramref name="node"/></returns>
-        private static Node<K, V> Maximum(Node<K, V> node)
-        {
-            Node<K, V> tmp = node;
-            while (tmp.Right != null)
-            {
-                tmp = tmp.Right;
-            }
-
-            return tmp;
-        }
-
-        /// <summary>
-        /// Returns the node with the smallest key where <paramref name="node"/> is considered the root node.
-        /// </summary>
-        /// <param name="node">Root Node</param>
-        /// <returns>Node with the minimum key in the tree of <paramref name="node"/></returns>
-        ///<exception cref="ArgumentNullException"><paramref name="node"/> is null</exception>
-        private static Node<K, V> Minimum(Node<K, V> node)
-        {
-            if (node == null)
-            {
-                throw new ArgumentNullException(nameof(node));
-            }
-            Node<K, V> tmp = node;
-            while (tmp.Left != null)
-            {
-                tmp = tmp.Left;
-            }
-
-            return tmp;
-        }
-
-        /// <summary>
         /// Returns the node whose key immediately less than or equal to <paramref name="key"/>.
         /// </summary>
         /// <param name="key">Key for which to find the floor node of</param>
@@ -419,7 +374,7 @@ namespace Ryujinx.Common.Collections
             {
                 throw new ArgumentNullException(nameof(key));
             }
-            Node<K, V> tmp = _root;
+            Node<K, V> tmp = Root;
 
             while (tmp != null)
             {
@@ -473,7 +428,7 @@ namespace Ryujinx.Common.Collections
             {
                 throw new ArgumentNullException(nameof(key));
             }
-            Node<K, V> tmp = _root;
+            Node<K, V> tmp = Root;
 
             while (tmp != null)
             {
@@ -515,294 +470,6 @@ namespace Ryujinx.Common.Collections
             return null;
         }
 
-        /// <summary>
-        /// Finds the node with the key is immediately greater than <paramref name="node"/>.
-        /// </summary>
-        /// <param name="node">Node to find the successor of</param>
-        /// <returns>Successor of <paramref name="node"/></returns>
-        private static Node<K, V> SuccessorOf(Node<K, V> node)
-        {
-            if (node.Right != null)
-            {
-                return Minimum(node.Right);
-            }
-            Node<K, V> parent = node.Parent;
-            while (parent != null && node == parent.Right)
-            {
-                node = parent;
-                parent = parent.Parent;
-            }
-            return parent;
-        }
-
-        /// <summary>
-        /// Finds the node whose key is immediately less than <paramref name="node"/>.
-        /// </summary>
-        /// <param name="node">Node to find the predecessor of</param>
-        /// <returns>Predecessor of <paramref name="node"/></returns>
-        private static Node<K, V> PredecessorOf(Node<K, V> node)
-        {
-            if (node.Left != null)
-            {
-                return Maximum(node.Left);
-            }
-            Node<K, V> parent = node.Parent;
-            while (parent != null && node == parent.Left)
-            {
-                node = parent;
-                parent = parent.Parent;
-            }
-            return parent;
-        }
-
-        #endregion
-
-        #region Private Methods (RBL)
-
-        private void RestoreBalanceAfterRemoval(Node<K, V> balanceNode)
-        {
-            Node<K, V> ptr = balanceNode;
-
-            while (ptr != _root && ColorOf(ptr) == Black)
-            {
-                if (ptr == LeftOf(ParentOf(ptr)))
-                {
-                    Node<K, V> sibling = RightOf(ParentOf(ptr));
-
-                    if (ColorOf(sibling) == Red)
-                    {
-                        SetColor(sibling, Black);
-                        SetColor(ParentOf(ptr), Red);
-                        RotateLeft(ParentOf(ptr));
-                        sibling = RightOf(ParentOf(ptr));
-                    }
-                    if (ColorOf(LeftOf(sibling)) == Black && ColorOf(RightOf(sibling)) == Black)
-                    {
-                        SetColor(sibling, Red);
-                        ptr = ParentOf(ptr);
-                    }
-                    else
-                    {
-                        if (ColorOf(RightOf(sibling)) == Black)
-                        {
-                            SetColor(LeftOf(sibling), Black);
-                            SetColor(sibling, Red);
-                            RotateRight(sibling);
-                            sibling = RightOf(ParentOf(ptr));
-                        }
-                        SetColor(sibling, ColorOf(ParentOf(ptr)));
-                        SetColor(ParentOf(ptr), Black);
-                        SetColor(RightOf(sibling), Black);
-                        RotateLeft(ParentOf(ptr));
-                        ptr = _root;
-                    }
-                }
-                else
-                {
-                    Node<K, V> sibling = LeftOf(ParentOf(ptr));
-
-                    if (ColorOf(sibling) == Red)
-                    {
-                        SetColor(sibling, Black);
-                        SetColor(ParentOf(ptr), Red);
-                        RotateRight(ParentOf(ptr));
-                        sibling = LeftOf(ParentOf(ptr));
-                    }
-                    if (ColorOf(RightOf(sibling)) == Black && ColorOf(LeftOf(sibling)) == Black)
-                    {
-                        SetColor(sibling, Red);
-                        ptr = ParentOf(ptr);
-                    }
-                    else
-                    {
-                        if (ColorOf(LeftOf(sibling)) == Black)
-                        {
-                            SetColor(RightOf(sibling), Black);
-                            SetColor(sibling, Red);
-                            RotateLeft(sibling);
-                            sibling = LeftOf(ParentOf(ptr));
-                        }
-                        SetColor(sibling, ColorOf(ParentOf(ptr)));
-                        SetColor(ParentOf(ptr), Black);
-                        SetColor(LeftOf(sibling), Black);
-                        RotateRight(ParentOf(ptr));
-                        ptr = _root;
-                    }
-                }
-            }
-            SetColor(ptr, Black);
-        }
-
-        private void RestoreBalanceAfterInsertion(Node<K, V> balanceNode)
-        {
-            SetColor(balanceNode, Red);
-            while (balanceNode != null && balanceNode != _root && ColorOf(ParentOf(balanceNode)) == Red)
-            {
-                if (ParentOf(balanceNode) == LeftOf(ParentOf(ParentOf(balanceNode))))
-                {
-                    Node<K, V> sibling = RightOf(ParentOf(ParentOf(balanceNode)));
-
-                    if (ColorOf(sibling) == Red)
-                    {
-                        SetColor(ParentOf(balanceNode), Black);
-                        SetColor(sibling, Black);
-                        SetColor(ParentOf(ParentOf(balanceNode)), Red);
-                        balanceNode = ParentOf(ParentOf(balanceNode));
-                    }
-                    else
-                    {
-                        if (balanceNode == RightOf(ParentOf(balanceNode)))
-                        {
-                            balanceNode = ParentOf(balanceNode);
-                            RotateLeft(balanceNode);
-                        }
-                        SetColor(ParentOf(balanceNode), Black);
-                        SetColor(ParentOf(ParentOf(balanceNode)), Red);
-                        RotateRight(ParentOf(ParentOf(balanceNode)));
-                    }
-                }
-                else
-                {
-                    Node<K, V> sibling = LeftOf(ParentOf(ParentOf(balanceNode)));
-
-                    if (ColorOf(sibling) == Red)
-                    {
-                        SetColor(ParentOf(balanceNode), Black);
-                        SetColor(sibling, Black);
-                        SetColor(ParentOf(ParentOf(balanceNode)), Red);
-                        balanceNode = ParentOf(ParentOf(balanceNode));
-                    }
-                    else
-                    {
-                        if (balanceNode == LeftOf(ParentOf(balanceNode)))
-                        {
-                            balanceNode = ParentOf(balanceNode);
-                            RotateRight(balanceNode);
-                        }
-                        SetColor(ParentOf(balanceNode), Black);
-                        SetColor(ParentOf(ParentOf(balanceNode)), Red);
-                        RotateLeft(ParentOf(ParentOf(balanceNode)));
-                    }
-                }
-            }
-            SetColor(_root, Black);
-        }
-
-        private void RotateLeft(Node<K, V> node)
-        {
-            if (node != null)
-            {
-                Node<K, V> right = RightOf(node);
-                node.Right = LeftOf(right);
-                if (LeftOf(right) != null)
-                {
-                    LeftOf(right).Parent = node;
-                }
-                right.Parent = ParentOf(node);
-                if (ParentOf(node) == null)
-                {
-                    _root = right;
-                }
-                else if (node == LeftOf(ParentOf(node)))
-                {
-                    ParentOf(node).Left = right;
-                }
-                else
-                {
-                    ParentOf(node).Right = right;
-                }
-                right.Left = node;
-                node.Parent = right;
-            }
-        }
-
-        private void RotateRight(Node<K, V> node)
-        {
-            if (node != null)
-            {
-                Node<K, V> left = LeftOf(node);
-                node.Left = RightOf(left);
-                if (RightOf(left) != null)
-                {
-                    RightOf(left).Parent = node;
-                }
-                left.Parent = node.Parent;
-                if (ParentOf(node) == null)
-                {
-                    _root = left;
-                }
-                else if (node == RightOf(ParentOf(node)))
-                {
-                    ParentOf(node).Right = left;
-                }
-                else
-                {
-                    ParentOf(node).Left = left;
-                }
-                left.Right = node;
-                node.Parent = left;
-            }
-        }
-        #endregion
-
-        #region Safety-Methods
-
-        // These methods save memory by allowing us to forego sentinel nil nodes, as well as serve as protection against NullReferenceExceptions.
-
-        /// <summary>
-        /// Returns the color of <paramref name="node"/>, or Black if it is null.
-        /// </summary>
-        /// <param name="node">Node</param>
-        /// <returns>The boolean color of <paramref name="node"/>, or black if null</returns>
-        private static bool ColorOf(Node<K, V> node)
-        {
-            return node == null || node.Color;
-        }
-
-        /// <summary>
-        /// Sets the color of <paramref name="node"/> node to <paramref name="color"/>.
-        /// <br></br>
-        /// This method does nothing if <paramref name="node"/> is null.
-        /// </summary>
-        /// <param name="node">Node to set the color of</param>
-        /// <param name="color">Color (Boolean)</param>
-        private static void SetColor(Node<K, V> node, bool color)
-        {
-            if (node != null)
-            {
-                node.Color = color;
-            }
-        }
-
-        /// <summary>
-        /// This method returns the left node of <paramref name="node"/>, or null if <paramref name="node"/> is null.
-        /// </summary>
-        /// <param name="node">Node to retrieve the left child from</param>
-        /// <returns>Left child of <paramref name="node"/></returns>
-        private static Node<K, V> LeftOf(Node<K, V> node)
-        {
-            return node?.Left;
-        }
-
-        /// <summary>
-        /// This method returns the right node of <paramref name="node"/>, or null if <paramref name="node"/> is null.
-        /// </summary>
-        /// <param name="node">Node to retrieve the right child from</param>
-        /// <returns>Right child of <paramref name="node"/></returns>
-        private static Node<K, V> RightOf(Node<K, V> node)
-        {
-            return node?.Right;
-        }
-
-        /// <summary>
-        /// Returns the parent node of <paramref name="node"/>, or null if <paramref name="node"/> is null.
-        /// </summary>
-        /// <param name="node">Node to retrieve the parent from</param>
-        /// <returns>Parent of <paramref name="node"/></returns>
-        private static Node<K, V> ParentOf(Node<K, V> node)
-        {
-            return node?.Parent;
-        }
         #endregion
 
         #region Interface Implementations
@@ -819,9 +486,9 @@ namespace Ryujinx.Common.Collections
 
         bool IDictionary<K, V>.Remove(K key)
         {
-            int count = _count;
+            int count = Count;
             Remove(key);
-            return count > _count;
+            return count > Count;
         }
 
         public bool TryGetValue(K key, [MaybeNullWhen(false)] out V value)
@@ -845,12 +512,6 @@ namespace Ryujinx.Common.Collections
             Add(item.Key, item.Value);
         }
 
-        public void Clear()
-        {
-            _root = null;
-            _count = 0;
-        }
-
         public bool Contains(KeyValuePair<K, V> item)
         {
             if (item.Key == null)
@@ -895,9 +556,9 @@ namespace Ryujinx.Common.Collections
 
             if (node.Value.Equals(item.Value))
             {
-                int count = _count;
+                int count = Count;
                 Remove(item.Key);
-                return count > _count;
+                return count > Count;
             }
 
             return false;
@@ -913,8 +574,6 @@ namespace Ryujinx.Common.Collections
             return GetKeyValues().GetEnumerator();
         }
 
-        public int Count => _count;
-
         public ICollection<K> Keys => GetKeyValues().Keys;
 
         public ICollection<V> Values => GetKeyValues().Values;
@@ -928,6 +587,7 @@ namespace Ryujinx.Common.Collections
         }
 
         #endregion
+
         #region Private Interface Helper Methods
 
         /// <summary>
@@ -938,14 +598,13 @@ namespace Ryujinx.Common.Collections
         {
             SortedList<K, V> set = new SortedList<K, V>();
             Queue<Node<K, V>> queue = new Queue<Node<K, V>>();
-            if (_root != null)
+            if (Root != null)
             {
-                queue.Enqueue(_root);
+                queue.Enqueue(Root);
             }
 
-            while (queue.Count > 0)
+            while (queue.TryDequeue(out Node<K, V> node))
             {
-                Node<K, V> node = queue.Dequeue();
                 set.Add(node.Key, node.Value);
                 if (null != node.Left)
                 {
@@ -959,6 +618,7 @@ namespace Ryujinx.Common.Collections
 
             return set;
         }
+
         #endregion
     }
 
@@ -967,16 +627,12 @@ namespace Ryujinx.Common.Collections
     /// </summary>
     /// <typeparam name="K">Key of the node</typeparam>
     /// <typeparam name="V">Value of the node</typeparam>
-    class Node<K, V>
+    public class Node<K, V> : IntrusiveRedBlackTreeNode<Node<K, V>> where K : IComparable<K>
     {
-        public bool Color = true;
-        public Node<K, V> Left = null;
-        public Node<K, V> Right = null;
-        public Node<K, V> Parent = null;
-        public K Key;
-        public V Value;
-
-        public Node(K key, V value, Node<K, V> parent)
+        internal K Key;
+        internal V Value;
+
+        internal Node(K key, V value, Node<K, V> parent)
         {
             Key = key;
             Value = value;