From 2a7645118519b62a44027ef52316010c9a7884b4 Mon Sep 17 00:00:00 2001
From: Matias Nitsche <mnitsche@dc.uba.ar>
Date: Sun, 19 Apr 2020 17:56:58 -0300
Subject: [PATCH] stm32l4 PWR: add VOS setting function

---
 arch/arm/src/stm32l4/stm32l4_pwr.c | 43 ++++++++++++++++++++++++++++++
 arch/arm/src/stm32l4/stm32l4_pwr.h | 21 +++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/arch/arm/src/stm32l4/stm32l4_pwr.c b/arch/arm/src/stm32l4/stm32l4_pwr.c
index eb93e89afa..7bd5d5c2ed 100644
--- a/arch/arm/src/stm32l4/stm32l4_pwr.c
+++ b/arch/arm/src/stm32l4/stm32l4_pwr.c
@@ -226,3 +226,46 @@ bool stm32l4_pwr_enableusv(bool set)
 
   return was_set;
 }
+
+/************************************************************************************
+ * Name: stm32_pwr_setvos
+ *
+ * Description:
+ *   Set voltage scaling for Vcore
+ *
+ * Input Parameters:
+ *   vos - Either 1 or 2, to set to Range 1 or 2, respectively
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *   At present, this function is called only from initialization logic.  If used
+ *   for any other purpose that protection to assure that its operation is atomic
+ *   will be required.
+ *
+ ************************************************************************************/
+
+void stm32_pwr_setvos(int vos)
+{
+  uint32_t regval;
+
+  if (vos != 1 && vos != 2)
+    {
+      return;
+    }
+
+  regval  = getreg32(STM32L4_PWR_CR1);
+  regval &= ~PWR_CR1_VOS_MASK;
+
+  if (vos == 1)
+    {
+      regval |= PWR_CR1_VOS_RANGE1;
+    }
+  else
+    {
+      regval |= PWR_CR1_VOS_RANGE2;
+    }
+
+  putreg32(regval, STM32L4_PWR_CR1);
+}
diff --git a/arch/arm/src/stm32l4/stm32l4_pwr.h b/arch/arm/src/stm32l4/stm32l4_pwr.h
index e709a3f733..8693bf17f1 100644
--- a/arch/arm/src/stm32l4/stm32l4_pwr.h
+++ b/arch/arm/src/stm32l4/stm32l4_pwr.h
@@ -119,6 +119,27 @@ bool stm32l4_pwr_enablebkp(bool writable);
 
 bool stm32l4_pwr_enableusv(bool set);
 
+/************************************************************************************
+ * Name: stm32_pwr_setvos
+ *
+ * Description:
+ *   Set voltage scaling for Vcore
+ *
+ * Input Parameters:
+ *   vos - Either 1 or 2, to set to Range 1 or 2, respectively
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *   At present, this function is called only from initialization logic.  If used
+ *   for any other purpose that protection to assure that its operation is atomic
+ *   will be required.
+ *
+ ************************************************************************************/
+
+void stm32_pwr_setvos(int vos);
+
 #undef EXTERN
 #if defined(__cplusplus)
 }