Updating OpAmp code with calibration functions, cleaning.

4 files changed, 582 insertions, 85 deletions

diff --git a/os/hal/include/hal_opamp.h b/os/hal/include/hal_opamp.h
index 383e928..b20c938 100644
--- a/os/hal/include/hal_opamp.h
+++ b/os/hal/include/hal_opamp.h
@@ -26,6 +26,9 @@
 /* Driver constants.                                                         */
 /*===========================================================================*/
 
+#define OPAMP_P_BELOW_M (0U)
+#define OPAMP_M_BELOW_P (1U)
+
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
 /*===========================================================================*/
@@ -44,8 +47,8 @@
 typedef enum {
   OPAMP_UNINIT = 0,                   /**< Not initialized.                   */
   OPAMP_STOP = 1,                     /**< Stopped.                           */
-  OPAMP_READY = 2,                    /**< Ready.                             */
-  OPAMP_ACTIVE = 3,                   /**< Active cycle phase.                */
+  OPAMP_ACTIVE = 2,                   /**< Active.                            */
+  OPAMP_CALIBRATING = 3,              /**< Calibration in progress.           */
 } opampstate_t;
 
 /**
@@ -60,29 +63,13 @@ typedef struct OPAMPDriver OPAMPDriver;
 /*===========================================================================*/
 
 /**
- * @name    Macro Functions
- * @{
- */
-/**
- * @brief   Enables the input capture.
- *
- * @param[in] opamp      pointer to the @p OPAMPDriver object
- *
+ * @brief   Calibrate opamps
+ * *
  * @iclass
  */
-#define opampEnableI(opamp) opamp_lld_enable(opamp)
-
-/**
- * @brief   Disables the input capture.
- *
- * @param[in] opamp      pointer to the @p OPAMPDriver object
- *
- * @iclass
- */
-#define opampDisableI(opamp) opamp_lld_disable(opamp)
+#define opampCalibrate() opamp_lld_calibrate()
 /** @} */
 
-
 /**
  * @name    Low Level driver helper macros
  * @{
diff --git a/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.c b/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.c
index 7251002..db988aa 100644
--- a/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.c
+++ b/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.c
@@ -1,6 +1,7 @@
 /*
     ChibiOS - Copyright (C) 2006..2019 Giovanni Di Sirio
               Copyright (C) 2019 Fabien Poussin (fabien.poussin (at) google's mail)
+              Copyright (C) 2019 Alexandre Bustico
 
     Licensed under the Apache License, Version 2.0 (the "License");
     you may not use this file except in compliance with the License.
@@ -98,29 +99,29 @@ void opamp_lld_init(void) {
 #if STM32_OPAMP_USE_OPAMP1
   /* Driver initialization.*/
   opampObjectInit(&OPAMPD1);
-  OPAMPD1.reg = OPAMP;
-  OPAMPD1.reg->CSR = 0;
+  OPAMPD1.opamp = OPAMP;
+  OPAMPD1.opamp->CSR = 0;
 #endif
 
 #if STM32_OPAMP_USE_OPAMP2
   /* Driver initialization.*/
   opampObjectInit(&OPAMPD2);
-  OPAMPD2.reg = OPAMP2;
-  OPAMPD2.reg->CSR = 0;
+  OPAMPD2.opamp = OPAMP2;
+  OPAMPD2.opamp->CSR = 0;
 #endif
 
 #if STM32_OPAMP_USE_OPAMP3
   /* Driver initialization.*/
   opampObjectInit(&OPAMPD3);
-  OPAMPD3.reg = OPAMP3;
-  OPAMPD3.reg->CSR = 0;
+  OPAMPD3.opamp = OPAMP3;
+  OPAMPD3.opamp->CSR = 0;
 #endif
 
 #if STM32_OPAMP_USE_OPAMP4
   /* Driver initialization.*/
   opampObjectInit(&OPAMPD4);
-  OPAMPD4.reg = OPAMP4;
-  OPAMPD4.reg->CSR = 0;
+  OPAMPD4.opamp = OPAMP4;
+  OPAMPD4.opamp->CSR = 0;
 #endif
 
 }
@@ -128,29 +129,29 @@ void opamp_lld_init(void) {
 /**
  * @brief   Configures and activates the OPAMP peripheral.
  *
- * @param[in] compp      pointer to the @p OPAMPDriver object
+ * @param[in] opampp      pointer to the @p OPAMPDriver object
  *
  * @notapi
  */
-void opamp_lld_start(OPAMPDriver *compp) {
+void opamp_lld_start(OPAMPDriver *opampp) {
 
   // Apply CSR Execpt the enable bit.
-  compp->reg->CSR = compp->config->csr & ~OPAMP_CSR_OPAMPxEN;
+  opampp->opamp->CSR = opampp->config->csr & ~OPAMP_CSR_OPAMPxEN;
 
 }
 
 /**
  * @brief   Deactivates the comp peripheral.
  *
- * @param[in] compp      pointer to the @p OPAMPDriver object
+ * @param[in] opampp      pointer to the @p OPAMPDriver object
  *
  * @notapi
  */
-void opamp_lld_stop(OPAMPDriver *compp) {
+void opamp_lld_stop(OPAMPDriver *opampp) {
 
-  if (compp->state == OPAMP_READY) {
+  if (opampp->state == OPAMP_ACTIVE) {
 
-    compp->reg->CSR = 0;
+    opampp->opamp->CSR = 0;
   }
 
 }
@@ -158,27 +159,457 @@ void opamp_lld_stop(OPAMPDriver *compp) {
 /**
  * @brief   Enables the output.
  *
- * @param[in] compp      pointer to the @p OPAMPDriver object
+ * @param[in] opampp      pointer to the @p OPAMPDriver object
  *
  * @notapi
  */
-void opamp_lld_enable(OPAMPDriver *compp) {
+void opamp_lld_enable(OPAMPDriver *opampp) {
 
-   compp->reg->CSR |= OPAMP_CSR_OPAMPxEN; /* Enable */
+   opampp->opamp->CSR |= OPAMP_CSR_OPAMPxEN; /* Enable */
 }
 
 /**
  * @brief   Disables the output.
  *
- * @param[in] compp      pointer to the @p OPAMPDriver object
+ * @param[in] opampp      pointer to the @p OPAMPDriver object
  *
  * @notapi
  */
-void opamp_lld_disable(OPAMPDriver *compp) {
+void opamp_lld_disable(OPAMPDriver *opampp) {
 
-  compp->reg->CSR &= ~OPAMP_CSR_OPAMPxEN; /* Disable */
+  opampp->opamp->CSR &= ~OPAMP_CSR_OPAMPxEN; /* Disable */
 }
 
+#if STM32_OPAMP_USER_TRIM_ENABLED
+
+void opamp_lld_calibrate_once(void)
+{
+#if STM32_OPAMP_USE_OPAMP1
+  uint32_t trimmingvaluen1 = 16U;
+  uint32_t trimmingvaluep1 = 16U;
+  OPAMPD1.state = OPAMP_CALIBRATING;
+#define CSRm OPAMPD1.opamp->CSR
+  /* Set Calibration mode */
+  /* Non-inverting input connected to calibration reference voltage. */
+  CSRm |= OPAMP_CSR_FORCEVP;
+  /*  user trimming values are used for offset calibration */
+  CSRm |= OPAMP_CSR_USERTRIM;
+  /* Enable calibration */
+  CSRm |= OPAMP_CSR_CALON;
+  /* 1st calibration - N  Select 90U% VREF */
+  MODIFY_REG(CSRm, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_90P);
+  /* Enable the opamps */
+  CSRm |= OPAMP_CSR_OPAMPxEN;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+  uint32_t trimmingvaluen2 = 16U;
+  uint32_t trimmingvaluep2 = 16U;
+  OPAMPD2.state = OPAMP_CALIBRATING;
+#define CSRm OPAMPD2.opamp->CSR
+  CSRm |= OPAMP_CSR_FORCEVP;
+  CSRm |= OPAMP_CSR_USERTRIM;
+  CSRm |= OPAMP_CSR_CALON;
+  MODIFY_REG(CSRm, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_90P);
+  CSRm |= OPAMP_CSR_OPAMPxEN;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+  uint32_t trimmingvaluen3 = 16U;
+  uint32_t trimmingvaluep3 = 16U;
+  OPAMPD3.state = OPAMP_CALIBRATING;
+#define CSRm OPAMPD3.opamp->CSR
+  CSRm |= OPAMP_CSR_FORCEVP;
+  CSRm |= OPAMP_CSR_USERTRIM;
+  CSRm |= OPAMP_CSR_CALON;
+  MODIFY_REG(CSRm, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_90P);
+  CSRm |= OPAMP_CSR_OPAMPxEN;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+  uint32_t trimmingvaluen4 = 16U;
+  uint32_t trimmingvaluep4 = 16U;
+  OPAMPD4.state = OPAMP_CALIBRATING;
+#define CSRm OPAMPD4.opamp->CSR
+  CSRm |= OPAMP_CSR_FORCEVP;
+  CSRm |= OPAMP_CSR_USERTRIM;
+  CSRm |= OPAMP_CSR_CALON;
+  MODIFY_REG(CSRm, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_90P);
+  CSRm |= OPAMP_CSR_OPAMPxEN;
+#undef CSRm
+#endif
+
+  chSysPolledDelayX(MS2RTC(STM32_SYSCLK, 20));
+  uint32_t delta = 8U;
+
+  while (delta != 0U) {
+#if STM32_OPAMP_USE_OPAMP1
+    MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen1<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+    MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen2<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+    MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen3<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+    MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen4<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+
+    /* OFFTRIMmax delay 2 ms as per datasheet (electrical characteristics */
+    /* Offset trim time: during calibration, minimum time needed between */
+    /* two steps to have 1 mV accuracy */
+    chSysPolledDelayX(MS2RTC(STM32_SYSCLK, 2));
+
+#if STM32_OPAMP_USE_OPAMP1
+    if (OPAMPD1.opamp->CSR & OPAMP_CSR_OUTCAL)   {
+      /* OPAMP_CSR_OUTCAL is HIGH try higher trimming */
+      trimmingvaluen1 += delta;
+    } else  {
+      /* OPAMP_CSR_OUTCAL is LOW try lower trimming */
+      trimmingvaluen1 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+    if (OPAMPD2.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen2 += delta;
+    } else {
+      trimmingvaluen2 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+    if (OPAMPD3.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen3 += delta;
+    } else {
+      trimmingvaluen3 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+    if (OPAMPD4.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen4 += delta;
+    }  else {
+      trimmingvaluen4 -= delta;
+    }
+#endif
+
+    delta >>= 1U;
+
+  }
+
+  /* Still need to check if righ calibration is current value or un step below */
+  /* Indeed the first value that causes the OUTCAL bit to change from 1 to 0U */
+#if STM32_OPAMP_USE_OPAMP1
+  MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+       trimmingvaluen1<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+  MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+       trimmingvaluen2<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+  MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+       trimmingvaluen3<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+  MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+       trimmingvaluen4<<OPAMP_CSR_TRIMOFFSETN_Pos);
+#endif
+
+  /* OFFTRIMmax delay 2 ms as per datasheet (electrical characteristics */
+  /* Offset trim time: during calibration, minimum time needed between */
+  /* two steps to have 1 mV accuracy */
+  chSysPolledDelayX(MS2RTC(STM32_SYSCLK, 2));
+
+#if STM32_OPAMP_USE_OPAMP1
+    if (OPAMPD1.opamp->CSR & OPAMP_CSR_OUTCAL)   {
+      /* OPAMP_CSR_OUTCAL is HIGH try higher trimming */
+      trimmingvaluen1  += (trimmingvaluen1 != 31);
+      MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+     trimmingvaluen1<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+    if (OPAMPD2.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen2  += (trimmingvaluen2 != 31);
+      MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+     trimmingvaluen2<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+    if (OPAMPD3.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen3  += (trimmingvaluen3 != 31);
+      MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+     trimmingvaluen3<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+    if (OPAMPD4.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluen4  += (trimmingvaluen4 != 31);
+      MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETN,
+     trimmingvaluen4<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    }
+#endif
+
+    /* 2nd calibration - P */
+    /* Select 10U% VREF */
+#if STM32_OPAMP_USE_OPAMP1
+  MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_10P);
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+  MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_10P);
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+  MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_10P);
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+  MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_CALSEL, OPAMPx_CSR_CALSEL_10P);
+#endif
+
+  delta = 8U;
+
+  while (delta != 0U)   {
+#if STM32_OPAMP_USE_OPAMP1
+    MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep1<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+    MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep2<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+    MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep3<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+    MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep4<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+
+
+    /* OFFTRIMmax delay 2 ms as per datasheet (electrical characteristics */
+    /* Offset trim time: during calibration, minimum time needed between */
+    /* two steps to have 1 mV accuracy */
+    chSysPolledDelayX(MS2RTC(STM32_SYSCLK, 2));
+#if STM32_OPAMP_USE_OPAMP1
+    if (OPAMPD1.opamp->CSR & OPAMP_CSR_OUTCAL)   {
+      /* OPAMP_CSR_OUTCAL is HIGH try higher trimming */
+      trimmingvaluep1 += delta;
+    } else  {
+      /* OPAMP_CSR_OUTCAL is LOW try lower trimming */
+      trimmingvaluep1 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+    if (OPAMPD2.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep2 += delta;
+    } else {
+      trimmingvaluep2 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+    if (OPAMPD3.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep3 += delta;
+    } else {
+      trimmingvaluep3 -= delta;
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+    if (OPAMPD4.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep4 += delta;
+    }  else {
+      trimmingvaluep4 -= delta;
+    }
+#endif
+
+    delta >>= 1U;
+  }
+
+  /* Still need to check if righ calibration is current value or un step below */
+  /* Indeed the first value that causes the OUTCAL bit to change from 1 to 0U */
+#if STM32_OPAMP_USE_OPAMP1
+  MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+       trimmingvaluep1<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+  MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+       trimmingvaluep2<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+  MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+       trimmingvaluep3<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+  MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+       trimmingvaluep4<<OPAMP_CSR_TRIMOFFSETP_Pos);
+#endif
+
+  /* OFFTRIMmax delay 2 ms as per datasheet (electrical characteristics */
+  /* Offset trim time: during calibration, minimum time needed between */
+  /* two steps to have 1 mV accuracy */
+  chSysPolledDelayX(MS2RTC(STM32_SYSCLK, 2));
+
+#if STM32_OPAMP_USE_OPAMP1
+    if (OPAMPD1.opamp->CSR & OPAMP_CSR_OUTCAL)   {
+      /* OPAMP_CSR_OUTCAL is HIGH try higher trimming */
+      trimmingvaluep1 += (trimmingvaluep1 != 31);
+      MODIFY_REG(OPAMPD1.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+     trimmingvaluep1<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+    if (OPAMPD2.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep2 += (trimmingvaluep2 != 31);
+      MODIFY_REG(OPAMPD2.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+     trimmingvaluep2<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+    if (OPAMPD3.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep3 += (trimmingvaluep3 != 31);
+      MODIFY_REG(OPAMPD3.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+     trimmingvaluep3<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+    if (OPAMPD4.opamp->CSR & OPAMP_CSR_OUTCAL)    {
+      trimmingvaluep4 += (trimmingvaluep4 != 31);
+      MODIFY_REG(OPAMPD4.opamp->CSR, OPAMP_CSR_TRIMOFFSETP,
+     trimmingvaluep4<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    }
+#endif
+
+#if STM32_OPAMP_USE_OPAMP1
+#define CSRm OPAMPD1.opamp->CSR
+    /* Disable calibration */
+    CSRm &= ~OPAMP_CSR_CALON;
+    /* Disable the OPAMPs */
+    CSRm &= ~OPAMP_CSR_OPAMPxEN;
+    /* Set normal operating mode back */
+    CSRm &= ~OPAMP_CSR_FORCEVP;
+    /* Write calibration result N */
+    OPAMPD1.trim_n = trimmingvaluen1;
+    /* Write calibration result P */
+    OPAMPD1.trim_p = trimmingvaluep1;
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen1<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep1<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    OPAMPD1.state = OPAMP_STOP;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP2
+#define CSRm OPAMPD2.opamp->CSR
+    /* Disable calibration */
+    CSRm &= ~OPAMP_CSR_CALON;
+    /* Disable the OPAMPs */
+    CSRm &= ~OPAMP_CSR_OPAMPxEN;
+    /* Set normal operating mode back */
+    CSRm &= ~OPAMP_CSR_FORCEVP;
+    /* Write calibration result N */
+    OPAMPD2.trim_n = trimmingvaluen2;
+    /* Write calibration result P */
+    OPAMPD2.trim_p = trimmingvaluep2;
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen2<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep2<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    OPAMPD2.state = OPAMP_STOP;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP3
+#define CSRm OPAMPD3.opamp->CSR
+    /* Disable calibration */
+    CSRm &= ~OPAMP_CSR_CALON;
+    /* Disable the OPAMPs */
+    CSRm &= ~OPAMP_CSR_OPAMPxEN;
+    /* Set normal operating mode back */
+    CSRm &= ~OPAMP_CSR_FORCEVP;
+    /* Write calibration result N */
+    OPAMPD3.trim_n = trimmingvaluen3;
+    /* Write calibration result P */
+    OPAMPD3.trim_p = trimmingvaluep3;
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen3<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep3<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    OPAMPD3.state = OPAMP_STOP;
+#undef CSRm
+#endif
+
+#if STM32_OPAMP_USE_OPAMP4
+#define CSRm OPAMPD4.opamp->CSR
+    /* Disable calibration */
+    CSRm &= ~OPAMP_CSR_CALON;
+    /* Disable the OPAMPs */
+    CSRm &= ~OPAMP_CSR_OPAMPxEN;
+    /* Set normal operating mode back */
+    CSRm &= ~OPAMP_CSR_FORCEVP;
+    /* Write calibration result N */
+    OPAMPD4.trim_n = trimmingvaluen4;
+    /* Write calibration result P */
+    OPAMPD4.trim_p = trimmingvaluep4;
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETN,
+         trimmingvaluen4<<OPAMP_CSR_TRIMOFFSETN_Pos);
+    MODIFY_REG(CSRm, OPAMP_CSR_TRIMOFFSETP,
+         trimmingvaluep4<<OPAMP_CSR_TRIMOFFSETP_Pos);
+    OPAMPD4.state = OPAMP_STOP;
+#undef CSRm
+#endif
+}
+
+void opamp_lld_calibrate(void)
+{
+  uint8_t trim_n[4] = {255};
+  uint8_t trim_p[4] = {255};
+  bool done;
+  do {
+    done = true;
+    opamp_lld_calibrate_once();
+#if STM32_OPAMP_USE_OPAMP1
+    done = done && (OPAMPD1.trim_n == trim_n[0]) &&  (OPAMPD1.trim_p == trim_p[0]);
+    trim_n[0] = OPAMPD1.trim_n;
+    trim_p[0] = OPAMPD1.trim_p;
+#endif
+#if STM32_OPAMP_USE_OPAMP2
+    done = done && (OPAMPD2.trim_n == trim_n[1]) &&  (OPAMPD2.trim_p == trim_p[1]);
+    trim_n[1] = OPAMPD2.trim_n;
+    trim_p[1] = OPAMPD2.trim_p;
+#endif
+#if STM32_OPAMP_USE_OPAMP3
+    done = done && (OPAMPD3.trim_n == trim_n[2]) &&  (OPAMPD3.trim_p == trim_p[2]);
+    trim_n[2] = OPAMPD3.trim_n;
+    trim_p[2] = OPAMPD3.trim_p;
+#endif
+#if STM32_OPAMP_USE_OPAMP4
+    done = done && (OPAMPD4.trim_n == trim_n[3]) &&  (OPAMPD4.trim_p == trim_p[3]);
+    trim_n[3] = OPAMPD4.trim_n;
+    trim_p[3] = OPAMPD4.trim_p;
+#endif
+  } while (!done);
+
+}
+#endif // STM32_OPAMP_USER_TRIM_ENABLED
 #endif /* HAL_USE_OPAMP */
 
 /** @} */
diff --git a/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.h b/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.h
index 508cea7..6a701b9 100644
--- a/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.h
+++ b/os/hal/ports/STM32/LLD/OPAMPv1/hal_opamp_lld.h
@@ -34,47 +34,29 @@
 /* Driver constants.                                                         */
 /*===========================================================================*/
 
+#define OPAMPx_CSR_PGAGAIN_x2                    ((uint32_t)0x00000000)
+#define OPAMPx_CSR_PGAGAIN_x4                    OPAMP_CSR_PGGAIN_0
+#define OPAMPx_CSR_PGAGAIN_x8                    OPAMP_CSR_PGGAIN_1
+#define OPAMPx_CSR_PGAGAIN_x16                   ((uint32_t)0x0000C000)
+#define OPAMPx_CSR_PGAGAIN_LESS_IFB_VM0          (0b10 << 16)
+#define OPAMPx_CSR_PGAGAIN_LESS_IFB_VM1          (0b11 << 16)
 
-#define STM32_OPAMP_InvertingInput_IO1          ((uint32_t)0x00000000) /*!< IO1 (PC5 for OPAMP1 and OPAMP2, PB10 for OPAMP3 and OPAMP4)
-                                                                       connected to OPAMPx inverting input */
-#define STM32_OPAMP_InvertingInput_IO2          OPAMP_CSR_VMSEL_0      /*!< IO2 (PA3 for OPAMP1, PA5 for OPAMP2, PB2 for OPAMP3, PD8 for OPAMP4)
-                                                                       connected to OPAMPx inverting input */
-#define STM32_OPAMP_InvertingInput_PGA          OPAMP_CSR_VMSEL_1      /*!< Resistor feedback output connected to OPAMPx inverting input (PGA mode) */
-#define STM32_OPAMP_InvertingInput_Vout         OPAMP_CSR_VMSEL        /*!< Vout connected to OPAMPx inverting input (follower mode) */
+#define OPAMPx_CSR_PGACONNECT_GROUND             ((uint32_t)0x00000000)
+#define OPAMPx_CSR_PGACONNECT_IO1                OPAMP_CSR_PGGAIN_3
+#define OPAMPx_CSR_PGACONNECT_IO2                ((uint32_t)0x00030000)
 
+#define OPAMPx_CSR_CALSEL_3P3                    ((uint32_t)0x00000000)
+#define OPAMPx_CSR_CALSEL_10P                    OPAMP_CSR_CALSEL_0
+#define OPAMPx_CSR_CALSEL_50P                    OPAMP_CSR_CALSEL_1
+#define OPAMPx_CSR_CALSEL_90P                    OPAMP_CSR_CALSEL
 
-#define STM32_OPAMP_NonInvertingInput_IO1       ((uint32_t)0x00000000) /*!< IO1 (PA7 for OPAMP1, PD14 for OPAMP2, PB13 for OPAMP3, PD11 for OPAMP4)
-                                                                       connected to OPAMPx non inverting input */
-#define STM32_OPAMP_NonInvertingInput_IO2       OPAMP_CSR_VPSEL_0      /*!< IO2 (PA5 for OPAMP1, PB14 for OPAMP2, PA5 for OPAMP3, PB11 for OPAMP4)
-                                                                       connected to OPAMPx non inverting input */
-#define STM32_OPAMP_NonInvertingInput_IO3       OPAMP_CSR_VPSEL_1      /*!< IO3 (PA3 for OPAMP1, PB0 for OPAMP2, PA1 for OPAMP3, PA4 for OPAMP4)
-                                                                       connected to OPAMPx non inverting input */
-#define STM32_OPAMP_NonInvertingInput_IO4       OPAMP_CSR_VPSEL        /*!< IO4 (PA1 for OPAMP1, PA7 for OPAMP2, PB0 for OPAMP3, PB13 for OPAMP4)
-                                                                       connected to OPAMPx non inverting input */
+#define OPAMPx_CSR_TRIM_FACTORY                  ((uint32_t)0x00000000)
+#define OPAMPx_CSR_TRIM_USER                     OPAMP_CSR_USERTRIM     /*!< User trimming */
 
+#define OPAMPx_CSR_OUTPUT_NORMAL                 ((uint32_t)0x00000000)
+#define OPAMPx_CSR_OUTPUT_INVERTED               OPAMP_CSR_OUTCAL
 
-#define STM32_OPAMP_PGAGain_2                   ((uint32_t)0x00000000)
-#define STM32_OPAMP_PGAGain_4                   OPAMP_CSR_PGGAIN_0
-#define STM32_OPAMP_PGAGain_8                   OPAMP_CSR_PGGAIN_1
-#define STM32_OPAMP_PGAGain_16                  ((uint32_t)0x0000C000)
-
-#define STM32_OPAMP_PGAConnect_No               ((uint32_t)0x00000000)
-#define STM32_OPAMP_PGAConnect_IO1              OPAMP_CSR_PGGAIN_3
-#define STM32_OPAMP_PGAConnect_IO2              ((uint32_t)0x00030000)
-
-#define STM32_OPAMP_Input_Inverting             ((uint32_t)0x00000018) /*!< Inverting input */
-#define STM32_OPAMP_Input_NonInverting          ((uint32_t)0x00000013) /*!< Non inverting input */
-
-#define STM32_OPAMP_Vref_3VDDA                  ((uint32_t)0x00000000) /*!< OPMAP Vref = 3.3% VDDA */
-#define STM32_OPAMP_Vref_10VDDA                 OPAMP_CSR_CALSEL_0     /*!< OPMAP Vref = 10% VDDA  */
-#define STM32_OPAMP_Vref_50VDDA                 OPAMP_CSR_CALSEL_1     /*!< OPMAP Vref = 50% VDDA  */
-#define STM32_OPAMP_Vref_90VDDA                 OPAMP_CSR_CALSEL       /*!< OPMAP Vref = 90% VDDA  */
-
-#define STM32_OPAMP_Trimming_Factory            ((uint32_t)0x00000000) /*!< Factory trimming */
-#define STM32_OPAMP_Trimming_User               OPAMP_CSR_USERTRIM     /*!< User trimming */
-
-#define STM32_OPAMP_OutputLevel_High            OPAMP_CSR_OUTCAL
-#define STM32_OPAMP_OutputLevel_Low             ((uint32_t)0x00000000)
+#define OPAMPx_CSR_LOCK                          OPAMP_CSR_LOCK
 
 
 #if defined(STM32F302xB) || defined(STM32F302xC) || defined(STM32F302xD) \
@@ -97,7 +79,87 @@
 #define STM32_HAS_OPAMP2 FALSE
 #define STM32_HAS_OPAMP3 FALSE
 #define STM32_HAS_OPAMP4 FALSE
+#endif
+
+
+#if STM32_HAS_OPAMP1
+#define OPAMP1_CSR_VPSEL_PA07       	((uint32_t)0x00000000)
+#define OPAMP1_CSR_VPSEL_PA05     		OPAMP_CSR_VPSEL_0
+#define OPAMP1_CSR_VPSEL_PA03     		OPAMP_CSR_VPSEL_1
+#define OPAMP1_CSR_VPSEL_PA01     		OPAMP_CSR_VPSEL
+
+#define OPAMP1_CSR_VMSEL_PC05         ((uint32_t)0x00000000)
+#define OPAMP1_CSR_VMSEL_PA03     		OPAMP_CSR_VMSEL_0
+#define OPAMP1_CSR_VMSEL_PGA  				OPAMP_CSR_VMSEL_1
+#define OPAMP1_CSR_VMSEL_FOLWR				OPAMP_CSR_VMSEL
+
+#define OPAMP1_CSR_VMSSEL_PC05				((uint32_t)0x00000000)
+#define OPAMP1_CSR_VMSSEL_PA03				OPAMP_CSR_VMSSEL
+
+#define OPAMP1_CSR_VPSSEL_PA07				((uint32_t)0x00000000)
+#define OPAMP1_CSR_VPSSEL_PA05				OPAMP_CSR_VPSSEL_0
+#define OPAMP1_CSR_VPSSEL_PA03				OPAMP_CSR_VPSSEL_1
+#define OPAMP1_CSR_VPSSEL_PA01				OPAMP_CSR_VPSSEL
+#endif
+
+#if STM32_HAS_OPAMP2
+#define OPAMP2_CSR_VPSEL_PD14       	((uint32_t)0x00000000)
+#define OPAMP2_CSR_VPSEL_PB14   			OPAMP_CSR_VPSEL_0
+#define OPAMP2_CSR_VPSEL_PB00       	OPAMP_CSR_VPSEL_1
+#define OPAMP2_CSR_VPSEL_PA07     		OPAMP_CSR_VPSEL
+
+#define OPAMP2_CSR_VMSEL_PC05     		((uint32_t)0x00000000)
+#define OPAMP2_CSR_VMSEL_PA05     		OPAMP_CSR_VMSEL_0
+#define OPAMP2_CSR_VMSEL_PGA		  		OPAMP_CSR_VMSEL_1
+#define OPAMP2_CSR_VMSEL_FOLWR				OPAMP_CSR_VMSEL
+
+#define OPAMP2_CSR_VMSSEL_PC05				((uint32_t)0x00000000)
+#define OPAMP2_CSR_VMSSEL_PA05				OPAMP_CSR_VMSSEL
+
+#define OPAMP2_CSR_VPSSEL_PD14				((uint32_t)0x00000000)
+#define OPAMP2_CSR_VPSSEL_PB14				OPAMP_CSR_VPSSEL_0
+#define OPAMP2_CSR_VPSSEL_PB00				OPAMP_CSR_VPSSEL_1
+#define OPAMP2_CSR_VPSSEL_PA07				OPAMP_CSR_VPSSEL
+#endif
+
+#if STM32_HAS_OPAMP3
+#define OPAMP3_CSR_VPSEL_PB13       	((uint32_t)0x00000000)
+#define OPAMP3_CSR_VPSEL_PA05       	OPAMP_CSR_VPSEL_0
+#define OPAMP3_CSR_VPSEL_PA01     		OPAMP_CSR_VPSEL_1
+#define OPAMP3_CSR_VPSEL_PB00       	OPAMP_CSR_VPSEL
+
+#define OPAMP3_CSR_VMSEL_PB10         ((uint32_t)0x00000000)
+#define OPAMP3_CSR_VMSEL_PB02       	OPAMP_CSR_VMSEL_0
+#define OPAMP3_CSR_VMSEL_PGA		  		OPAMP_CSR_VMSEL_1
+#define OPAMP3_CSR_VMSEL_FOLWR				OPAMP_CSR_VMSEL
+
+#define OPAMP3_CSR_VMSSEL_PB10				((uint32_t)0x00000000)
+#define OPAMP3_CSR_VMSSEL_PB02				OPAMP_CSR_VMSSEL
+
+#define OPAMP3_CSR_VPSSEL_PB13				((uint32_t)0x00000000)
+#define OPAMP3_CSR_VPSSEL_PA05				OPAMP_CSR_VPSSEL_0
+#define OPAMP3_CSR_VPSSEL_PA01				OPAMP_CSR_VPSSEL_1
+#define OPAMP3_CSR_VPSSEL_PB00				OPAMP_CSR_VPSSEL
+#endif
+
+#if STM32_HAS_OPAMP4
+#define OPAMP4_CSR_VPSEL_PD11         ((uint32_t)0x00000000)
+#define OPAMP4_CSR_VPSEL_PB11         OPAMP_CSR_VPSEL_0
+#define OPAMP4_CSR_VPSEL_PA04       	OPAMP_CSR_VPSEL_1
+#define OPAMP4_CSR_VPSEL_PB13       	OPAMP_CSR_VPSEL
+
+#define OPAMP4_CSR_VMSEL_PB10       	((uint32_t)0x00000000)
+#define OPAMP4_CSR_VMSEL_PD08       	OPAMP_CSR_VMSEL_0
+#define OPAMP4_CSR_VMSEL_PGA	  			OPAMP_CSR_VMSEL_1
+#define OPAMP4_CSR_VMSEL_FOLWR				OPAMP_CSR_VMSEL
 
+#define OPAMP4_CSRVMSSEL_PB10				((uint32_t)0x00000000)
+#define OPAMP4_CSR_VMSSEL_PD08				OPAMP_CSR_VMSSEL
+
+#define OPAMP4_CSR_VPSSEL_PD11				((uint32_t)0x00000000)
+#define OPAMP4_CSR_VPSSEL_PB11				OPAMP_CSR_VPSSEL_0
+#define OPAMP4_CSR_VPSSEL_PA04				OPAMP_CSR_VPSSEL_1
+#define OPAMP4_CSR_VPSSEL_PB13				OPAMP_CSR_VPSSEL
 #endif
 
 
@@ -146,6 +208,15 @@
 #define STM32_OPAMP_USE_OPAMP4                  FALSE
 #endif
 
+/**
+ * @brief   OPAMPD TRIM and CALIBRATION enable switch.
+ * @details If set to @p TRUE the support for USER_TRIM is included and calibration is done @init
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_OPAMP_USER_TRIM_ENABLED) || defined(__DOXYGEN__)
+#define STM32_OPAMP_USER_TRIM_ENABLED		TRUE
+#endif
+
 
 /** @} */
 
@@ -210,7 +281,12 @@ struct OPAMPDriver {
   /**
    * @brief Pointer to the OPAMPx registers block.
    */
-  OPAMP_TypeDef               *reg;
+  OPAMP_TypeDef               *opamp;
+
+#if STM32_OPAMP_USER_TRIM_ENABLED
+  uint16_t			trim_p;
+  uint16_t			trim_n;
+#endif
 };
 
 /*===========================================================================*/
@@ -246,6 +322,9 @@ extern "C" {
   void opamp_lld_stop(OPAMPDriver *compp);
   void opamp_lld_enable(OPAMPDriver *compp);
   void opamp_lld_disable(OPAMPDriver *compp);
+#if STM32_OPAMP_USER_TRIM_ENABLED
+  void opamp_lld_calibrate(void);
+#endif
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/src/hal_opamp.c b/os/hal/src/hal_opamp.c
index ba718a7..79a5ec8 100644
--- a/os/hal/src/hal_opamp.c
+++ b/os/hal/src/hal_opamp.c
@@ -86,11 +86,11 @@ void opampStart(OPAMPDriver *opampp, const OPAMPConfig *config) {
   osalDbgCheck((opampp != NULL) && (config != NULL));
 
   osalSysLock();
-  osalDbgAssert((opampp->state == OPAMP_STOP) || (opampp->state == OPAMP_READY),
+  osalDbgAssert((opampp->state == OPAMP_STOP) || (opampp->state == OPAMP_ACTIVE),
               "invalid state");
   opampp->config = config;
   opamp_lld_start(opampp);
-  opampp->state = OPAMP_READY;
+  opampp->state = OPAMP_ACTIVE;
   osalSysUnlock();
 }
 
@@ -106,7 +106,7 @@ void opampStop(OPAMPDriver *opampp) {
   osalDbgCheck(opampp != NULL);
 
   osalSysLock();
-  osalDbgAssert((opampp->state == OPAMP_STOP) || (opampp->state == OPAMP_READY),
+  osalDbgAssert((opampp->state == OPAMP_STOP) || (opampp->state == OPAMP_ACTIVE),
               "invalid state");
   opamp_lld_stop(opampp);
   opampp->state = OPAMP_STOP;
@@ -125,7 +125,7 @@ void opampEnable(OPAMPDriver *opampp) {
   osalDbgCheck(opampp != NULL);
 
   osalSysLock();
-  osalDbgAssert(opampp->state == OPAMP_READY, "invalid state");
+  osalDbgAssert(opampp->state == OPAMP_ACTIVE, "invalid state");
   opamp_lld_enable(opampp);
   opampp->state = OPAMP_ACTIVE;
   osalSysUnlock();
@@ -143,10 +143,10 @@ void opampDisable(OPAMPDriver *opampp) {
   osalDbgCheck(opampp != NULL);
 
   osalSysLock();
-  osalDbgAssert((opampp->state == OPAMP_READY) || (opampp->state == OPAMP_ACTIVE),
+  osalDbgAssert((opampp->state == OPAMP_ACTIVE),
              "invalid state");
   opamp_lld_disable(opampp);
-  opampp->state = OPAMP_READY;
+  opampp->state = OPAMP_ACTIVE;
   osalSysUnlock();
 }