From ec978d97e028a538598f4b08b2e018d4912f0f7e Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Wed, 12 Jun 2013 11:29:12 +0200
Subject: score: Add _Chain_Insert_ordered_unprotected()

---
 cpukit/score/inline/rtems/score/chain.inl | 41 +++++++++++++++++++++++++++++++
 testsuites/sptests/spchain/init.c         | 34 +++++++++++++++++++++++++
 testsuites/sptests/spchain/spchain.doc    |  1 +
 testsuites/sptests/spchain/spchain.scn    |  1 +
 4 files changed, 77 insertions(+)

diff --git a/cpukit/score/inline/rtems/score/chain.inl b/cpukit/score/inline/rtems/score/chain.inl
index 7290d6ecac..190eedf8d6 100644
--- a/cpukit/score/inline/rtems/score/chain.inl
+++ b/cpukit/score/inline/rtems/score/chain.inl
@@ -726,6 +726,47 @@ RTEMS_INLINE_ROUTINE bool _Chain_Get_with_empty_check_unprotected(
   return is_empty_now;
 }
 
+/**
+ * @brief Chain node order.
+ *
+ * @param[in] left The left node.
+ * @param[in] right The right node.
+ *
+ * @retval true According to the order the left node precedes the right node.
+ * @retval false Otherwise.
+ */
+typedef bool ( *Chain_Node_order )(
+  const Chain_Node *left,
+  const Chain_Node *right
+);
+
+/**
+ * @brief Inserts a node into the chain according to the order relation.
+ *
+ * After the operation the chain contains the node to insert and the order
+ * relation holds for all nodes from the head up to the inserted node.  Nodes
+ * after the inserted node are not moved.
+ *
+ * @param[in/out] chain The chain.
+ * @param[in/out] to_insert The node to insert.
+ * @param[in] order The order relation.
+ */
+RTEMS_INLINE_ROUTINE void _Chain_Insert_ordered_unprotected(
+  Chain_Control *chain,
+  Chain_Node *to_insert,
+  Chain_Node_order order
+)
+{
+  const Chain_Node *tail = _Chain_Immutable_tail( chain );
+  Chain_Node *next = _Chain_First( chain );
+
+  while ( next != tail && !( *order )( to_insert, next ) ) {
+    next = _Chain_Next( next );
+  }
+
+  _Chain_Insert_unprotected( _Chain_Previous( next ), to_insert );
+}
+
 /** @} */
 
 #endif
diff --git a/testsuites/sptests/spchain/init.c b/testsuites/sptests/spchain/init.c
index f8d54b0fa4..72895b6748 100644
--- a/testsuites/sptests/spchain/init.c
+++ b/testsuites/sptests/spchain/init.c
@@ -218,6 +218,39 @@ static void test_chain_node_count(void)
   }
 }
 
+static bool test_order( const Chain_Node *left, const Chain_Node *right )
+{
+  return left < right;
+}
+
+static void test_chain_insert_ordered( void )
+{
+  Chain_Control chain = CHAIN_INITIALIZER_EMPTY(chain);
+  Chain_Node nodes[5];
+  const Chain_Node *tail;
+  const Chain_Node *node;
+  size_t n = RTEMS_ARRAY_SIZE( nodes );
+  size_t i = 0;
+
+  puts( "INIT - Verify _Chain_Insert_ordered_unprotected" );
+
+  _Chain_Insert_ordered_unprotected( &chain, &nodes[4], test_order );
+  _Chain_Insert_ordered_unprotected( &chain, &nodes[2], test_order );
+  _Chain_Insert_ordered_unprotected( &chain, &nodes[0], test_order );
+  _Chain_Insert_ordered_unprotected( &chain, &nodes[3], test_order );
+  _Chain_Insert_ordered_unprotected( &chain, &nodes[1], test_order );
+
+  tail = _Chain_Immutable_tail( &chain );
+  node = _Chain_Immutable_first( &chain );
+  while ( node != tail && i < n ) {
+    rtems_test_assert( node == &nodes[ i ] );
+    ++i;
+    node = _Chain_Immutable_next( node );
+  }
+
+  rtems_test_assert( i == n );
+}
+
 rtems_task Init(
   rtems_task_argument ignored
 )
@@ -260,6 +293,7 @@ rtems_task Init(
   test_chain_control_layout();
   test_chain_control_initializer();
   test_chain_node_count();
+  test_chain_insert_ordered();
 
   puts( "*** END OF RTEMS CHAIN API TEST ***" );
   rtems_test_exit(0);
diff --git a/testsuites/sptests/spchain/spchain.doc b/testsuites/sptests/spchain/spchain.doc
index 29c9d1045c..8cf80892a5 100644
--- a/testsuites/sptests/spchain/spchain.doc
+++ b/testsuites/sptests/spchain/spchain.doc
@@ -24,6 +24,7 @@ directives:
   rtems_chain_get_with_notification
   rtems_chain_get_with_wait
   rtems_chain_node_count_unprotected
+  _Chain_Insert_ordered_unprotected
 
 concepts:
 
diff --git a/testsuites/sptests/spchain/spchain.scn b/testsuites/sptests/spchain/spchain.scn
index 9cf4d5212f..39f3795e20 100644
--- a/testsuites/sptests/spchain/spchain.scn
+++ b/testsuites/sptests/spchain/spchain.scn
@@ -13,4 +13,5 @@ INIT - Verify rtems_chain_get_with_wait
 INIT - Verify rtems_chain_control layout
 INIT - Verify rtems_chain_control initializer
 INIT - Verify rtems_chain_node_count_unprotected
+INIT - Verify _Chain_Insert_ordered_unprotected
 *** END OF RTEMS CHAIN API TEST ***
-- 
cgit v1.2.3