diff --git a/includes/menu.inc b/includes/menu.inc
index dfc5d32..15295b2 100644
--- a/includes/menu.inc
+++ b/includes/menu.inc
@@ -370,6 +370,7 @@ function menu_get_item($path = NULL, $router_item = NULL) {
       ->execute()->fetchAssoc();
     if ($router_item) {
       $map = _menu_translate($router_item, $original_map);
+      $router_item['original_map'] = $original_map;
       if ($map === FALSE) {
         $router_items[$path] = FALSE;
         return FALSE;
@@ -843,13 +844,16 @@ function menu_tree_output($tree) {
  * @return
  *   An tree of menu links in an array, in the order they should be rendered.
  */
-function menu_tree_all_data($menu_name, $item = NULL) {
+function menu_tree_all_data($menu_name, $item = NULL, $max_depth = NULL) {
   $tree = &drupal_static(__FUNCTION__, array());
 
   // Use $mlid as a flag for whether the data being loaded is for the whole tree.
   $mlid = isset($item['mlid']) ? $item['mlid'] : 0;
+  if (isset($max_depth)) {
+    $max_depth = min($max_depth, MENU_MAX_DEPTH);
+  }
   // Generate a cache ID (cid) specific for this $menu_name and $item.
-  $cid = 'links:' . $menu_name . ':all-cid:' . $mlid;
+  $cid = 'links:' . $menu_name . ':all-cid:' . $mlid . ':' . (int)$max_depth;
 
   if (!isset($tree[$cid])) {
     // If the static variable doesn't have the data, check {cache_menu}.
@@ -885,7 +889,9 @@ function menu_tree_all_data($menu_name, $item = NULL) {
         $query->orderBy('p' . $i, 'ASC');
       }
       $query->condition('ml.menu_name', $menu_name);
-
+      if (isset($max_depth)) {
+        $query->condition('ml.depth', $max_depth, '<=');
+      }
       if ($mlid) {
         // The tree is for a single item, so we need to match the values in its
         // p columns and 0 (the top level) with the plid values of other links.
@@ -905,7 +911,13 @@ function menu_tree_all_data($menu_name, $item = NULL) {
       // Select the links from the table, and recursively build the tree. We
       // LEFT JOIN since there is no match in {menu_router} for an external
       // link.
-      $data['tree'] = menu_tree_data($query->execute(), $parents);
+      $result = $query->execute();
+      $result->setFetchMode(PDO::FETCH_ASSOC);
+      $links = array();
+      foreach ($result as $item) {
+        $links[] = $item;
+      }
+      $data['tree'] = menu_tree_data($links, $parents);
       $data['node_links'] = array();
       menu_tree_collect_node_links($data['tree'], $data['node_links']);
       // Cache the data, if it is not already in the cache.
@@ -939,13 +951,16 @@ function menu_tree_all_data($menu_name, $item = NULL) {
  *   submenu below the link if there is one, and it is a subtree that has the
  *   same structure described for the top-level array.
  */
-function menu_tree_page_data($menu_name) {
+function menu_tree_page_data($menu_name, $max_depth = NULL) {
   $tree = &drupal_static(__FUNCTION__, array());
 
   // Load the menu item corresponding to the current page.
   if ($item = menu_get_item()) {
+    if (isset($max_depth)) {
+      $max_depth = min($max_depth, MENU_MAX_DEPTH);
+    }
     // Generate a cache ID (cid) specific for this page.
-    $cid = 'links:' . $menu_name . ':page-cid:' . $item['href'] . ':' . (int)$item['access'];
+    $cid = 'links:' . $menu_name . ':page-cid:' . $item['href'] . ':' . (int)$item['access'] . ':' . (int)$max_depth;
 
     if (!isset($tree[$cid])) {
       // If the static variable doesn't have the data, check {cache_menu}.
@@ -1036,7 +1051,7 @@ function menu_tree_page_data($menu_name) {
         // LEFT JOIN since there is no match in {menu_router} for an external
         // link.
         $query = db_select('menu_links', 'ml');
-        $query->leftJoin('menu_router', 'm', 'm.path = ml.router_path');
+         $query->leftJoin('menu_router', 'm', 'm.path = ml.router_path');
         $query->fields('ml');
         $query->fields('m', array(
           'load_functions',
@@ -1056,7 +1071,17 @@ function menu_tree_page_data($menu_name) {
         }
         $query->condition('ml.menu_name', $menu_name);
         $query->condition('ml.plid', $args, 'IN');
-        $data['tree'] = menu_tree_data($query->execute(), $parents);
+        if (isset($max_depth)) {
+          $query->condition('ml.depth', $max_depth, '<=');
+        }
+
+        $result = $query->execute();
+        $result->setFetchMode(PDO::FETCH_ASSOC);
+        $links = array();
+        foreach ($result as $item) {
+          $links[] = $item;
+        }
+        $data['tree'] = menu_tree_data($links, $parents);
         $data['node_links'] = array();
         menu_tree_collect_node_links($data['tree'], $data['node_links']);
         // Cache the data, if it is not already in the cache.
@@ -1163,7 +1188,7 @@ function _menu_tree_check_access(&$tree) {
  * Build the data representing a menu tree.
  *
  * @param $result
- *   The database result.
+ *   An array of links (associative arrays) ordered by p1..p9.
  * @param $parents
  *   An array of the plid values that represent the path from the current page
  *   to the root of the menu tree.
@@ -1172,72 +1197,41 @@ function _menu_tree_check_access(&$tree) {
  * @return
  *   See menu_tree_page_data for a description of the data structure.
  */
-function menu_tree_data($result = NULL, $parents = array(), $depth = 1) {
-  list(, $tree) = _menu_tree_data($result, $parents, $depth);
-  return $tree;
+function menu_tree_data($result = array(), $parents = array(), $depth = 1) {
+  $result = array_reverse($result);
+  return _menu_tree_data($result, $parents, $depth);
 }
 
 /**
  * Recursive helper function to build the data representing a menu tree.
  *
- * The function is a bit complex because the rendering of an item depends on
- * the next menu item. So we are always rendering the element previously
- * processed not the current one.
+ * The function is a bit complex because the rendering of a link depends on
+ * the next menu link.
  */
-function _menu_tree_data($result, $parents, $depth, $previous_element = '') {
+function _menu_tree_data(&$links, $parents, $depth) {
   $remnant = NULL;
   $tree = array();
-  foreach ($result as $item) {
-    $item = is_object($item) ? get_object_vars($item) : $item;
+  while ($item = array_pop($links)) {
     // We need to determine if we're on the path to root so we can later build
     // the correct active trail and breadcrumb.
     $item['in_active_trail'] = in_array($item['mlid'], $parents);
-    // The current item is the first in a new submenu.
-    if ($item['depth'] > $depth) {
-      // _menu_tree returns an item and the menu tree structure.
-      list($item, $below) = _menu_tree_data($result, $parents, $item['depth'], $item);
-      if ($previous_element) {
-        $tree[$previous_element['mlid']] = array(
-          'link' => $previous_element,
-          'below' => $below,
-        );
-      }
-      else {
-        $tree = $below;
-      }
+    $next = end($links);
+    $tree[$item['mlid']] = array(
+      'link' => $item,
+      'below' => array(),
+    );
+    // The next item is the first in a new submenu.
+    if ($next && $next['depth'] > $depth) {
+      // _menu_tree_data returns the sub-tree.
+      $tree[$item['mlid']]['below'] = _menu_tree_data($links, $parents, $next['depth']);
       // We need to fall back one level.
-      if (!isset($item) || $item['depth'] < $depth) {
-        return array($item, $tree);
-      }
-      // This will be the link to be output in the next iteration.
-      $previous_element = $item;
-    }
-    // We are at the same depth, so we use the previous element.
-    elseif ($item['depth'] == $depth) {
-      if ($previous_element) {
-        // Only the first time.
-        $tree[$previous_element['mlid']] = array(
-          'link' => $previous_element,
-          'below' => FALSE,
-        );
-      }
-      // This will be the link to be output in the next iteration.
-      $previous_element = $item;
     }
-    // The submenu ended with the previous item, so pass back the current item.
-    else {
-      $remnant = $item;
+    elseif (!$next || $next['depth'] < $depth) {
+      // Exit loop and return.
       break;
     }
   }
-  if ($previous_element) {
-    // We have one more link dangling.
-    $tree[$previous_element['mlid']] = array(
-      'link' => $previous_element,
-      'below' => FALSE,
-    );
-  }
-  return array($remnant, $tree);
+  return $tree;
 }
 
 /**
@@ -1405,7 +1399,7 @@ function menu_navigation_links($menu_name, $level = 0) {
   }
 
   // Get the menu hierarchy for the current page.
-  $tree = menu_tree_page_data($menu_name);
+  $tree = menu_tree_page_data($menu_name, $level + 1);
 
   // Go down the active trail until the right level is reached.
   while ($level-- > 0 && $tree) {