summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2012-02-14 17:48:25 +0000
committerpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2012-02-14 17:48:25 +0000
commitfa01a80b095d5f01861085adf85fc8160525e464 (patch)
tree8e046d91a1d4cd89ed2ef349449ccf9907635c9e
parenteb27d3ee72eef6f00fc98e9896014d8c785f8c0d (diff)
downloadpx4-nuttx-fa01a80b095d5f01861085adf85fc8160525e464.tar.gz
px4-nuttx-fa01a80b095d5f01861085adf85fc8160525e464.tar.bz2
px4-nuttx-fa01a80b095d5f01861085adf85fc8160525e464.zip
More STM32 quadrature encoder code
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@4391 42af7a65-404d-4744-a932-0658087f49c3
-rw-r--r--nuttx/arch/arm/src/stm32/chip/stm32_tim.h2
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_qencoder.c543
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_qencoder.h44
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_tim.c46
4 files changed, 601 insertions, 34 deletions
diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_tim.h b/nuttx/arch/arm/src/stm32/chip/stm32_tim.h
index 8822589c0..10176b81d 100644
--- a/nuttx/arch/arm/src/stm32/chip/stm32_tim.h
+++ b/nuttx/arch/arm/src/stm32/chip/stm32_tim.h
@@ -1,7 +1,7 @@
/****************************************************************************************************
* arch/arm/src/stm32/chip/stm32_tim.h
*
- * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ * Copyright (C) 2009, 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
diff --git a/nuttx/arch/arm/src/stm32/stm32_qencoder.c b/nuttx/arch/arm/src/stm32/stm32_qencoder.c
index 5b138b1d8..747d74219 100644
--- a/nuttx/arch/arm/src/stm32/stm32_qencoder.c
+++ b/nuttx/arch/arm/src/stm32/stm32_qencoder.c
@@ -43,18 +43,68 @@
#include <errno.h>
#include <nuttx/arch.h>
-#include <nuttx/kmalloc.h>
+#include <nuttx/irq.h>
#include <nuttx/sensors/qencoder.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_internal.h"
#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
#include "stm32_qencoder.h"
#ifdef CONFIG_QENCODER
/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Debug ****************************************************************************/
+/* Non-standard debug that may be enabled just for testing QENCODER */
+
+#ifndef CONFIG_DEBUG
+# undef CONFIG_DEBUG_QENCODER
+#endif
+
+#ifdef CONFIG_DEBUG_QENCODER
+# define qedbg dbg
+# define qelldbg lldbg
+# ifdef CONFIG_DEBUG_VERBOSE
+# define qevdbg vdbg
+# define qellvdbg llvdbg
+# define stm32_dumpgpio(p,m) stm32_dumpgpio(p,m)
+# else
+# define qelldbg(x...)
+# define qellvdbg(x...)
+# define stm32_dumpgpio(p,m)
+# endif
+#else
+# define qedbg(x...)
+# define qelldbg(x...)
+# define qevdbg(x...)
+# define qellvdbg(x...)
+# define stm32_dumpgpio(p,m)
+#endif
+
+/************************************************************************************
* Private Types
************************************************************************************/
+/* Constant configuration structure that is retained in FLASH */
+
+struct stm32_qeconfig_s
+{
+ uint8_t timid; /* Timer ID {1,2,3,4,5,8} */
+ uint8_t irq; /* Timer update IRQ */
+ uint32_t base; /* Register base address */
+ xcpt_t handler; /* Interrupt handler for this IRQ */
+};
+
+/* Overall, RAM-based state structure */
+
struct stm32_lowerhalf_s
{
/* The first field of this state structure must be a pointer to the lower-
@@ -64,11 +114,52 @@ struct stm32_lowerhalf_s
FAR const struct qe_ops_s *ops; /* Lower half callback structure */
/* STM32 driver-specific fields: */
+
+ FAR const struct stm32_qeconfig_s *config; /* static onfiguration */
+
+ bool inuse; /* True: The lower-half driver is in-use */
+ volatile int32_t position; /* The current position offset */
};
/************************************************************************************
* Private Function Prototypes
************************************************************************************/
+/* Helper functions */
+
+static uint16_t stm32_getreg16(struct stm32_lowerhalf_s *priv, int offset);
+static void stm32_putreg16(struct stm32_lowerhalf_s *priv, int offset, uint16_t value);
+static uint32_t stm32_getreg32(FAR struct stm32_lowerhalf_s *priv, int offset);
+static void stm32_putreg32(FAR struct stm32_lowerhalf_s *priv, int offset, uint32_t value);
+
+#if defined(CONFIG_DEBUG_QENCODER) && defined(CONFIG_DEBUG_VERBOSE)
+static void stm32_dumpregs(struct stm32_lowerhalf_s *priv, FAR const char *msg);
+#else
+# define stm32_dumpregs(priv,msg)
+#endif
+
+static FAR struct stm32_lowerhalf_s *stm32_tim2lower(int tim);
+
+/* Interrupt handling */
+
+static int stm32_interrupt(FAR struct stm32_lowerhalf_s *priv);
+#ifdef CONFIG_STM32_TIM1
+static int stm32_tim1interrupt(int irq, FAR void *context);
+#endif
+#ifdef CONFIG_STM32_TIM2
+static int stm32_tim2interrupt(int irq, FAR void *context);
+#endif
+#ifdef CONFIG_STM32_TIM3
+static int stm32_tim3interrupt(int irq, FAR void *context);
+#endif
+#ifdef CONFIG_STM32_TIM4
+static int stm32_tim4interrupt(int irq, FAR void *context);
+#endif
+#ifdef CONFIG_STM32_TIM5
+static int stm32_tim5interrupt(int irq, FAR void *context);
+#endif
+#ifdef CONFIG_STM32_TIM8
+static int stm32_tim8interrupt(int irq, FAR void *context);
+#endif
/* Lower-half Quadrature Encoder Driver Methods */
@@ -78,12 +169,12 @@ static int stm32_position(FAR struct qe_lowerhalf_s *lower, int32_t *pos);
static int stm32_reset(FAR struct qe_lowerhalf_s *lower);
static int stm32_ioctl(FAR struct qe_lowerhalf_s *lower, int cmd, unsigned long arg);
- /************************************************************************************
+/************************************************************************************
* Private Data
************************************************************************************/
/* The lower half callback structure */
-FAR const struct qe_ops_s g_qecallbacks =
+static const struct qe_ops_s g_qecallbacks =
{
.setup = stm32_setup,
.shutdown = stm32_shutdown,
@@ -92,11 +183,384 @@ FAR const struct qe_ops_s g_qecallbacks =
.ioctl = stm32_ioctl,
};
+/* Per-timer state structures */
+
+#ifdef CONFIG_STM32_TIM1
+static const struct stm32_qeconfig_s g_tim1config =
+{
+ .timid = 1,
+ .irq = STM32_IRQ_TIM1UP,
+ .base = STM32_TIM1_BASE,
+ .handler = stm32_tim1interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim1lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim1config,
+ .inuse = false,
+};
+
+#endif
+#ifdef CONFIG_STM32_TIM2
+static const struct stm32_qeconfig_s g_tim2config =
+{
+ .timid = 2,
+ .irq = STM32_IRQ_TIM2,
+ .base = STM32_TIM2_BASE,
+ .handler = stm32_tim2interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim2lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim2config,
+ .inuse = false,
+};
+
+#endif
+#ifdef CONFIG_STM32_TIM3
+static const struct stm32_qeconfig_s g_tim3config =
+{
+ .timid = 3,
+ .irq = STM32_IRQ_TIM3,
+ .base = STM32_TIM3_BASE,
+ .handler = stm32_tim3interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim3lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim3config,
+ .inuse = false,
+};
+
+#endif
+#ifdef CONFIG_STM32_TIM4
+static const struct stm32_qeconfig_s g_tim4config =
+{
+ .timid = 4,
+ .irq = STM32_IRQ_TIM4,
+ .base = STM32_TIM4_BASE,
+ .handler = stm32_tim4interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim4lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim4config,
+ .inuse = false,
+};
+
+#endif
+#ifdef CONFIG_STM32_TIM5
+static const struct stm32_qeconfig_s g_tim5config =
+{
+ .timid = 5,
+ .irq = STM32_IRQ_TIM5,
+ .base = STM32_TIM5_BASE,
+ .handler = stm32_tim5interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim5lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim5config,
+ .inuse = false,
+};
+
+#endif
+#ifdef CONFIG_STM32_TIM8
+static const struct stm32_qeconfig_s g_tim8config =
+{
+ .timid = 8,
+ .irq = STM32_IRQ_TIM8UP,
+ .base = STM32_TIM8_BASE,
+ .handler = stm32_tim8interrupt,
+};
+
+static struct stm32_lowerhalf_s g_tim8lower =
+{
+ .ops = &g_qecallbacks,
+ .config = &g_tim8config
+ .inuse = false,
+};
+#endif
+
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
+ * Name: stm32_getreg16
+ *
+ * Description:
+ * Read the value of a 16-bit timer register.
+ *
+ * Input Parameters:
+ * priv - A reference to the lower half status
+ * offset - The offset to the register to read
+ *
+ * Returned Value:
+ * The current contents of the specified register
+ *
+ ************************************************************************************/
+
+static uint16_t stm32_getreg16(struct stm32_lowerhalf_s *priv, int offset)
+{
+ return getreg16(priv->config->base + offset);
+}
+
+/************************************************************************************
+ * Name: stm32_putreg16
+ *
+ * Description:
+ * Write a value to a 16-bit timer register.
+ *
+ * Input Parameters:
+ * priv - A reference to the lower half status
+ * offset - The offset to the register to read
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+static void stm32_putreg16(struct stm32_lowerhalf_s *priv, int offset, uint16_t value)
+{
+ putreg16(value, priv->config->base + offset);
+}
+
+/************************************************************************************
+ * Name: stm32_getreg32
+ *
+ * Description:
+ * Read the value of a 32-bit timer register. This applies only for the STM32 F4
+ * 32-bit registers (CNT, ARR, CRR1-4) in the 32-bit timers TIM2-5 (but works OK
+ * with the 16-bit TIM1,8 and F1 registers).
+ *
+ * Input Parameters:
+ * priv - A reference to the lower half status
+ * offset - The offset to the register to read
+ *
+ * Returned Value:
+ * The current contents of the specified register
+ *
+ ************************************************************************************/
+
+static uint32_t stm32_getreg32(FAR struct stm32_lowerhalf_s *priv, int offset)
+{
+ return getreg32(priv->config->base + offset);
+}
+
+/************************************************************************************
+ * Name: stm32_putreg16
+ *
+ * Description:
+ * Write a value to a 32-bit timer register. This applies only for the STM32 F4
+ * 32-bit registers (CNT, ARR, CRR1-4) in the 32-bit timers TIM2-5 (but works OK
+ * with the 16-bit TIM1,8 and F1 registers).
+ *
+ * Input Parameters:
+ * priv - A reference to the lower half status
+ * offset - The offset to the register to read
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+static void stm32_putreg32(FAR struct stm32_lowerhalf_s *priv, int offset, uint32_t value)
+{
+ putreg32(value, priv->config->base + offset);
+}
+
+/****************************************************************************
+ * Name: stm32_dumpregs
+ *
+ * Description:
+ * Dump all timer registers.
+ *
+ * Input parameters:
+ * priv - A reference to the QENCODER block status
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_DEBUG_QENCODER) && defined(CONFIG_DEBUG_VERBOSE)
+static void stm32_dumpregs(struct stm32_lowerhalf_s *priv, FAR const char *msg)
+{
+ qevdbg("%s:\n", msg);
+ qevdbg(" CR1: %04x CR2: %04x SMCR: %04x DIER: %04x\n",
+ stm32_getreg16(priv, STM32_GTIM_CR1_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CR2_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_SMCR_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_DIER_OFFSET));
+ qevdbg(" SR: %04x EGR: %04x CCMR1: %04x CCMR2: %04x\n",
+ stm32_getreg16(priv, STM32_GTIM_SR_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_EGR_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CCMR1_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CCMR2_OFFSET));
+ qevdbg(" CCER: %04x CNT: %04x PSC: %04x ARR: %04x\n",
+ stm32_getreg16(priv, STM32_GTIM_CCER_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CNT_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_PSC_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_ARR_OFFSET));
+ qevdbg(" CCR1: %04x CCR2: %04x CCR3: %04x CCR4: %04x\n",
+ stm32_getreg16(priv, STM32_GTIM_CCR1_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CCR2_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CCR3_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_CCR4_OFFSET));
+#if defined(CONFIG_STM32_TIM1_QENCODER) || defined(CONFIG_STM32_TIM8_QENCODER)
+ if (priv->timtype == TIMTYPE_ADVANCED)
+ {
+ qevdbg(" RCR: %04x BDTR: %04x DCR: %04x DMAR: %04x\n",
+ stm32_getreg16(priv, STM32_ATIM_RCR_OFFSET),
+ stm32_getreg16(priv, STM32_ATIM_BDTR_OFFSET),
+ stm32_getreg16(priv, STM32_ATIM_DCR_OFFSET),
+ stm32_getreg16(priv, STM32_ATIM_DMAR_OFFSET));
+ }
+ else
+#endif
+ {
+ qevdbg(" DCR: %04x DMAR: %04x\n",
+ stm32_getreg16(priv, STM32_GTIM_DCR_OFFSET),
+ stm32_getreg16(priv, STM32_GTIM_DMAR_OFFSET));
+ }
+}
+#endif
+
+/************************************************************************************
+ * Name: stm32_tim2lower
+ *
+ * Description:
+ * Map a timer number to a device structure
+ *
+ ************************************************************************************/
+
+static FAR struct stm32_lowerhalf_s *stm32_tim2lower(int tim)
+{
+ switch (tim)
+ {
+#ifdef CONFIG_STM32_TIM1
+ case 1:
+ return &g_tim1lower;
+#endif
+#ifdef CONFIG_STM32_TIM2
+ case 2:
+ return &g_tim2lower;
+#endif
+#ifdef CONFIG_STM32_TIM3
+ case 3:
+#endif
+#ifdef CONFIG_STM32_TIM4
+ case 4:
+ return &g_tim4lower;
+#endif
+#ifdef CONFIG_STM32_TIM5
+ case 5:
+ return &g_tim5lower;
+#endif
+#ifdef CONFIG_STM32_TIM8
+ case 8:
+ return &g_tim8lower;
+#endif
+ default:
+ return NULL;
+ }
+}
+
+/************************************************************************************
+ * Name: stm32_setup
+ *
+ * Description:
+ * Common timer interrupt handling
+ *
+ ************************************************************************************/
+
+static int stm32_interrupt(FAR struct stm32_lowerhalf_s *priv)
+{
+ uint16_t regval;
+
+ /* Verify that this is an update interrupt. Nothing else is expected. */
+
+ regval = stm32_getreg16(priv, STM32_GTIM_SR_OFFSET);
+ DEBUGASSERT((regval & ATIM_SR_UIF) != 0);
+
+ /* Clear the UIF interrupt bit */
+
+ stm32_putreg16(priv, STM32_GTIM_SR_OFFSET, regval & ~GTIM_SR_UIF);
+
+ /* Check the direction bit in the CR1 register and add or subtract the
+ * maximum value, as appropriate.
+ */
+
+ regval = stm32_getreg16(priv, STM32_GTIM_CR1_OFFSET);
+ if ((regval & ATIM_CR1_DIR) != 0)
+ {
+ priv->position -= (int32_t)0x0000fff0;
+ }
+ else
+ {
+ priv->position += (int32_t)0x0000fff0;
+ }
+
+ return OK;
+}
+
+/************************************************************************************
+ * Name: stm32_intNinterrupt
+ *
+ * Description:
+ * TIMN interrupt handler
+ *
+ ************************************************************************************/
+
+ #ifdef CONFIG_STM32_TIM1
+static int stm32_tim1interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim1lower);
+}
+#endif
+
+#ifdef CONFIG_STM32_TIM2
+static int stm32_tim2interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim2lower);
+}
+#endif
+
+#ifdef CONFIG_STM32_TIM3
+static int stm32_tim3interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim3lower);
+}
+#endif
+
+#ifdef CONFIG_STM32_TIM4
+static int stm32_tim4interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim4lower);
+}
+#endif
+
+#ifdef CONFIG_STM32_TIM5
+static int stm32_tim5interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim5lower);
+}
+#endif
+
+#ifdef CONFIG_STM32_TIM8
+static int stm32_tim8interrupt(int irq, FAR void *context)
+{
+ return stm32_interrupt(&g_tim8lower);
+}
+#endif
+
+/************************************************************************************
* Name: stm32_setup
*
* Description:
@@ -138,8 +602,33 @@ static int stm32_shutdown(FAR struct qe_lowerhalf_s *lower)
static int stm32_position(FAR struct qe_lowerhalf_s *lower, int32_t *pos)
{
-#warning "Missing logic"
- return -ENOSYS;
+ FAR struct stm32_lowerhalf_s *priv = (FAR struct stm32_lowerhalf_s *)lower;
+ int32_t position;
+ int32_t verify;
+ uint32_t count;
+
+ DEBUGASSERT(lower && priv->inuse);
+
+ /* Loop until we are certain that no interrupt occurred between samples */
+
+ do
+ {
+ /* Don't let another task pre-empt us until we get the measurement. The timer
+ * interrupt may still be processed
+ */
+
+ sched_lock();
+ position = priv->position;
+ count = stm32_getreg32(priv, STM32_GTIM_CNT_OFFSET);
+ verify = priv->position;
+ sched_unlock();
+ }
+ while (position != verify);
+
+ /* Return the position measurement */
+
+ *pos = position + (int32_t)count;
+ return OK;
}
/************************************************************************************
@@ -152,8 +641,20 @@ static int stm32_position(FAR struct qe_lowerhalf_s *lower, int32_t *pos)
static int stm32_reset(FAR struct qe_lowerhalf_s *lower)
{
-#warning "Missing logic"
- return -ENOSYS;
+ FAR struct stm32_lowerhalf_s *priv = (FAR struct stm32_lowerhalf_s *)lower;
+ irqstate_t flags;
+
+ DEBUGASSERT(lower && priv->inuse);
+
+ /* Reset the timer and the counter. Interrupts are disabled to make this atomic
+ * (if possible)
+ */
+
+ flags = irqsave();
+ stm32_putreg32(priv, STM32_GTIM_CNT_OFFSET, 0);
+ priv->position = 0;
+ irqrestore(flags);
+ return OK;
}
/************************************************************************************
@@ -184,38 +685,44 @@ static int stm32_ioctl(FAR struct qe_lowerhalf_s *lower, int cmd, unsigned long
*
* Input Parameters:
* devpath - The full path to the driver to register. E.g., "/dev/qe0"
+ * tim - The timer number to used. time must be an element of {1,2,3,4,5,8}
*
* Returned Values:
* Zero on success; A negated errno value is returned on failure.
*
************************************************************************************/
-int stm32_qeinitialize(FAR const char *devpath)
+int stm32_qeinitialize(FAR const char *devpath, int tim)
{
- FAR struct stm32_lowerhalf_s *lower;
+ FAR struct stm32_lowerhalf_s *priv;
int ret;
- /* Allocate an instance to the device-specific, lower-half state structure */
+ /* Find the pre-allocated timer state structure corresponding to this timer */
- lower = (FAR struct stm32_lowerhalf_s *)kmalloc(sizeof(struct stm32_lowerhalf_s));
- if (lower)
+ priv = stm32_tim2lower(tim);
+ if (priv)
{
- return -ENOMEM;
+ return -ENXIO;
}
- /* Initialize the allocated state structure */
+ /* Make sure that it is available */
- lower->ops = &g_qecallbacks;
+ if (priv->inuse)
+ {
+ return -EBUSY;
+ }
- /* Register the lower-half driver */
+ /* Register the priv-half driver */
- ret = qe_register(devpath, (FAR struct qe_lowerhalf_s *)lower);
+ ret = qe_register(devpath, (FAR struct qe_lowerhalf_s *)priv);
if (ret < 0)
{
- kfree(lower);
return ret;
}
+ /* The driver is now in-use */
+
+ priv->inuse = true;
return OK;
}
diff --git a/nuttx/arch/arm/src/stm32/stm32_qencoder.h b/nuttx/arch/arm/src/stm32/stm32_qencoder.h
index 5c977caad..f0b93f857 100644
--- a/nuttx/arch/arm/src/stm32/stm32_qencoder.h
+++ b/nuttx/arch/arm/src/stm32/stm32_qencoder.h
@@ -49,6 +49,47 @@
/************************************************************************************
* Included Files
************************************************************************************/
+/* Timer devices may be used for different purposes. One special purpose is as
+ * a quadrature encoder input device. If CONFIG_STM32_TIMn is defined then the
+ * CONFIG_STM32_TIMn_QE must also be defined to indicate that timer "n" is intended
+ * to be used for as a quadrature encoder.
+ */
+
+#ifndef CONFIG_STM32_TIM1
+# undef CONFIG_STM32_TIM1_QE
+#endif
+#ifndef CONFIG_STM32_TIM2
+# undef CONFIG_STM32_TIM2_QE
+#endif
+#ifndef CONFIG_STM32_TIM3
+# undef CONFIG_STM32_TIM3_QE
+#endif
+#ifndef CONFIG_STM32_TIM4
+# undef CONFIG_STM32_TIM4_QE
+#endif
+#ifndef CONFIG_STM32_TIM5
+# undef CONFIG_STM32_TIM5_QE
+#endif
+#ifndef CONFIG_STM32_TIM8
+# undef CONFIG_STM32_TIM8_QE
+#endif
+
+/* Only timers 2-5, and 1 & 8 can be used as a quadrature encoder (at least for the
+ * STM32 F4)
+ */
+
+#undef CONFIG_STM32_TIM6_QE
+#undef CONFIG_STM32_TIM7_QE
+#undef CONFIG_STM32_TIM9_QE
+#undef CONFIG_STM32_TIM10_QE
+#undef CONFIG_STM32_TIM11_QE
+#undef CONFIG_STM32_TIM12_QE
+#undef CONFIG_STM32_TIM13_QE
+#undef CONFIG_STM32_TIM14_QE
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
/************************************************************************************
* Name: stm32_qeinitialize
@@ -59,13 +100,14 @@
*
* Input Parameters:
* devpath - The full path to the driver to register. E.g., "/dev/qe0"
+ * tim - The timer number to used. time must be an element of {1,2,3,4,5,8}
*
* Returned Values:
* Zero on success; A negated errno value is returned on failure.
*
************************************************************************************/
-int stm32_qeinitialize(FAR const char *devpath);
+int stm32_qeinitialize(FAR const char *devpath, int tim);
#endif /* CONFIG_QENCODER */
#endif /* __ARCH_ARM_SRC_STM32_STM32_QENCODER_H */
diff --git a/nuttx/arch/arm/src/stm32/stm32_tim.c b/nuttx/arch/arm/src/stm32/stm32_tim.c
index 39b0b8295..d19c69c03 100644
--- a/nuttx/arch/arm/src/stm32/stm32_tim.c
+++ b/nuttx/arch/arm/src/stm32/stm32_tim.c
@@ -81,49 +81,67 @@
* CONFIG_STM32_TIMn_DAC may also be defined to indicate that timer "n" is intended
* to be used for that purpose.
*
+ * - To use a Quadrature Encoder. If CONFIG_STM32_TIMn is defined then
+ * CONFIG_STM32_TIMn_QE may also be defined to indicate that timer "n" is intended
+ * to be used for that purpose.
+ *
* In any of these cases, the timer will not be used by this timer module.
*/
-#if defined(CONFIG_STM32_TIM1_PWM) || defined (CONFIG_STM32_TIM1_ADC) || defined(CONFIG_STM32_TIM1_DAC)
+#if defined(CONFIG_STM32_TIM1_PWM) || defined (CONFIG_STM32_TIM1_ADC) || \
+ defined(CONFIG_STM32_TIM1_DAC) || defined(CONFIG_STM32_TIM1_QE)
# undef CONFIG_STM32_TIM1
#endif
-#if defined(CONFIG_STM32_TIM2_PWM) || defined (CONFIG_STM32_TIM2_ADC) || defined(CONFIG_STM32_TIM2_DAC)
+#if defined(CONFIG_STM32_TIM2_PWM) || defined (CONFIG_STM32_TIM2_ADC) || \
+ defined(CONFIG_STM32_TIM2_DAC) || defined(CONFIG_STM32_TIM2_QE)
# undef CONFIG_STM32_TIM2
#endif
-#if defined(CONFIG_STM32_TIM3_PWM) || defined (CONFIG_STM32_TIM3_ADC) || defined(CONFIG_STM32_TIM3_DAC)
+#if defined(CONFIG_STM32_TIM3_PWM) || defined (CONFIG_STM32_TIM3_ADC) || \
+ defined(CONFIG_STM32_TIM3_DAC) || defined(CONFIG_STM32_TIM3_QE)
# undef CONFIG_STM32_TIM3
#endif
-#if defined(CONFIG_STM32_TIM4_PWM) || defined (CONFIG_STM32_TIM4_ADC) || defined(CONFIG_STM32_TIM4_DAC)
+#if defined(CONFIG_STM32_TIM4_PWM) || defined (CONFIG_STM32_TIM4_ADC) || \
+ defined(CONFIG_STM32_TIM4_DAC) || defined(CONFIG_STM32_TIM4_QE)
# undef CONFIG_STM32_TIM4
#endif
-#if defined(CONFIG_STM32_TIM5_PWM) || defined (CONFIG_STM32_TIM5_ADC) || defined(CONFIG_STM32_TIM5_DAC)
+#if defined(CONFIG_STM32_TIM5_PWM) || defined (CONFIG_STM32_TIM5_ADC) || \
+ defined(CONFIG_STM32_TIM5_DAC) || defined(CONFIG_STM32_TIM5_QE)
# undef CONFIG_STM32_TIM5
#endif
-#if defined(CONFIG_STM32_TIM6_PWM) || defined (CONFIG_STM32_TIM6_ADC) || defined(CONFIG_STM32_TIM6_DAC)
+#if defined(CONFIG_STM32_TIM6_PWM) || defined (CONFIG_STM32_TIM6_ADC) || \
+ defined(CONFIG_STM32_TIM6_DAC) || defined(CONFIG_STM32_TIM6_QE)
# undef CONFIG_STM32_TIM6
#endif
-#if defined(CONFIG_STM32_TIM7_PWM) || defined (CONFIG_STM32_TIM7_ADC) || defined(CONFIG_STM32_TIM7_DAC)
+#if defined(CONFIG_STM32_TIM7_PWM) || defined (CONFIG_STM32_TIM7_ADC) || \
+ defined(CONFIG_STM32_TIM7_DAC) || defined(CONFIG_STM32_TIM7_QE)
# undef CONFIG_STM32_TIM7
#endif
-#if defined(CONFIG_STM32_TIM8_PWM) || defined (CONFIG_STM32_TIM8_ADC) || defined(CONFIG_STM32_TIM8_DAC)
+#if defined(CONFIG_STM32_TIM8_PWM) || defined (CONFIG_STM32_TIM8_ADC) || \
+ defined(CONFIG_STM32_TIM8_DAC) || defined(CONFIG_STM32_TIM8_QE)
# undef CONFIG_STM32_TIM8
#endif
-#if defined(CONFIG_STM32_TIM9_PWM) || defined (CONFIG_STM32_TIM9_ADC) || defined(CONFIG_STM32_TIM9_DAC)
+#if defined(CONFIG_STM32_TIM9_PWM) || defined (CONFIG_STM32_TIM9_ADC) || \
+ defined(CONFIG_STM32_TIM9_DAC) || defined(CONFIG_STM32_TIM9_QE)
# undef CONFIG_STM32_TIM9
#endif
-#if defined(CONFIG_STM32_TIM10_PWM) || defined (CONFIG_STM32_TIM10_ADC) || defined(CONFIG_STM32_TIM10_DAC)
+#if defined(CONFIG_STM32_TIM10_PWM) || defined (CONFIG_STM32_TIM10_ADC) || \
+ defined(CONFIG_STM32_TIM10_DAC) || defined(CONFIG_STM32_TIM10_QE)
# undef CONFIG_STM32_TIM10
#endif
-#if defined(CONFIG_STM32_TIM11_PWM) || defined (CONFIG_STM32_TIM11_ADC) || defined(CONFIG_STM32_TIM11_DAC)
+#if defined(CONFIG_STM32_TIM11_PWM) || defined (CONFIG_STM32_TIM11_ADC) || \
+ defined(CONFIG_STM32_TIM11_DAC) || defined(CONFIG_STM32_TIM11_QE)
# undef CONFIG_STM32_TIM11
#endif
-#if defined(CONFIG_STM32_TIM12_PWM) || defined (CONFIG_STM32_TIM12_ADC) || defined(CONFIG_STM32_TIM12_DAC)
+#if defined(CONFIG_STM32_TIM12_PWM) || defined (CONFIG_STM32_TIM12_ADC) || \
+ defined(CONFIG_STM32_TIM12_DAC) || defined(CONFIG_STM32_TIM12_QE)
# undef CONFIG_STM32_TIM12
#endif
-#if defined(CONFIG_STM32_TIM13_PWM) || defined (CONFIG_STM32_TIM13_ADC) || defined(CONFIG_STM32_TIM13_DAC)
+#if defined(CONFIG_STM32_TIM13_PWM) || defined (CONFIG_STM32_TIM13_ADC) || \
+ defined(CONFIG_STM32_TIM13_DAC) || defined(CONFIG_STM32_TIM13_QE)
# undef CONFIG_STM32_TIM13
#endif
-#if defined(CONFIG_STM32_TIM14_PWM) || defined (CONFIG_STM32_TIM14_ADC) || defined(CONFIG_STM32_TIM14_DAC)
+#if defined(CONFIG_STM32_TIM14_PWM) || defined (CONFIG_STM32_TIM14_ADC) || \
+ defined(CONFIG_STM32_TIM14_DAC) || defined(CONFIG_STM32_TIM14_QE)
# undef CONFIG_STM32_TIM14
#endif