ubi: Rewrite freePriority

freePriority is called by RedundancyPriority::priority() to ensure that
there are no duplicate priority values. Originally, freePriority would
call priority() whenever it updated a priority value, but this would call
freePriority again. This can cause the priorities stored on flash to not
match the D-Bus values. The priorities on flash are used to determine
which version to boot from so that leads to unexpected behavior.

Example is if A has priority 1 and B has priority 2, and the priority of
B is changed to 1, this triggers the new value of 1 to be stored on
flash, then A is bumped to 2, but then B that originally had 2 is bumped
to 3, so that at the end of the operation, B has priority 3 on flash but
the correct 1 in D-Bus.

The solution is to prevent freePriority from calling itself, by sorting
all versions by priority in ascending order, so that if a version is
bumped, then only the remaining versions need to be checked. Then
locally update the priority values on flash and on D-Bus for each
changed one.

Tested: Changed priorities multipled times and verified the mismatch is not
        seen anymore.

Change-Id: I704ee98f356a1a77f431b83e4b9d787b2671aeb2
Signed-off-by: Adriana Kobylak <anoo@us.ibm.com>
diff --git a/item_updater.hpp b/item_updater.hpp
index 3260003..01466bb 100644
--- a/item_updater.hpp
+++ b/item_updater.hpp
@@ -94,7 +94,9 @@
     virtual ~ItemUpdater() = default;
 
     /** @brief Sets the given priority free by incrementing
-     *  any existing priority with the same value by 1
+     *  any existing priority with the same value by 1. It will then continue
+     *  to resolve duplicate priorities caused by this increase, by increasing
+     *  the priority by 1 until there are no more duplicate values.
      *
      *  @param[in] value - The priority that needs to be set free.
      *  @param[in] versionId - The Id of the version for which we
diff --git a/ubi/item_updater_ubi.cpp b/ubi/item_updater_ubi.cpp
index 227bc1a..ccc540a 100644
--- a/ubi/item_updater_ubi.cpp
+++ b/ubi/item_updater_ubi.cpp
@@ -269,18 +269,37 @@
 
 void ItemUpdaterUbi::freePriority(uint8_t value, const std::string& versionId)
 {
-    // TODO openbmc/openbmc#1896 Improve the performance of this function
+    //  Versions with the lowest priority in front
+    std::priority_queue<std::pair<int, std::string>,
+                        std::vector<std::pair<int, std::string>>,
+                        std::greater<std::pair<int, std::string>>>
+        versionsPQ;
+
     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);
-            }
+            versionsPQ.push(std::make_pair(
+                intf.second->redundancyPriority.get()->priority(),
+                intf.second->versionId));
         }
     }
+
+    while (!versionsPQ.empty())
+    {
+        if (versionsPQ.top().first == value &&
+            versionsPQ.top().second != versionId)
+        {
+            // Increase priority by 1 and update its value
+            ++value;
+            storeToFile(versionsPQ.top().second, value);
+            auto it = activations.find(versionsPQ.top().second);
+            it->second->redundancyPriority.get()->sdbusplus::xyz::
+                openbmc_project::Software::server::RedundancyPriority::priority(
+                    value);
+        }
+        versionsPQ.pop();
+    }
 }
 
 bool ItemUpdaterUbi::erase(std::string entryId)