#include "stm32mp1xx.h"
#include "core_delay.h"
#include "rtthread.h"
#include <rthw.h>


#define DWT_CR      *(__IO uint32_t *)0xE0001000
#define DWT_CYCCNT  *(__IO uint32_t *)0xE0001004
#define DEM_CR      *(__IO uint32_t *)0xE000EDFC

#define DEM_CR_TRCENA (1 << 24)
#define DWT_CR_CYCCNTENA (1 << 0)




//获取系统主频
static uint32_t get_sys_clocks_freq (void)
{
  return HAL_RCC_GetMCUFreq();
}





int delay_init(void)
{
  DEM_CR |= (uint32_t)DEM_CR_TRCENA;
  DWT_CYCCNT = (uint32_t)0u;
  DWT_CR |= (uint32_t)DWT_CR_CYCCNTENA;
  return 0;
}


static uint32_t delay_get_cnt(void)
{
  return ((uint32_t)DWT_CYCCNT);
}

static uint32_t delay_get_tick(void)
{
  return ((uint32_t)DWT_CYCCNT*1000/get_sys_clocks_freq());
}



// 低于1ms的精确延时
void delay_us(uint32_t us)
{
  uint32_t ticks;
  uint32_t told, tnow, tcnt=0;

  /* 需要的节拍数 */
  ticks = us * (get_sys_clocks_freq() / 1000000);
  tcnt = 0;
  told = (uint32_t)delay_get_cnt();

  while (1) 
  {
    tnow = (uint32_t)delay_get_cnt();
    if (tnow != told) 
    {
      /* 32 位计数器是递增计数器 */
      if (tnow > told) 
      {
        tcnt += tnow - told;
      }
      /* 重新装载 */
      else 
      {
        tcnt += UINT32_MAX - told + tnow;
      }

      told = tnow;

      /*时间超过/等于要延迟的时间,则退出 */
      if (tcnt >= ticks)break;
    }
  }
}


// 实现rt_thread 的us级延时
void rt_hw_us_delay(rt_uint32_t us)
{
  delay_us(us);
}


void delay_ms(uint32_t ms)
{
  for(int i=0;i<ms;i++)
  {
    delay_us(1000);
  }
}