ChinaUWBProject.git

对比新文件
			@@ -0,0 +1,529 @@
			/*
			* Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
			* its subsidiaries and affiliates (collectly called MKSEMI).
			*
			* All rights reserved.
			*
			* Redistribution and use in source and binary forms, with or without
			* modification, are permitted provided that the following conditions are met:
			*
			* 1. Redistributions of source code must retain the above copyright notice,
			* this list of conditions and the following disclaimer.
			*
			* 2. Redistributions in binary form, except as embedded into an MKSEMI
			* integrated circuit in a product or a software update for such product,
			* must reproduce the above copyright notice, this list of conditions and
			* the following disclaimer in the documentation and/or other materials
			* provided with the distribution.
			*
			* 3. Neither the name of MKSEMI nor the names of its contributors may be used
			* to endorse or promote products derived from this software without
			* specific prior written permission.
			*
			* 4. This software, with or without modification, must only be used with a
			* MKSEMI integrated circuit.
			*
			* 5. Any software provided in binary form under this license must not be
			* reverse engineered, decompiled, modified and/or disassembled.
			*
			* THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
			* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
			* MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
			* DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
			* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
			* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
			* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
			* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
			* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
			*/

			#include "mk_misc.h"
			#include "mk_dual_timer.h"
			#include "mk_clock.h"
			#include "mk_trace.h"
			#include "mk_sleep_timer.h"
			#include "mk_reset.h"
			// #include "board.h"

			uint32_t mk_chip_id(void)
			{
			return REG_READ(0x4000000C);
			}

			void mk_chip_uuid(uint8_t *uuid)
			{
			ASSERT(uuid, "uuid is null");
			uint8_t y = REG_READ_BYTE(0x40010074);
			uint8_t x = REG_READ_BYTE(0x40010075);
			uint8_t w = REG_READ_BYTE(0x40010076);
			uint8_t l = REG_READ_BYTE(0x4001007F) - 'A';

			uuid[0] = ((l << 3) & 0x80) \| (y & 0x7F);
			uuid[1] = ((l << 4) & 0x80) \| (x & 0x7f);
			uuid[2] = ((l << 5) & 0xE0) \| (w & 0x1f);
			uuid[3] = REG_READ_BYTE(0x4001007A);
			uuid[4] = REG_READ_BYTE(0x4001007B);
			uuid[5] = REG_READ_BYTE(0x4001007C);
			uuid[6] = REG_READ_BYTE(0x4001007D);
			uuid[7] = REG_READ_BYTE(0x4001007E);
			}

			/* BOD */

			static const struct BOD_BOR_CFG_T default_bod_cfg = {
			.level = BOD_BOR_TH_LVL0,
			.hyst = BOD_BOR_HYST_LVL0,
			};

			void bod_open(const struct BOD_BOR_CFG_T *cfg)
			{
			const struct BOD_BOR_CFG_T *config = (cfg == NULL) ? &default_bod_cfg : cfg;

			// enable BOD
			uint32_t bod_bor_cfg = SYSCON->BOD_BOR;
			bod_bor_cfg &= 0xFF00FFFF;
			bod_bor_cfg \|= SYSCON_BOD_EN_MSK \| SYSCON_BOD_TH_SEL(config->level) \| SYSCON_BOD_HYST_SEL(config->hyst);
			SYSCON->BOD_BOR = bod_bor_cfg;

			NVIC_SetPriority(BOD_IRQn, IRQ_PRIORITY_NORMAL);
			NVIC_ClearPendingIRQ(BOD_IRQn);
			NVIC_EnableIRQ(BOD_IRQn);
			}

			void bod_close(void)
			{
			// disable BOD
			SYSCON->BOD_BOR &= ~SYSCON_BOD_EN_MSK;

			NVIC_DisableIRQ(BOD_IRQn);
			NVIC_ClearPendingIRQ(BOD_IRQn);
			}

			void BOD_IRQHandler(void)
			{
			}

			/* BOR */

			static const struct BOD_BOR_CFG_T default_bor_cfg = {
			.level = BOD_BOR_TH_LVL0,
			.hyst = BOD_BOR_HYST_LVL0,
			};

			void bor_open(const struct BOD_BOR_CFG_T *cfg)
			{
			const struct BOD_BOR_CFG_T *config = (cfg == NULL) ? &default_bor_cfg : cfg;

			// enable BOR
			uint32_t bod_bor_cfg = SYSCON->BOD_BOR;
			bod_bor_cfg &= 0xFFFFFF00;
			bod_bor_cfg \|= SYSCON_BOR_EN_MSK \| SYSCON_BOR_TH_SEL(config->level) \| SYSCON_BOR_HYST_SEL(config->hyst);
			SYSCON->BOD_BOR = bod_bor_cfg;
			}

			void bor_close(void)
			{
			// disable BOD
			SYSCON->BOD_BOR &= ~SYSCON_BOR_EN_MSK;
			}

			/* SYS Timer */

			uint32_t sys_timer_freq = 0;

			void sys_timer_open(void)
			{
			// DIV = 1 (max: ~68.8s, resolution: ~16ns)
			// DIV = 16 (max: ~1101.2s, resolution: ~256ns)
			// DIV = 256 (max: ~17620.3s resolution: ~65us)
			struct DUAL_TIMER_CFG_T sys_timer_cfg = {
			.type = DUAL_TIMER_TYPE_FREERUNNING,
			.prescale = DUAL_TIMER_PRESCALE_DIV1,
			.width = DUAL_TIMER_SIZE_32BIT,
			.int_en = false,
			.load = 0xffffffff,
			.callback = NULL,
			};

			dual_timer_open(DUAL_TIMER_ID0, &sys_timer_cfg);
			dual_timer_start(DUAL_TIMER_ID0, sys_timer_cfg.load);
			uint16_t div = sys_timer_cfg.prescale == DUAL_TIMER_PRESCALE_DIV1 ? 1 : sys_timer_cfg.prescale == DUAL_TIMER_PRESCALE_DIV16 ? 16 : 256;
			sys_timer_freq = clock_get_frequency(CLOCK_APB_CLK) / div;
			}

			void sys_timer_close(void)
			{
			dual_timer_close(DUAL_TIMER_ID0);
			}

			uint32_t sys_timer_get(void)
			{
			return -dual_timer_get(DUAL_TIMER_ID0);
			}

			// max: 68829 us (DIV = 1)
			void sys_timer_delay_us(uint32_t time_us)
			{
			uint32_t start = dual_timer_get(DUAL_TIMER_ID0);
			uint32_t ticks = __US_TO_TICKS(time_us);

			while (start - dual_timer_get(DUAL_TIMER_ID0) < ticks)
			{
			}
			}

			// max: 68829 ms (DIV = 1)
			void sys_timer_delay_ms(uint32_t time_ms)
			{
			uint32_t start = dual_timer_get(DUAL_TIMER_ID0);
			uint32_t ticks = __MS_TO_TICKS(time_ms);

			while (start - dual_timer_get(DUAL_TIMER_ID0) < ticks)
			{
			}
			}

			/* MAC timer */

			void mac_timer_open(drv_callback_t callback)
			{
			// DIV = 1 (max: ~68.8s, resolution: ~16ns)
			// DIV = 16 (max: ~1101.2s, resolution: ~256ns)
			// DIV = 256 (max: ~17620.3s resolution: ~65us)
			struct DUAL_TIMER_CFG_T mac_timer_cfg = {
			.type = DUAL_TIMER_TYPE_ONESHOT,
			.prescale = DUAL_TIMER_PRESCALE_DIV1,
			.width = DUAL_TIMER_SIZE_32BIT,
			.int_en = true,
			.load = 0xffffffff,
			.callback = NULL,
			};

			mac_timer_cfg.callback = callback;
			dual_timer_open(DUAL_TIMER_ID1, &mac_timer_cfg);
			}

			void mac_timer_close(void)
			{
			dual_timer_close(DUAL_TIMER_ID1);
			}

			void mac_timer_start(uint32_t count)
			{
			dual_timer_start(DUAL_TIMER_ID1, count);
			// board_led_on(BOARD_LED_1);
			}

			void mac_timer_stop(void)
			{
			dual_timer_stop(DUAL_TIMER_ID1);
			// board_led_off(BOARD_LED_1);
			}

			/* SYS Tick */

			static struct SYS_TICK_ENV_T sys_tick_env = {0};

			void sys_tick_start(uint32_t ticks)
			{
			ASSERT(ticks <= SysTick_LOAD_RELOAD_Msk + 1, "Reload value impossible\r\n");
			SysTick->LOAD = (uint32_t)(ticks - 1UL); /* set reload register */
			NVIC_SetPriority(SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
			SysTick->VAL = 0; /* A write of any value clears the field to 0 */
			SysTick->CTRL = SysTick_CTRL_TICKINT_Msk \| SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
			}

			uint32_t sys_tick_us(void)
			{
			uint32_t lock = int_lock();

			uint32_t count = sys_tick_env.count;
			uint32_t load = SysTick->LOAD + 1;
			uint32_t val = SysTick->VAL;

			uint32_t flag_pending = REG_READ(0xE000ED04) & (1 << 26);
			if (flag_pending)
			{
			count += 1;

			val = load;
			}
			else if (val == 0)
			{
			val = load;
			}

			uint32_t tick_us = (count * 328 + (load - val)) * 30;

			int_unlock(lock);

			return tick_us;
			}

			uint32_t sys_tick_ms(void)
			{
			uint32_t lock = int_lock();

			uint32_t count = sys_tick_env.count;
			uint32_t load = SysTick->LOAD + 1;
			uint32_t val = SysTick->VAL;

			uint32_t flag_pending = REG_READ(0xE000ED04) & (1 << 26);
			if (flag_pending)
			{
			count += 1;

			val = load;
			}
			else if (val == 0)
			{
			val = load;
			}

			uint32_t tick_ms = (count * 10 + (load - val) * 10 / 328);

			int_unlock(lock);

			return tick_ms;
			}

			uint32_t sys_tick_get(void)
			{
			return sys_tick_env.count;
			}

			void sys_tick_callback_set(void (*callback)(void))
			{
			sys_tick_env.callback = callback;
			}

			uint32_t sys_tick_elapse_ms(void)
			{
			uint32_t lock = int_lock();

			uint32_t load = SysTick->LOAD + 1;
			uint32_t val = SysTick->VAL;

			uint32_t flag_pending = REG_READ(0xE000ED04) & (1 << 26);
			if (flag_pending)
			{
			val = 0;
			}
			else if (val == 0)
			{
			val = load;
			}

			uint32_t elapse_ms = (load - val) * 10 / 328;

			int_unlock(lock);

			return elapse_ms;
			}

			void sys_tick_pause(void)
			{
			// Stop SysTick
			SysTick->CTRL = 0;
			uint32_t val = SysTick->VAL;
			uint32_t load = SysTick->LOAD + 1;
			uint32_t flag_pending = REG_READ(0xE000ED04) & (1 << 26);

			if (flag_pending)
			{
			sys_tick_env.count += 1;
			val = load;

			// clear pending systick interrupt
			REG_WRITE(0xE000ED04, (1 << 25));
			}
			else if (val == 0)
			{
			val = load;
			}

			// Store SysTick counter
			sys_tick_env.load = load;
			// SysTick->VAL cannot be set
			sys_tick_env.elapse += (load - val);
			}

			void sys_tick_resume(void)
			{
			sys_tick_start(sys_tick_env.load);

			uint32_t slp_cnt = high_xtal_off_time();
			uint32_t cnt = slp_cnt / sys_tick_env.load;
			sys_tick_env.elapse += (slp_cnt - cnt * sys_tick_env.load);
			while (sys_tick_env.elapse >= sys_tick_env.load)
			{
			sys_tick_env.elapse -= sys_tick_env.load;
			cnt += 1;
			}

			sys_tick_env.count += cnt;
			if ((sys_tick_env.callback != NULL) && (cnt))
			{
			sys_tick_env.callback();
			}
			}

			void SysTick_Handler(void)
			{
			// board_led_on(BOARD_LED_2);
			sys_tick_env.count++;
			if (sys_tick_env.callback != NULL)
			{
			sys_tick_env.callback();
			}
			// board_led_off(BOARD_LED_2);
			}

			void sys_reset(uint32_t error)
			{
			//LOG_INFO(TRACE_MODULE_DRIVER, "system reboot%x", error);

			delay_us(10000);

			// reboot
			reset_module(RESET_MODULE_REBOOT);
			}

			void delay_us(uint32_t cnt)
			{
			#define SYSTEM_CLOCK_MHZ 62.4

			// the larger LSLS bits the more accurate of this delay function but the shorter of available delay time
			#define SYSTEM_CLOCK_LSLS_BITS 5
			#define SYSTEM_CLOCK_LSLS (uint16_t)(SYSTEM_CLOCK_MHZ * (1 << SYSTEM_CLOCK_LSLS_BITS))

			// you should adjust these num manually if SYSTEM_CLOCK_MHZ or SYSTEM_CLOCK_LSLS_BITS changes
			#define SYSTEM_CLOCK_MULT_HI 0x07 // (uint8_t)((SYSTEM_CLOCK_LSLS & 0xFF00) >> 8)
			#define SYSTEM_CLOCK_MULT_LO 0xCD // (uint8_t)(SYSTEM_CLOCK_LSLS & 0x00FF)

			#define AAPCS_PREP_CYCLES 18
			#define DELAY_LOOP_CYCLES 4

			#if defined(__GNUC__) && !defined(__ARMCC_VERSION)
			__asm volatile(".syntax unified\n");
			#endif

			__asm volatile(
			"cmp r0, #0\n"
			"beq exit%=\n"
			"push {r4,r5}\n"
			"movs r5, %[mult_hi]\n"
			"lsls r4, r5, #8\n"
			"movs r5, %[mult_lo]\n"
			"adds r4, r4, r5\n"
			"muls r0, r4\n"
			"lsrs r0, r0, %[shift]\n"
			"subs r0, r0, %[adjust]\n"
			"loop%=:\n"
			"subs r0, r0, %[decr]\n"
			"bhi loop%=\n"
			"pop {r4,r5}\n"
			"exit%=:\n"
			:
			: [mult_hi] "i"(SYSTEM_CLOCK_MULT_HI), [mult_lo] "i"(SYSTEM_CLOCK_MULT_LO), [shift] "i"(SYSTEM_CLOCK_LSLS_BITS), [adjust] "i"(AAPCS_PREP_CYCLES),
			[decr] "i"(DELAY_LOOP_CYCLES));

			#if defined(__GNUC__) && !defined(__ARMCC_VERSION)
			__asm volatile(".syntax divided\n");
			#endif
			}

			uint8_t count_bits(uint32_t value)
			{
			uint8_t count = 0;
			while (value)
			{
			value &= value - 1;
			count++;
			}
			return count;
			}

			uint8_t search_byte_right_one(uint8_t value)
			{
			uint8_t index = 1;
			if (value == 0)
			{
			return 0;
			}

			if ((value & 0x0F) == 0)
			{
			value >>= 4;
			index += 4;
			}

			if ((value & 0x03) == 0)
			{
			value >>= 2;
			index += 2;
			}

			if ((value & 0x01) == 0)
			{
			index += 1;
			}
			return index;
			}

			uint8_t byte_right_one_mask(uint8_t value)
			{
			return (value & (~value + 1));
			}

			int32_t average_filter(int32_t input)
			{
			#define TOTAL_SAMPLES (5)
			#if 1
			// arithmetical averaging
			static int32_t data_buffer[TOTAL_SAMPLES] = {0};
			static uint8_t data_num = 0;
			int64_t sum = 0;

			// if buffer is full, remove the oldest data
			if (data_num == TOTAL_SAMPLES)
			{
			data_num--;
			}

			// add a new data in buffer
			for (int i = data_num; i > 0; i--)
			{
			data_buffer[i] = data_buffer[i - 1];
			sum += data_buffer[i];
			}
			data_buffer[0] = input;
			sum += data_buffer[0];
			data_num++;

			return (int32_t)(sum / data_num);
			#else
			// weighted averaging
			#endif
			}

			int16_t mk_q7_to_s16(int16_t data)
			{
			return (data / 128);
			}

			int16_t mk_s16_to_q7(int16_t data)
			{
			return (data * 128);
			}

			float mk_q7_to_f32(int16_t data)
			{
			return (float)(data / 128.0);
			}

			int16_t mk_f32_to_q7(float data)
			{
			return (int16_t)(data * 128);
			}