arch/arm64/imx9: Fix cntrfrq_el0 to correct value

arch/arm64/src/imx9/Make.defs

@@ -75,3 +75,7 @@ endif
 ifeq ($(CONFIG_IMX9_FLEXSPI_NOR), y)
   CHIP_CSRCS += imx9_flexspi_nor.c
 endif
+
+ifeq ($(CONFIG_IMX9_BOOTLOADER), y)
+  CHIP_CSRCS += imx9_system_ctl.c
+endif

arch/arm64/src/imx9/imx9_boot.c

@@ -43,7 +43,7 @@
 #include "imx9_serial.h"
 #include "imx9_gpio.h"
 #include "imx9_lowputc.h"
-
+#include "imx9_system_ctl.h"
 /****************************************************************************
  * Private Data
  ****************************************************************************/
@@ -95,7 +95,12 @@ void arm64_el_init(void)
 #if (CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3)
   /* At EL3, cntfrq_el0 is uninitialized. It must be set. */
 
-  write_sysreg(CONFIG_BOOTLOADER_SYS_CLOCK, cntfrq_el0);
+  uint32_t freq;
+
+  freq = getreg32(IMX9_SYS_CTR_CONTROL_BASE + SYS_CTR_CNTFID0);
+  write_sysreg(freq, cntfrq_el0);
+  modifyreg32(IMX9_SYS_CTR_CONTROL_BASE + SYS_CTR_CNTCR, SC_CNTCR_FREQ0 |
+    SC_CNTCR_FREQ1, SC_CNTCR_FREQ0 | SC_CNTCR_ENABLE | SC_CNTCR_HDBG);
 #endif
 }
 
@@ -109,6 +114,9 @@ void arm64_el_init(void)
 
 void arm64_chip_boot(void)
 {
+#ifdef CONFIG_IMX9_BOOTLOADER
+  imx9_mix_powerup();
+#endif
   /* MAP IO and DRAM, enable MMU. */
 
   arm64_mmu_init(true);
@@ -118,7 +126,6 @@ void arm64_chip_boot(void)
   imx9_clockconfig();
 
   /* Do UART early initialization & pin muxing */
-
 #ifdef CONFIG_IMX9_LPUART
   imx9_lowsetup();
 #endif

arch/arm64/src/imx9/imx9_system_ctl.c

@@ -0,0 +1,131 @@
+/****************************************************************************
+ * arch/arm64/src/imx9/imx9_system_ctl.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <assert.h>
+#include <debug.h>
+
+#include <nuttx/cache.h>
+#ifdef CONFIG_PAGING
+#  include <nuttx/page.h>
+#endif
+
+#include <arch/chip/chip.h>
+#include "imx9_system_ctl.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+void imx9_mix_powerup(void)
+{
+  uint32_t val = 0;
+
+  /* Authen ctrl, enable NS access to slice registers */
+
+  modifyreg32(IMX9_SRC_MEDIA_SLICE_BASE
+    + SRC_SLICE_AUTHEN_CTRL_OFFSET, 0, BIT(9));
+  modifyreg32(IMX9_SRC_ML_SLICE_BASE
+    + SRC_SLICE_AUTHEN_CTRL_OFFSET, 0, BIT(9));
+
+  /* Enable s400 handsake */
+
+  modifyreg32(IMX9_BLK_CTRL_S_AONMIX2_BASE
+    + AON_MIX_LP_HANDSAKE, 0, BIT(13));
+
+  /* Counter mode */
+
+  modifyreg32(IMX9_SRC_MEDIA_SLICE_BASE
+    + SRC_SLICE_PSW_ACK_CTRL0_OFFSET, BIT(28), BIT(29));
+  modifyreg32(IMX9_SRC_ML_SLICE_BASE
+    + SRC_SLICE_PSW_ACK_CTRL0_OFFSET, BIT(28), BIT(29));
+
+  /* release media and ml from reset */
+
+  modifyreg32(IMX9_SRC_GENERAL_REG_BASE
+    + SRC_CTRL_OFFSET, 0, (BIT(4) | BIT(5)));
+
+  /* Enable mem in Low power auto sequence */
+
+  modifyreg32(IMX9_SRC_MEDIA_MEM_BASE + MEM_CTRL_OFFSET, 0, BIT(2));
+  modifyreg32(IMX9_SRC_ML_MEM_BASE + MEM_CTRL_OFFSET, 0, BIT(2));
+
+  /* Mediamix powerdown */
+
+  modifyreg32(IMX9_SRC_MEDIA_SLICE_BASE
+    + SRC_SLICE_SW_CTRL_OFFSET, 0, BIT(31));
+
+  val = getreg32(IMX9_SRC_MEDIA_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+  if (val & 1)
+    {
+      while (!(val & BIT(12)))
+        val = getreg32(IMX9_SRC_MEDIA_SLICE_BASE
+          + SRC_SLICE_FUNC_STAT_OFFSET);
+
+      up_udelay(1);
+    }
+  else
+    {
+      while (!(val & BIT(0)))
+        val = getreg32(IMX9_SRC_MEDIA_SLICE_BASE
+          + SRC_SLICE_FUNC_STAT_OFFSET);
+    }
+
+  /* Power on */
+
+  modifyreg32(IMX9_SRC_MEDIA_SLICE_BASE
+    + SRC_SLICE_SW_CTRL_OFFSET, BIT(31), 0);
+  while (!(val & BIT(4)))
+    val = getreg32(IMX9_SRC_MEDIA_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+
+  /* ML powerdown */
+
+  modifyreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_SW_CTRL_OFFSET, 0, BIT(31));
+
+  val = getreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+  if (val & 1)
+    {
+      while (!(val & BIT(12)))
+        val = getreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+      up_udelay(1);
+    }
+  else
+    {
+      while (!(val & BIT(0)))
+        val = getreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+    }
+
+  /* Power on */
+
+  modifyreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_SW_CTRL_OFFSET, BIT(31), 0);
+  while (!(val & BIT(4)))
+      val = getreg32(IMX9_SRC_ML_SLICE_BASE + SRC_SLICE_FUNC_STAT_OFFSET);
+
+  /* Disable isolation usb, dsi, csi */
+
+  putreg32(0, IMX9_SRC_GENERAL_REG_BASE + SRC_SP_ISO_CTRL_OFFSET);
+}
+

arch/arm64/src/imx9/imx9_system_ctl.h

@@ -0,0 +1,88 @@
+/****************************************************************************
+ * arch/arm64/src/imx9/imx9_system_ctl.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __ARCH_ARM64_SRC_IMX9_IMX9_SYSTEM_CTL_H
+#define __ARCH_ARM64_SRC_IMX9_IMX9_SYSTEM_CTL_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <arch/board/board.h>
+#include "arm64_internal.h"
+#include "hardware/imx9_memorymap.h"
+
+#define SRC_CTRL_OFFSET             0x10
+#define SRC_SP_ISO_CTRL_OFFSET      0x10C
+#define MEM_CTRL_OFFSET             0x4
+#define SRC_SLICE_SW_CTRL_OFFSET    0x20
+#define SRC_SLICE_FUNC_STAT_OFFSET  0xb4
+#define SRC_SLICE_AUTHEN_CTRL_OFFSET   0x4
+#define SRC_SLICE_PSW_ACK_CTRL0_OFFSET 0x80
+
+#define SYS_CTR_CNTFID0             0x20
+#define SYS_CTR_CNTCR               0x0
+
+#define SC_CNTCR_ENABLE             0x1
+#define SC_CNTCR_HDBG               0x2
+#define SC_CNTCR_FREQ0              0x100
+#define SC_CNTCR_FREQ1              0x200
+
+#define AON_MIX_LP_HANDSAKE         0x110
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C"
+{
+#else
+#define EXTERN extern
+#endif
+
+/****************************************************************************
+ * Name: imx9_mix_powerup
+ *
+ * Description:
+ *  Powercycle ML and media mix and disable isolation
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void imx9_mix_powerup(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif
+
+#endif /* __ARCH_ARM64_SRC_IMX9_IMX9_SYSTEM_CTL_H */