Fix STM32MP2 timer cleanup.

libplatsupport/plat_include/stm32mp2/platsupport/plat/timer.h

@@ -60,9 +60,11 @@ typedef struct {
     volatile stm32_regs_t *hw;
     uint32_t periodic_t;
     uint32_t cnt_h; /* keep overflow for get_time */
+    void *rcc;
 } stm32_t;
 
 int stm32mp2_timer_init(stm32_t *stm32, ps_io_ops_t ops);
+void stm32mp2_timer_destroy(stm32_t *stm32, ps_io_ops_t ops);
 void stm32_timer_reset(stm32_t *stm32);
 uint64_t stm32_get_time(stm32_t *stm32);
 int stm32_set_timeout(stm32_t *dmt, uint64_t ns, bool periodic);

libplatsupport/src/plat/stm32mp2/ltimer.c

@@ -83,17 +83,17 @@ static void destroy(void *data)
 {
     stm32_ltimer_t *stm32_ltimer = data;
 
-    stm32_stop_timer(&stm32_ltimer->stm32_timer);
-
-    if (stm32_ltimer->stm32_timer.hw) {
-        ps_pmem_unmap(&stm32_ltimer->ops, pmems, (void *) stm32_ltimer->stm32_timer.hw);
-    }
+    stm32mp2_timer_destroy(&stm32_ltimer->stm32_timer, stm32_ltimer->ops);
 
     if (stm32_ltimer->irq_id != PS_INVALID_IRQ_ID) {
         int error = ps_irq_unregister(&stm32_ltimer->ops.irq_ops, stm32_ltimer->irq_id);
         ZF_LOGF_IF(error, "Failed to unregister IRQ");
     }
 
+    if (stm32_ltimer->stm32_timer.hw) {
+        ps_pmem_unmap(&stm32_ltimer->ops, pmems, (void *) stm32_ltimer->stm32_timer.hw);
+    }
+
     ps_free(&stm32_ltimer->ops.malloc_ops, sizeof(stm32_ltimer_t), stm32_ltimer);
 }
 
@@ -159,8 +159,8 @@ int ltimer_default_init(ltimer_t *ltimer, ps_io_ops_t ops, ltimer_callback_fn_t
 
     stm32_map_base = ps_pmem_map(&ops, pmems, false, PS_MEM_NORMAL);
     if (stm32_map_base == NULL) {
-        destroy(ltimer->data);
-        return EINVAL;
+        rc = EINVAL;
+        goto err_pmem_map;
     }
     stm32_ltimer->stm32_timer.hw = stm32_map_base;
 
@@ -171,16 +171,26 @@ int ltimer_default_init(ltimer_t *ltimer, ps_io_ops_t ops, ltimer_callback_fn_t
     stm32_ltimer->irq_id = ps_irq_register(&ops.irq_ops, irqs, handle_irq_wrapper,
                                            &stm32_ltimer->callback_data);
     if (stm32_ltimer->irq_id < 0) {
-        destroy(ltimer->data);
-        return EIO;
+        rc = EIO;
+        goto err_irq_register;
     }
 
     rc = stm32mp2_timer_init(&stm32_ltimer->stm32_timer, ops);
     if (rc) {
         ZF_LOGE("Failed to init stm32 timeout timer");
-        destroy(&stm32_ltimer);
-        return rc;
+        goto err_timer_init;
     }
 
     return 0;
+
+err_timer_init:
+    ps_irq_unregister(&ops.irq_ops, stm32_ltimer->irq_id);
+
+err_irq_register:
+    ps_pmem_unmap(&ops, pmems, (void *) stm32_ltimer->stm32_timer.hw);
+
+err_pmem_map:
+    ps_free(&ops.malloc_ops, sizeof(stm32_ltimer_t), stm32_ltimer);
+
+    return rc;
 }

libplatsupport/src/plat/stm32mp2/timer.c

@@ -89,26 +89,36 @@ void enable_rcc(void *rcc)
     while (!(*rcc_cfgr & (BIT(2) | BIT(1))));
 }
 
-int stm32mp2_timer_init(stm32_t *stm32, ps_io_ops_t ops)
+void stm32mp2_timer_destroy(stm32_t *stm32_timer,  ps_io_ops_t ops)
+{
+    if (stm32_timer->rcc == NULL) {
+        return;
+    }
+
+    stm32_stop_timer(stm32_timer);
+    ps_io_unmap(&ops.io_mapper, stm32_timer->rcc, RCC_TIM2_SIZE);
+}
+
+int stm32mp2_timer_init(stm32_t *stm32_timer, ps_io_ops_t ops)
 {
-    void *rcc = NULL;
     ps_io_ops_t *io_ops = &ops;
 
     /* TIM2 RCC */
-    MAP_IF_NULL(io_ops, RCC_TIM2, rcc);
-    if (rcc == NULL) {
+    MAP_IF_NULL(io_ops, RCC_TIM2, stm32_timer->rcc);
+    if (stm32_timer->rcc == NULL) {
+        ZF_LOGE("Failed map TIM2 RCC");
         return EINVAL;
     }
 
-    enable_rcc(rcc);
+    enable_rcc(stm32_timer->rcc);
 
-    stm32->hw->psc = (uint16_t)((TICK_NS * TIM2_HZ) / NS_IN_S) - 1;
-    stm32->hw->arr = UINT32_MAX;
-    stm32->hw->egr  = STM32_TIM_EGR_UG;    /* force update event */
-    stm32->hw->dier = STM32_TIM_DIER_UIE;  /* enable interrupt for overflow */
-    stm32->hw->ccer = STM32_TIM_CCER_CC1E; /* enable channel1 output */
+    stm32_timer->hw->psc = (uint16_t)((TICK_NS * TIM2_HZ) / NS_IN_S) - 1;
+    stm32_timer->hw->arr = UINT32_MAX;
+    stm32_timer->hw->egr  = STM32_TIM_EGR_UG;    /* force update event */
+    stm32_timer->hw->dier = STM32_TIM_DIER_UIE;  /* enable interrupt for overflow */
+    stm32_timer->hw->ccer = STM32_TIM_CCER_CC1E; /* enable channel1 output */
 
-    stm32_start_timer(stm32);
+    stm32_start_timer(stm32_timer);
 
     return 0;
 }