From f9937b28cc5dd0842a4152ff7cdf61592652a597 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Wed, 14 Jul 2021 14:42:20 +0200 Subject: [PATCH] stm32g4: add CORDIC driver --- arch/arm/src/stm32/Kconfig | 1 + arch/arm/src/stm32/Make.defs | 4 + arch/arm/src/stm32/stm32_cordic.c | 330 ++++++++++++++++++++++++++++++ arch/arm/src/stm32/stm32_cordic.h | 55 +++++ 4 files changed, 390 insertions(+) create mode 100644 arch/arm/src/stm32/stm32_cordic.c create mode 100644 arch/arm/src/stm32/stm32_cordic.h diff --git a/arch/arm/src/stm32/Kconfig b/arch/arm/src/stm32/Kconfig index d39e56c39d..bb9d24d735 100644 --- a/arch/arm/src/stm32/Kconfig +++ b/arch/arm/src/stm32/Kconfig @@ -2752,6 +2752,7 @@ config STM32_CORDIC bool "CORDIC Accelerator" default n depends on STM32_HAVE_CORDIC + select MATH_CORDIC_USE_Q31 config STM32_BKP bool "BKP" diff --git a/arch/arm/src/stm32/Make.defs b/arch/arm/src/stm32/Make.defs index c013fd08da..29cdd3a6f5 100644 --- a/arch/arm/src/stm32/Make.defs +++ b/arch/arm/src/stm32/Make.defs @@ -267,3 +267,7 @@ endif ifeq ($(CONFIG_STM32_FOC),y) CHIP_CSRCS += stm32_foc.c endif + +ifeq ($(CONFIG_STM32_CORDIC),y) +CHIP_CSRCS += stm32_cordic.c +endif diff --git a/arch/arm/src/stm32/stm32_cordic.c b/arch/arm/src/stm32/stm32_cordic.c new file mode 100644 index 0000000000..8d3327b46b --- /dev/null +++ b/arch/arm/src/stm32/stm32_cordic.c @@ -0,0 +1,330 @@ +/**************************************************************************** + * arch/arm/src/stm32/stm32_cordic.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 + +#include +#include +#include +#include + +#include +#include + +#include "arm_arch.h" +#include "chip.h" + +#include "hardware/stm32g4xxxx_cordic.h" + +#include "stm32_cordic.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define STM32_CORDIC_PRECISION (3) +#define STM32_CORDIC_ARGSIZE (0) /* Argument size is 32-bit */ +#define STM32_CORDIC_RESSIZE (0) /* Result size is 32-bit */ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This structure represents the state of one PWM timer */ + +struct stm32_cordic_s +{ + FAR const struct cordic_ops_s *ops; /* Lower half operations */ + uint32_t base; /* The base address of the CORDIC */ + bool inuse; /* True: driver is in-use */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Register access */ + +static uint32_t cordic_getreg(FAR struct stm32_cordic_s *priv, int offset); +static void cordic_putreg(FAR struct stm32_cordic_s *priv, int offset, + uint32_t value); + +/* Ops */ + +int cordic_calc(FAR struct cordic_lowerhalf_s *lower, + FAR struct cordic_calc_s *calc); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* STM32 specific CORDIC ops */ + +struct cordic_ops_s g_stm32_cordic_ops = +{ + .calc = cordic_calc +}; + +/* STM32 CORDIC device */ + +struct stm32_cordic_s g_stm32_cordic_dev = +{ + .ops = &g_stm32_cordic_ops, + .base = STM32_CORDIC_BASE, + .inuse = false +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cordic_getreg + ****************************************************************************/ + +static uint32_t cordic_getreg(FAR struct stm32_cordic_s *priv, int offset) +{ + return getreg32(priv->base + offset); +} + +/**************************************************************************** + * Name: cordic_putreg + ****************************************************************************/ + +static void cordic_putreg(FAR struct stm32_cordic_s *priv, int offset, + uint32_t value) +{ + putreg32(value, priv->base + offset); +} + +/**************************************************************************** + * Name: cordic_calc + ****************************************************************************/ + +int cordic_calc(FAR struct cordic_lowerhalf_s *lower, + FAR struct cordic_calc_s *calc) +{ + FAR struct stm32_cordic_s *priv = (FAR struct stm32_cordic_s *)lower; + int ret = OK; + uint32_t csr = 0; + bool arg2_inc = false; + uint8_t scale = 0; + + DEBUGASSERT(lower); + DEBUGASSERT(calc); + + /* Configure CORDIC function */ + + switch (calc->func) + { + case CORDIC_CALC_FUNC_COS: + { + csr |= CORDIC_CSR_FUNC_COS; + arg2_inc = true; + scale = 0; + break; + } + + case CORDIC_CALC_FUNC_SIN: + { + csr |= CORDIC_CSR_FUNC_SIN; + arg2_inc = true; + scale = 0; + break; + } + + case CORDIC_CALC_FUNC_PHASE: + { + csr |= CORDIC_CSR_FUNC_PHASE; + arg2_inc = true; + scale = 0; + break; + } + + case CORDIC_CALC_FUNC_MOD: + { + csr |= CORDIC_CSR_FUNC_MOD; + arg2_inc = true; + scale = 0; + break; + } + + case CORDIC_CALC_FUNC_ARCTAN: + { + csr |= CORDIC_CSR_FUNC_ARCTAN; + arg2_inc = true; + scale = 0; + break; + } + + case CORDIC_CALC_FUNC_HCOS: + { + csr |= CORDIC_CSR_FUNC_HCOS; + arg2_inc = false; + scale = 1; + break; + } + + case CORDIC_CALC_FUNC_HSIN: + { + csr |= CORDIC_CSR_FUNC_HSIN; + arg2_inc = false; + scale = 1; + break; + } + + case CORDIC_CALC_FUNC_HARCTAN: + { + csr |= CORDIC_CSR_FUNC_HARCTAN; + arg2_inc = false; + scale = 1; + break; + } + + case CORDIC_CALC_FUNC_LN: + { + csr |= CORDIC_CSR_FUNC_LN; + arg2_inc = false; + scale = 1; + break; + } + + case CORDIC_CALC_FUNC_SQRT: + { + csr |= CORDIC_CSR_FUNC_SQRT; + arg2_inc = false; + scale = 1; + break; + } + + default: + { + ret = -EINVAL; + goto errout; + } + } + + /* Configure precision */ + + csr |= ((STM32_CORDIC_PRECISION << CORDIC_CSR_PRECISION_SHIFT) & + CORDIC_CSR_PRECISION_MASK); + + /* Configure scale */ + + csr |= ((scale << CORDIC_CSR_SCALE_SHIFT) & CORDIC_CSR_SCALE_MASK); + + /* Configure width of output data */ + + csr |= STM32_CORDIC_RESSIZE; + + /* Configure width of input data */ + + csr |= STM32_CORDIC_ARGSIZE; + + /* Include secondary argument */ + + if (arg2_inc == true) + { + csr |= CORDIC_CSR_NARGS; + } + + /* Include secondary result */ + + if (calc->res2_incl == true) + { + csr |= CORDIC_CSR_NRES; + } + + /* Write CSR */ + + cordic_putreg(priv, STM32_CORDIC_CSR_OFFSET, csr); + + /* Write arguments */ + + cordic_putreg(priv, STM32_CORDIC_WDATA_OFFSET, calc->arg1); + + if (arg2_inc == true) + { + cordic_putreg(priv, STM32_CORDIC_WDATA_OFFSET, calc->arg2); + } + + /* Read results - blocking. + * NOTE: We don't need to wait for RRDY flag as wait sates are + * inserted automatically on RDATA read. + */ + + calc->res1 = cordic_getreg(priv, STM32_CORDIC_RDATA_OFFSET); + + if (calc->res2_incl == true) + { + calc->res2 = cordic_getreg(priv, STM32_CORDIC_RDATA_OFFSET); + } + else + { + calc->res2 = 0; + } + +errout: + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_cordicinitialize + * + * Description: + * Initialize a CORDIC device. This function must be called + * from board-specific logic. + * + * Returned Value: + * On success, a pointer to the lower half CORDIC driver is returned. + * NULL is returned on any failure. + * + ****************************************************************************/ + +FAR struct cordic_lowerhalf_s *stm32_cordicinitialize(void) +{ + FAR struct cordic_lowerhalf_s *lower = NULL; + + if (g_stm32_cordic_dev.inuse == true) + { + _err("STM32 CORDIC device already in use\n"); + set_errno(EBUSY); + goto errout; + } + + /* Get lower-half device */ + + lower = (FAR struct cordic_lowerhalf_s *) &g_stm32_cordic_dev; + + /* The driver is now in-use */ + + g_stm32_cordic_dev.inuse = true; + +errout: + return lower; +} diff --git a/arch/arm/src/stm32/stm32_cordic.h b/arch/arm/src/stm32/stm32_cordic.h new file mode 100644 index 0000000000..2baded4d5b --- /dev/null +++ b/arch/arm/src/stm32/stm32_cordic.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * arch/arm/src/stm32/stm32_cordic.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_ARM_SRC_STM32_STM32_CORDIC_H +#define __ARCH_ARM_SRC_STM32_STM32_CORDIC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "chip.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_cordicinitialize + * + * Description: + * Initialize a CORDIC device. This function must be called + * from board-specific logic. + * + * Returned Value: + * On success, a pointer to the lower half CORDIC driver is returned. + * NULL is returned on any failure. + * + ****************************************************************************/ + +FAR struct cordic_lowerhalf_s *stm32_cordicinitialize(void); + +#endif /* __ARCH_ARM_SRC_STM32_STM32_CORDIC_H */