diff --git a/activation.cpp b/activation.cpp
index 1ed765c..9859cd6 100644
--- a/activation.cpp
+++ b/activation.cpp
@@ -153,44 +153,18 @@
 
 uint8_t RedundancyPriority::priority(uint8_t value)
 {
-    parent.parent.freePriority(value, parent.versionId);
+    // Set the priority value so that the freePriority() function can order
+    // the versions by priority.
+    auto newPriority = softwareServer::RedundancyPriority::priority(value);
     storeToFile(parent.versionId, value);
-
-    // Update U-Boot env variable to point to this version if it has the
-    // lowest priority. Otherwise, reset the UbootEnvVars to find the lowest
-    // priority version and set that in U-Boot.
-    if (parent.parent.isLowestPriority(value))
-    {
-        parent.updateUbootEnvVars();
-    }
-    else
-    {
-        parent.parent.resetUbootEnvVars();
-    }
-
-    return softwareServer::RedundancyPriority::priority(value);
+    parent.parent.freePriority(value, parent.versionId);
+    return newPriority;
 }
 
-// TODO: openbmc/openbmc#2369 Add recovery policy to updateubootvars
-//       unit template.
-void Activation::updateUbootEnvVars()
+uint8_t RedundancyPriority::sdbusPriority(uint8_t value)
 {
-    auto method = bus.new_method_call(
-            SYSTEMD_BUSNAME,
-            SYSTEMD_PATH,
-            SYSTEMD_INTERFACE,
-            "StartUnit");
-    auto updateEnvVarsFile = "obmc-flash-bmc-updateubootvars@" + versionId +
-            ".service";
-    method.append(updateEnvVarsFile, "replace");
-    auto result = bus.call(method);
-
-    //Check that the bus call didn't result in an error
-    if (result.is_method_error())
-    {
-        log<level::ERR>("Failed to update u-boot env variables",
-                        entry("VERSIONID=%s", versionId));
-    }
+    storeToFile(parent.versionId, value);
+    return softwareServer::RedundancyPriority::priority(value);
 }
 
 void Activation::unitStateChange(sdbusplus::message::message& msg)
diff --git a/activation.hpp b/activation.hpp
index d1d050a..a6e1111 100644
--- a/activation.hpp
+++ b/activation.hpp
@@ -49,11 +49,13 @@
          *  @param[in] path   - The Dbus object path
          *  @param[in] parent - Parent object.
          *  @param[in] value  - The redundancyPriority value
+         *  @param[in] freePriority  - Call freePriorioty, default to true
          */
         RedundancyPriority(sdbusplus::bus::bus& bus,
                                    const std::string& path,
                                    Activation& parent,
-                                   uint8_t value) :
+                                   uint8_t value,
+                                   bool freePriority=true) :
                                    RedundancyPriorityInherit(bus,
                                    path.c_str(), true),
                                    parent(parent),
@@ -61,7 +63,15 @@
                                    path(path)
         {
             // Set Property
-            priority(value);
+            if (freePriority)
+            {
+                priority(value);
+            }
+            else
+            {
+                sdbusPriority(value);
+            }
+
             std::vector<std::string> interfaces({interface});
             bus.emit_interfaces_added(path.c_str(), interfaces);
         }
@@ -72,7 +82,8 @@
             bus.emit_interfaces_removed(path.c_str(), interfaces);
         }
 
-        /** @brief Overloaded Priority property set function
+        /** @brief Overriden Priority property set function, calls freePriority
+         *         to bump the duplicated priority values.
          *
          *  @param[in] value - uint8_t
          *
@@ -80,6 +91,14 @@
          */
         uint8_t priority(uint8_t value) override;
 
+        /** @brief Non-Overriden Priority property set function
+         *
+         *  @param[in] value - uint8_t
+         *
+         *  @return Success or exception thrown
+         */
+        uint8_t sdbusPriority(uint8_t value);
+
         /** @brief Priority property get function
          *
          *  @returns uint8_t - The Priority value
@@ -313,13 +332,6 @@
          */
         void unsubscribeFromSystemdSignals();
 
-        /**
-         * @brief Updates the U-Boot variables to point to this activation's
-         *        versionId, so that the systems boots from this version on
-         *        the next reboot.
-         */
-        void updateUbootEnvVars();
-
         /** @brief Persistent sdbusplus DBus bus connection */
         sdbusplus::bus::bus& bus;
 
diff --git a/item_updater.cpp b/item_updater.cpp
index 33f4096..151986f 100644
--- a/item_updater.cpp
+++ b/item_updater.cpp
@@ -1,4 +1,5 @@
 #include <fstream>
+#include <set>
 #include <string>
 #include <phosphor-logging/log.hpp>
 #include <phosphor-logging/elog.hpp>
@@ -254,7 +255,8 @@
                              bus,
                              path,
                              *(activations.find(id)->second),
-                             priority);
+                             priority,
+                             false);
             }
         }
     }
@@ -396,18 +398,58 @@
 
 void ItemUpdater::freePriority(uint8_t value, const std::string& versionId)
 {
-    //TODO openbmc/openbmc#1896 Improve the performance of this function
+    std::map<std::string, uint8_t> priorityMap;
+
+    // Insert the requested version and priority, it may not exist yet.
+    priorityMap.insert(std::make_pair(versionId, value));
+
     for (const auto& intf : activations)
     {
         if (intf.second->redundancyPriority)
         {
-            if (intf.second->redundancyPriority.get()->priority() == value &&
-                intf.second->versionId != versionId)
-            {
-                intf.second->redundancyPriority.get()->priority(value + 1);
-            }
+            priorityMap.insert(std::make_pair(
+                    intf.first,
+                    intf.second->redundancyPriority.get()->priority()));
         }
     }
+
+    // Lambda function to compare 2 priority values, use <= to allow duplicates
+    typedef std::function<bool(
+            std::pair<std::string, uint8_t>,
+            std::pair<std::string, uint8_t>)> cmpPriority;
+    cmpPriority cmpPriorityFunc = [](
+            std::pair<std::string, uint8_t> priority1,
+            std::pair<std::string, uint8_t> priority2)
+    {
+        return priority1.second <= priority2.second;
+    };
+
+    // Sort versions by ascending priority
+    std::set<std::pair<std::string, uint8_t>, cmpPriority> prioritySet(
+            priorityMap.begin(), priorityMap.end(), cmpPriorityFunc);
+
+    auto freePriorityValue = value;
+    for (auto& element : prioritySet)
+    {
+        if (element.first == versionId)
+        {
+            continue;
+        }
+        if (element.second == freePriorityValue)
+        {
+            ++freePriorityValue;
+            auto it = activations.find(element.first);
+            it->second->redundancyPriority.get()->sdbusPriority(
+                    freePriorityValue);
+        }
+    }
+
+    auto lowestVersion = prioritySet.begin()->first;
+    if (value == prioritySet.begin()->second)
+    {
+        lowestVersion = versionId;
+    }
+    updateUbootEnvVars(lowestVersion);
 }
 
 void ItemUpdater::reset()
@@ -587,6 +629,26 @@
     return true;
 }
 
+void ItemUpdater::updateUbootEnvVars(const std::string& versionId)
+{
+    auto method = bus.new_method_call(
+            SYSTEMD_BUSNAME,
+            SYSTEMD_PATH,
+            SYSTEMD_INTERFACE,
+            "StartUnit");
+    auto updateEnvVarsFile = "obmc-flash-bmc-updateubootvars@" + versionId +
+            ".service";
+    method.append(updateEnvVarsFile, "replace");
+    auto result = bus.call(method);
+
+    //Check that the bus call didn't result in an error
+    if (result.is_method_error())
+    {
+        log<level::ERR>("Failed to update u-boot env variables",
+                        entry("VERSIONID=%s", versionId));
+    }
+}
+
 void ItemUpdater::resetUbootEnvVars()
 {
     decltype(activations.begin()->second->redundancyPriority.get()->priority())
@@ -609,8 +671,7 @@
     }
 
     // Update the U-boot environment variable to point to the lowest priority
-    auto it = activations.find(lowestPriorityVersion);
-    it->second->updateUbootEnvVars();
+    updateUbootEnvVars(lowestPriorityVersion);
 }
 
 } // namespace updater
diff --git a/item_updater.hpp b/item_updater.hpp
index 7b65b80..ea88c4b 100644
--- a/item_updater.hpp
+++ b/item_updater.hpp
@@ -114,12 +114,21 @@
          */
         bool isLowestPriority(uint8_t value);
 
-    /**
-     * @brief Updates the uboot variables to point to BMC version with lowest
-     *        priority, so that the system boots from this version on the
-     *        next boot.
-     */
-    void resetUbootEnvVars();
+        /**
+         * @brief Updates the U-Boot variables to point to the requested
+         *        versionId, so that the systems boots from this version on
+         *        the next reboot.
+         *
+         * @param[in] versionId - The version to point the system to boot from.
+         */
+        void updateUbootEnvVars(const std::string& versionId);
+
+        /**
+         * @brief Updates the uboot variables to point to BMC version with lowest
+         *        priority, so that the system boots from this version on the
+         *        next boot.
+         */
+        void resetUbootEnvVars();
 
     private:
         /** @brief Callback function for Software.Version match.