From fa61bb38cd93c130658a7cd3c665abd9b0364a2b Mon Sep 17 00:00:00 2001
From: Maciej Gibowicz <maciej.gibowicz@mudita.com>
Date: Wed, 21 Apr 2021 14:45:03 +0200
Subject: [PATCH] [EGD-5661] Add power management for filesystem

Add peripheral control (USDHC and PLL2 clocks)
for file system support.
This will save energy and extend battery life.
---
 module-sys/SystemManager/SystemManager.cpp    | 17 ++++++
 module-sys/SystemManager/SystemManager.hpp    |  5 ++
 .../include/purefs/blkdev/disk_image.hpp      |  3 +
 .../linux/purefs/src/blkdev/disk_image.cpp    | 12 ++++
 .../include/purefs/blkdev/disk_emmc.hpp       |  8 ++-
 .../rt1051/purefs/src/blkdev/disk_emmc.cpp    | 55 ++++++++++++++++++-
 .../include/user/purefs/vfs_subsystem.hpp     |  2 +-
 7 files changed, 98 insertions(+), 4 deletions(-)

diff --git a/module-sys/SystemManager/SystemManager.cpp b/module-sys/SystemManager/SystemManager.cpp
index 3fd9cb393..c13c07270 100644
--- a/module-sys/SystemManager/SystemManager.cpp
+++ b/module-sys/SystemManager/SystemManager.cpp
@@ -33,6 +33,7 @@
 #include <time/ScopedTime.hpp>
 #include "Timers/TimerFactory.hpp"
 #include <service-appmgr/StartupType.hpp>
+#include <purefs/vfs_subsystem.hpp>
 
 const inline size_t systemManagerStack = 4096 * 2;
 
@@ -642,6 +643,12 @@ namespace sys
 
         deviceManager->RegisterNewDevice(powerManager->getExternalRamDevice());
 
+        cpuSentinel = std::make_shared<sys::CpuSentinel>(
+            service::name::system_manager, this, [this](bsp::CpuFrequencyHz newFrequency) {
+                UpdateResourcesAfterCpuFrequencyChange(newFrequency);
+            });
+        powerManager->RegisterNewSentinel(cpuSentinel);
+
         return ReturnCodes::Success;
     }
 
@@ -773,6 +780,16 @@ namespace sys
         return MessageNone{};
     }
 
+    void SystemManager::UpdateResourcesAfterCpuFrequencyChange(bsp::CpuFrequencyHz newFrequency)
+    {
+        if (newFrequency == bsp::CpuFrequencyHz::Level_1) {
+            purefs::subsystem::disk_mgr()->pm_control(purefs::blkdev::pm_state::suspend);
+        }
+        else {
+            purefs::subsystem::disk_mgr()->pm_control(purefs::blkdev::pm_state::active);
+        }
+    }
+
     std::vector<std::shared_ptr<Service>> SystemManager::servicesList;
     std::vector<std::shared_ptr<app::Application>> SystemManager::applicationsList;
     cpp_freertos::MutexStandard SystemManager::serviceDestroyMutex;
diff --git a/module-sys/SystemManager/SystemManager.hpp b/module-sys/SystemManager/SystemManager.hpp
index f729151d1..63edd06cb 100644
--- a/module-sys/SystemManager/SystemManager.hpp
+++ b/module-sys/SystemManager/SystemManager.hpp
@@ -189,6 +189,9 @@ namespace sys
         /// periodic update of cpu statistics
         void CpuStatisticsTimerHandler();
 
+        /// used for power management control for the filesystem
+        void UpdateResourcesAfterCpuFrequencyChange(bsp::CpuFrequencyHz newFrequency);
+
         MessagePointer handlePhoneModeRequest(PhoneModeRequest *request);
         MessagePointer handleTetheringStateRequest(TetheringStateRequest *request);
         MessagePointer enableTethering(TetheringEnabledResponse *response);
@@ -208,6 +211,8 @@ namespace sys
         InitFunction systemInit;
         std::vector<std::string> readyForCloseRegister;
 
+        std::shared_ptr<sys::CpuSentinel> cpuSentinel;
+
         static std::vector<std::shared_ptr<Service>> servicesList;
         static std::vector<std::shared_ptr<app::Application>> applicationsList;
         static cpp_freertos::MutexStandard serviceDestroyMutex;
diff --git a/module-vfs/board/linux/purefs/include/purefs/blkdev/disk_image.hpp b/module-vfs/board/linux/purefs/include/purefs/blkdev/disk_image.hpp
index 092f4b816..c77bf4e1c 100644
--- a/module-vfs/board/linux/purefs/include/purefs/blkdev/disk_image.hpp
+++ b/module-vfs/board/linux/purefs/include/purefs/blkdev/disk_image.hpp
@@ -28,10 +28,13 @@ namespace purefs::blkdev
         auto status() const -> media_status override;
         auto get_info(info_type what, hwpart_t hwpart) const -> scount_t override;
         auto erase(sector_t lba, std::size_t count, hwpart_t hwpart) -> int override;
+        auto pm_control(pm_state target_state) -> int override;
+        auto pm_read(pm_state &current_state) -> int override;
         auto range_valid(sector_t lba, std::size_t count, hwpart_t hwpart) const -> bool;
         auto open_and_truncate(hwpart_t hwpart) -> int;
 
       private:
+        pm_state pmState{pm_state::active};
         std::vector<int> m_filedes;
         std::vector<std::size_t> m_sectors;
         const std::string m_image_name;
diff --git a/module-vfs/board/linux/purefs/src/blkdev/disk_image.cpp b/module-vfs/board/linux/purefs/src/blkdev/disk_image.cpp
index c7481d9c5..799a0aca9 100644
--- a/module-vfs/board/linux/purefs/src/blkdev/disk_image.cpp
+++ b/module-vfs/board/linux/purefs/src/blkdev/disk_image.cpp
@@ -186,4 +186,16 @@ namespace purefs::blkdev
         }
         return 0;
     }
+
+    auto disk_image::pm_control(pm_state target_state) -> int
+    {
+        pmState = target_state;
+        return 0;
+    }
+    auto disk_image::pm_read(pm_state &current_state) -> int
+    {
+        current_state = pmState;
+        return 0;
+    }
+
 } // namespace purefs::blkdev
diff --git a/module-vfs/board/rt1051/purefs/include/purefs/blkdev/disk_emmc.hpp b/module-vfs/board/rt1051/purefs/include/purefs/blkdev/disk_emmc.hpp
index ae8acf280..0b0b1bc41 100644
--- a/module-vfs/board/rt1051/purefs/include/purefs/blkdev/disk_emmc.hpp
+++ b/module-vfs/board/rt1051/purefs/include/purefs/blkdev/disk_emmc.hpp
@@ -3,6 +3,7 @@
 
 #pragma once
 
+#include "drivers/usdhc/DriverUSDHC.hpp"
 #include <purefs/blkdev/disk.hpp>
 #include <mutex.hpp>
 #include <memory>
@@ -33,14 +34,19 @@ namespace purefs::blkdev
         auto status() const -> media_status override;
         auto get_info(info_type what, hwpart_t hwpart) const -> scount_t override;
         auto erase(sector_t lba, std::size_t count, hwpart_t hwpart) -> int override;
+        auto pm_control(pm_state target_state) -> int override;
+        auto pm_read(pm_state &current_state) -> int override;
 
       private:
         auto switch_partition(hwpart_t newpart) -> int;
 
       private:
         int initStatus;
-        std::unique_ptr<_mmc_card> mmcCard;
+        pm_state pmState{pm_state::active};
         mutable cpp_freertos::MutexRecursive mutex;
         std::atomic<hwpart_t> currHwPart{0};
+
+        std::unique_ptr<_mmc_card> mmcCard;
+        std::shared_ptr<drivers::DriverUSDHC> driverUSDHC;
     };
 } // namespace purefs::blkdev
diff --git a/module-vfs/board/rt1051/purefs/src/blkdev/disk_emmc.cpp b/module-vfs/board/rt1051/purefs/src/blkdev/disk_emmc.cpp
index dcae5fa70..f2c3c8d14 100644
--- a/module-vfs/board/rt1051/purefs/src/blkdev/disk_emmc.cpp
+++ b/module-vfs/board/rt1051/purefs/src/blkdev/disk_emmc.cpp
@@ -8,6 +8,7 @@
 
 #include <bsp/common.hpp>
 #include "board/rt1051/bsp/eMMC/fsl_mmc.h"
+#include "bsp/BoardDefinitions.hpp"
 
 namespace purefs::blkdev
 {
@@ -21,6 +22,9 @@ namespace purefs::blkdev
         mmcCard->enablePreDefinedBlockCount = true;
         mmcCard->host.base                  = USDHC2;
         mmcCard->host.sourceClock_Hz        = GetPerphSourceClock(PerphClock_USDHC2);
+
+        driverUSDHC = drivers::DriverUSDHC::Create(
+            "EMMC", static_cast<drivers::USDHCInstances>(BoardDefinitions::EMMC_USDHC_INSTANCE));
     }
 
     disk_emmc::~disk_emmc()
@@ -57,7 +61,13 @@ namespace purefs::blkdev
         if (err != kStatus_Success) {
             return err;
         }
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Enable();
+        }
         err = MMC_WriteBlocks(mmcCard.get(), static_cast<const uint8_t *>(buf), lba, count);
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Disable();
+        }
         if (err != kStatus_Success) {
             return err;
         }
@@ -80,7 +90,13 @@ namespace purefs::blkdev
         if (err != kStatus_Success) {
             return err;
         }
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Enable();
+        }
         err = MMC_ReadBlocks(mmcCard.get(), static_cast<uint8_t *>(buf), lba, count);
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Disable();
+        }
         if (err != kStatus_Success) {
             return err;
         }
@@ -96,8 +112,14 @@ namespace purefs::blkdev
         while ((GET_SDMMCHOST_STATUS(mmcCard->host.base) & CARD_DATA0_STATUS_MASK) != CARD_DATA0_NOT_BUSY) {
             taskYIELD();
         }
-
-        if (kStatus_Success != MMC_WaitWriteComplete(mmcCard.get())) {
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Enable();
+        }
+        auto err = MMC_WaitWriteComplete(mmcCard.get());
+        if (pmState == pm_state::suspend) {
+            driverUSDHC->Disable();
+        }
+        if (err != kStatus_Success) {
             return kStatus_SDMMC_WaitWriteCompleteFailed;
         }
         return statusBlkDevSuccess;
@@ -148,7 +170,13 @@ namespace purefs::blkdev
         }
         int ret{};
         if (newpart != currHwPart) {
+            if (pmState == pm_state::suspend) {
+                driverUSDHC->Enable();
+            }
             ret = MMC_SelectPartition(mmcCard.get(), static_cast<mmc_access_partition_t>(newpart));
+            if (pmState == pm_state::suspend) {
+                driverUSDHC->Disable();
+            }
             if (ret == kStatus_Success) {
                 currHwPart = newpart;
             }
@@ -159,4 +187,27 @@ namespace purefs::blkdev
         return ret;
     }
 
+    auto disk_emmc::pm_control(pm_state target_state) -> int
+    {
+        if (pmState != target_state) {
+            cpp_freertos::LockGuard lock(mutex);
+            if (target_state == pm_state::suspend) {
+                driverUSDHC->Disable();
+            }
+            else {
+                driverUSDHC->Enable();
+            }
+
+            pmState = target_state;
+        }
+
+        return kStatus_Success;
+    }
+
+    auto disk_emmc::pm_read(pm_state &current_state) -> int
+    {
+        current_state = pmState;
+        return kStatus_Success;
+    }
+
 } // namespace purefs::blkdev
diff --git a/module-vfs/include/user/purefs/vfs_subsystem.hpp b/module-vfs/include/user/purefs/vfs_subsystem.hpp
index e6d0350d9..44c13e97d 100644
--- a/module-vfs/include/user/purefs/vfs_subsystem.hpp
+++ b/module-vfs/include/user/purefs/vfs_subsystem.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once