fw: drivers: add STM32 microphone drivers

This commit is contained in:
Liam McLoughlin 2025-02-13 11:35:05 +00:00
parent 3f343671ce
commit f19ace2e3c
10 changed files with 1059 additions and 3 deletions

View file

@ -20,6 +20,7 @@
#include "drivers/exti.h"
#include "drivers/flash/qspi_flash_definitions.h"
#include "drivers/i2c_definitions.h"
#include "drivers/mic/stm32/dfsdm_definitions.h"
#include "drivers/pmic.h"
#include "drivers/qspi_definitions.h"
#include "drivers/stm32f2/dma_definitions.h"
@ -707,6 +708,34 @@ static QSPIFlash QSPI_FLASH_DEVICE = {
QSPIFlash * const QSPI_FLASH = &QSPI_FLASH_DEVICE;
static MicDeviceState DMA_BSS s_mic_state;
static MicDevice MIC_DEVICE = {
.state = &s_mic_state,
.filter = DFSDM_Filter0,
.channel = DFSDM_Channel0,
.extremes_detector_channel = DFSDM_ExtremChannel0,
.regular_channel = DFSDM_RegularChannel0,
.pdm_frequency = MHZ_TO_HZ(2),
.rcc_apb_periph = RCC_APB2Periph_DFSDM,
.dma = &DFSDM_DMA_REQUEST,
.ck_gpio = { GPIOD, GPIO_Pin_3, GPIO_PinSource3, GPIO_AF3_DFSDM },
.sd_gpio = { GPIOC, GPIO_Pin_1, GPIO_PinSource1, GPIO_AF3_DFSDM },
#if BOARD_ROBERT_BB || BOARD_ROBERT_EVT || BOARD_CUTTS_BB
.mic_power_state_fn = set_ldo3_power_state,
#elif BOARD_ROBERT_BB2
// the mic is always powered
#else
#error "Unknown board"
#endif
.power_on_delay_ms = 30,
.settling_delay_ms = 100,
.default_volume = 64,
.final_right_shift = 11,
};
MicDevice * const MIC = &MIC_DEVICE;
void board_early_init(void) {
spi_slave_port_init(ICE40LP->spi_port);
}

View file

@ -305,6 +305,8 @@ extern QSPIFlash * const QSPI_FLASH;
extern ICE40LPDevice * const ICE40LP;
extern SPISlavePort * const DIALOG_SPI;
extern MicDevice * const MIC;
#if BOARD_CUTTS_BB
extern TouchSensor * const EWD1000;
#endif

View file

@ -19,6 +19,7 @@
#include "drivers/exti.h"
#include "drivers/flash/qspi_flash_definitions.h"
#include "drivers/i2c_definitions.h"
#include "drivers/mic/stm32/dfsdm_definitions.h"
#include "drivers/qspi_definitions.h"
#include "drivers/stm32f2/dma_definitions.h"
#include "drivers/stm32f2/i2c_hal_definitions.h"
@ -414,6 +415,27 @@ static QSPIFlash QSPI_FLASH_DEVICE = {
QSPIFlash * const QSPI_FLASH = &QSPI_FLASH_DEVICE;
static MicDeviceState s_mic_state;
static MicDevice MIC_DEVICE = {
.state = &s_mic_state,
.filter = DFSDM_Filter0,
.channel = DFSDM_Channel2,
.extremes_detector_channel = DFSDM_ExtremChannel2,
.regular_channel = DFSDM_RegularChannel2,
.pdm_frequency = MHZ_TO_HZ(2),
.rcc_apb_periph = RCC_APB2Periph_DFSDM,
.dma = &DFSDM_DMA_REQUEST,
.ck_gpio = { GPIOC, GPIO_Pin_2, GPIO_PinSource2, GPIO_AF8_DFSDM },
.sd_gpio = { GPIOB, GPIO_Pin_14, GPIO_PinSource14, GPIO_AF8_DFSDM },
.power_on_delay_ms = 50,
.settling_delay_ms = 0,
.default_volume = 64,
.final_right_shift = 11,
};
MicDevice * const MIC = &MIC_DEVICE;
void board_early_init(void) {
}

View file

@ -238,4 +238,6 @@ extern const TemperatureSensor * const TEMPERATURE_SENSOR;
extern QSPIPort * const QSPI;
extern QSPIFlash * const QSPI_FLASH;
extern MicDevice * const MIC;
extern SPISlavePort * const DIALOG_SPI;

View file

@ -0,0 +1,468 @@
/*
* Copyright 2024 Google LLC
*
* Licensed 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.
*/
#include "drivers/mic.h"
#include "dfsdm_definitions.h"
#include "board/board.h"
#include "console/prompt.h"
#include "drivers/dma.h"
#include "drivers/gpio.h"
#include "drivers/periph_config.h"
#include "kernel/events.h"
#include "kernel/util/sleep.h"
#include "kernel/util/stop.h"
#include "os/mutex.h"
#include "os/tick.h"
#include "services/common/system_task.h"
#include "system/logging.h"
#include "system/passert.h"
#include "system/profiler.h"
#include "util/circular_buffer.h"
#define STM32F4_COMPATIBLE
#define STM32F7_COMPATIBLE
#include <mcu.h>
#include "FreeRTOS.h"
#include "semphr.h"
#define MAX_VOLUME (256)
#define LFSR_SEED (0x3AEF)
static bool prv_dma_handler(DMARequest *request, void *context, bool is_complete);
static void prv_enable_clocks(MicDevice *this) {
// Enable the device clocks
periph_config_acquire_lock();
periph_config_enable(this->filter, this->rcc_apb_periph);
periph_config_release_lock();
}
static void prv_disable_clocks(MicDevice *this) {
// Disable DFSDM clock
periph_config_acquire_lock();
periph_config_disable(this->filter, this->rcc_apb_periph);
periph_config_release_lock();
}
//! Configure GPIOs for DFSDM use
static void prv_enable_gpio(MicDevice *this, GPIOPuPd_TypeDef data_pupd) {
gpio_af_init(&this->ck_gpio, GPIO_OType_PP,
GPIO_Medium_Speed, GPIO_PuPd_NOPULL);
// During normal operation it is probably more power-efficient to let
// the data pin float (no pull) as no current would be wasted pulling
// against the data signal. But the mic's data output pin goes Hi-Z
// for half of each clock cycle, so power could be wasted on the
// input if the signal voltage moves around too much while the mic
// data output is Hi-Z.
//
// During self-test we want to enable a pull resistor so that we can
// accurately detect the absence of the mic.
gpio_af_init(&this->sd_gpio, GPIO_OType_PP,
GPIO_Medium_Speed, data_pupd);
}
//! Configure GPIOs for lowest power consumption
static void prv_disable_gpio(MicDevice *this) {
// Configure the clock pin as an output driving low so the microphone
// won't see any unintentional clock edges which would wake it up
// from sleep mode.
gpio_af_configure_fixed_output(&this->ck_gpio, false);
// Configure the data pin as an analog input, which is the lowest
// power state it can be in. The mic's data pin goes into Hi-Z mode
// when the mic is asleep, so the signal could float around and waste
// power if the pin is configured as a digital input.
gpio_af_configure_low_power(&this->sd_gpio);
}
static void prv_dfsdm_configure(MicDevice *this) {
const uint32_t k_max_sinc4_osr = 255;
PBL_ASSERTN(this->pdm_frequency > 0);
PBL_ASSERTN(this->pdm_frequency % MIC_SAMPLE_RATE == 0);
uint32_t oversampling_ratio = this->pdm_frequency / MIC_SAMPLE_RATE;
uint32_t sinc_order = (oversampling_ratio <= k_max_sinc4_osr) ? DFSDM_SincOrder_Sinc4 :
DFSDM_SincOrder_Sinc3;
// Calculate the right shift needed to contain the final value within 24 bits
int num_bits = ceil_log_two(oversampling_ratio);
num_bits *= (sinc_order == DFSDM_SincOrder_Sinc4) ? 4 : 3;
uint32_t right_shift = MAX(num_bits - 24, 0);
RCC_ClocksTypeDef clocks;
RCC_GetClocksFreq(&clocks);
uint32_t prescaler = clocks.PCLK2_Frequency / this->pdm_frequency;
// Disable the device before changing the config
DFSDM_Cmd(DISABLE);
DFSDM_ChannelCmd(this->channel, DISABLE);
DFSDM_FilterCmd(this->filter, DISABLE);
DFSDM_TransceiverInitTypeDef DFSDM_InitStruct;
DFSDM_TransceiverStructInit(&DFSDM_InitStruct);
DFSDM_InitStruct.DFSDM_Interface = DFSDM_Interface_SPI_RisingEdge;
DFSDM_InitStruct.DFSDM_Clock = DFSDM_Clock_Internal;
DFSDM_InitStruct.DFSDM_Input = DFSDM_Input_External;
DFSDM_InitStruct.DFSDM_Redirection = DFSDM_Redirection_Disabled;
DFSDM_InitStruct.DFSDM_PackingMode = DFSDM_PackingMode_Standard;
DFSDM_InitStruct.DFSDM_DataRightShift = right_shift;
DFSDM_InitStruct.DFSDM_Offset = 0;
DFSDM_InitStruct.DFSDM_CLKAbsenceDetector = DFSDM_CLKAbsenceDetector_Disable;
DFSDM_InitStruct.DFSDM_ShortCircuitDetector = DFSDM_ShortCircuitDetector_Enable;
DFSDM_TransceiverInit(this->channel, &DFSDM_InitStruct);
DFSDM_FilterInitTypeDef DFSDM_FilterInitStruct;
DFSDM_FilterStructInit(&DFSDM_FilterInitStruct);
DFSDM_FilterInitStruct.DFSDM_SincOrder = sinc_order;
DFSDM_FilterInitStruct.DFSDM_FilterOversamplingRatio = oversampling_ratio;
DFSDM_FilterInitStruct.DFSDM_IntegratorOversamplingRatio = 1;
DFSDM_FilterInit(this->filter, &DFSDM_FilterInitStruct);
DFSDM_ConfigClkOutputSource(DFSDM_ClkOutSource_SysClock);
DFSDM_ConfigClkOutputDivider(prescaler);
DFSDM_SelectRegularChannel(this->filter, this->regular_channel);
DFSDM_FastModeCmd(this->filter, ENABLE);
}
// must have initialized both the DFSDM and DMA
static void prv_dfsdm_enable(MicDevice *this) {
// Enable DFSDM and DMA
DFSDM_Cmd(ENABLE);
DFSDM_ChannelCmd(this->channel, ENABLE);
// Wait for microphone to power up
psleep(this->power_on_delay_ms);
// Configure DFSDM to use DMA and start the filter + DMA
DFSDM_DMATransferConfig(this->filter, DFSDM_DMAConversionMode_Regular, ENABLE);
DFSDM_FilterCmd(this->filter, ENABLE);
DFSDM_RegularContinuousModeCmd(this->filter, ENABLE);
DFSDM_StartSoftwareRegularConversion(this->filter);
dma_request_start_circular(this->dma, this->state->in_buffer, (void *)&this->filter->RDATAR,
sizeof(this->state->in_buffer), prv_dma_handler, (void *)this);
}
static void prv_dfsdm_disable(MicDevice *this) {
// Disable DMA and DFSDM
dma_request_stop(this->dma);
DFSDM_ChannelCmd(this->channel, DISABLE);
DFSDM_Cmd(DISABLE);
prv_disable_clocks(this);
}
void mic_init(MicDevice *this) {
PBL_ASSERTN(!this->state->initialized);
this->state->main_pending = false;
this->state->bg_pending = false;
this->state->running = false;
this->state->subscriber = (struct MicSubscriber){0};
this->state->volume = this->default_volume;
prv_disable_gpio(this);
dma_request_init(this->dma);
this->state->mic_mutex = mutex_create_recursive();
this->state->initialized = true;
}
void mic_set_volume(MicDevice *this, uint16_t volume) {
this->state->volume = MIN(MAX_VOLUME, volume);
}
bool mic_start(MicDevice *this, MicDataHandlerCB data_handler, void *context,
int16_t *audio_buffer, size_t audio_buffer_len) {
PBL_ASSERTN(this->state->initialized);
mutex_lock_recursive(this->state->mic_mutex);
bool success = false;
if (this->state->running) {
goto unlock;
}
circular_buffer_init(&this->state->circ_buffer,
this->state->circ_buf_store,
DFSDM_CIRC_BUFFER_SIZE);
this->state->subscriber = (struct MicSubscriber) {
.callback = data_handler,
.buffer = audio_buffer,
.context = context,
.size = audio_buffer_len,
.idx = 0
};
this->state->overflow_cnt = 0;
this->state->bytes_received = 0;
this->state->samples_to_discard = (MIC_SAMPLE_RATE * this->settling_delay_ms) / MS_PER_SECOND;
this->state->hpf_y1 = 0;
if (this->mic_power_state_fn) {
this->mic_power_state_fn(true);
}
// Seed the LFSR random number generator
this->state->prev_r = LFSR_SEED;
prv_enable_gpio(this, GPIO_PuPd_NOPULL);
prv_enable_clocks(this);
prv_dfsdm_configure(this);
prv_dfsdm_enable(this);
DFSDM_RegularContinuousModeCmd(this->filter, ENABLE);
DFSDM_StartSoftwareRegularConversion(this->filter);
stop_mode_disable(InhibitorMic);
this->state->running = true;
success = true;
unlock:
mutex_unlock_recursive(this->state->mic_mutex);
return success;
}
void mic_stop(MicDevice *this) {
mutex_lock_recursive(this->state->mic_mutex);
if (!this->state->running) {
goto unlock;
}
prv_dfsdm_disable(this);
prv_disable_gpio(this);
if (this->mic_power_state_fn) {
this->mic_power_state_fn(false);
}
stop_mode_enable(InhibitorMic);
this->state->running = false;
PBL_LOG(LOG_LEVEL_DEBUG, "Stopped microphone, dropped samples: %"PRIu32" bytes received: %"PRIu32,
this->state->overflow_cnt,
this->state->bytes_received);
unlock:
mutex_unlock_recursive(this->state->mic_mutex);
}
bool mic_is_running(MicDevice *this) {
return this->state->running;
}
static void prv_dispatch_samples(void *context_ptr) {
MicDevice *this = context_ptr;
mutex_lock_recursive(this->state->mic_mutex);
// if this->running is set to false (mic_stop is called)
// while the loop is running, the remaining samples must be discarded.
// If mic_stop is called from the subscriber callback, no more samples
// must be read into the subscriber buffer (it is assumed to be invalid
// memory at that point)
while (this->state->running) {
uint16_t size = circular_buffer_copy(
&this->state->circ_buffer,
(uint8_t *)&this->state->subscriber.buffer[this->state->subscriber.idx],
((this->state->subscriber.size - this->state->subscriber.idx) *
sizeof(int16_t)));
if (size == 0) {
break;
}
// Only call the subscriber when the buffer is full. This takes away the
// overhead of handling this in the subscriber module
this->state->subscriber.idx += (size / sizeof(int16_t));
if (this->state->subscriber.idx == this->state->subscriber.size) {
this->state->subscriber.callback(this->state->subscriber.buffer,
this->state->subscriber.idx,
this->state->subscriber.context);
this->state->subscriber.idx = 0;
}
// Make sure to maintain correct alignment when consuming bytes
size -= size % sizeof(int16_t);
circular_buffer_consume(&this->state->circ_buffer, size);
}
mutex_unlock_recursive(this->state->mic_mutex);
}
static void prv_dispatch_samples_main(void *context_ptr) {
MicDevice *this = context_ptr;
this->state->main_pending = false;
prv_dispatch_samples(context_ptr);
}
static void prv_dispatch_samples_bg(void *context_ptr) {
MicDevice *this = context_ptr;
this->state->bg_pending = false;
prv_dispatch_samples(context_ptr);
}
// Interrupt functions
////////////////////////////////////////////////////////////////////////////////////////////////
// Galois LFSR random number generator for dithering
static int16_t prv_get_lfsr(int16_t prev) {
uint16_t lfsr = (uint16_t)prev;
lfsr = (lfsr >> 1) ^ (-(lfsr & 1) & 0xB400);
return (int16_t)lfsr;
}
static bool prv_dma_handler(DMARequest *request, void *context, bool is_complete) {
MicDevice *this = context;
bool should_context_switch = false;
PROFILER_NODE_START(mic);
const int32_t *dfsdm_buffer = this->state->in_buffer[is_complete ? 1 : 0];
if (this->state->bytes_received == 0) {
// Seed the filter state to prevent transient at the beginning of recording
this->state->hpf_y1 = dfsdm_buffer[0] >> 8;
}
this->state->bytes_received += DFSDM_BUFFER_LENGTH * sizeof(*dfsdm_buffer);
for (int i = 0; i < DFSDM_BUFFER_LENGTH; i++) {
// move 24-bit value stored in upper 24-bits down to lower 24 bits
int32_t sample = dfsdm_buffer[i] >> 8;
// Single pole IIR filter to remove DC offset (cutoff frequency: 10Hz)
// Filter coefficients pre-calculated
const int64_t b1 = 32639; // b1 = exp(2 * PI * Fc) where Fc is 10/16000, scaled by 2^15
const int64_t a0 = 129; // a0 = 1 - b1, scaled by 2^15
// Filter calculation
int64_t y = a0 * (int64_t)sample + b1 * this->state->hpf_y1;
// Scale down the value
this->state->hpf_y1 = y >> 15;
// HPF derived from low pass filter result
sample -= this->state->hpf_y1;
// Gain control - multiply the sample by up to MAX_VOLUME (256). This adds at most 8 bits to
// the signal for full 32-bit resolution. Afterwards, add dither to reduce quantization noise,
// then shift the signal down to fit it into 16 bits.
// Note: we do not shift by the full 16 bits because, for normal speech, the received signal
// is closer to 20/21 bits on bigboards.
// Apply volume control
sample *= this->state->volume;
// Dither the sample by adding 2-LSB (post-shift) random TPDF (Triangular Probability Density
// Function) noise. Create TPDF noise by summing 2 random numbers together. Use the previously
// generated random number for computational efficiency (low pass filters the noise somewhat,
// but the result is good). Shift the value
int16_t r = prv_get_lfsr(this->state->prev_r);
// divide by 2 to prevent overflows
int16_t tpdf = (r / 2) + (this->state->prev_r / 2);
this->state->prev_r = r;
tpdf >>= this->final_right_shift - 1; // Shift to 2-LSB size relative to sample
sample += tpdf;
// Shift result to final bit depth and clip
sample >>= this->final_right_shift;
__SSAT(sample, 16); // saturate because we're clipping off the top of a 32-bit value
if (this->state->samples_to_discard > 0) {
this->state->samples_to_discard--;
} else {
if (!circular_buffer_write(&this->state->circ_buffer,
(const uint8_t *) &sample,
sizeof(int16_t))) {
this->state->overflow_cnt++;
break;
}
}
}
// Post a callback to KernelBG and KernelMain for faster servicing (check out PBL-40943 for more
// details)
bool main_switch_context = false;
bool system_task_switch_context = false;
if (!this->state->main_pending) {
this->state->main_pending = true;
PebbleEvent e = {
.type = PEBBLE_CALLBACK_EVENT,
.callback = {
.callback = prv_dispatch_samples_main,
.data = (void *)this
}
};
main_switch_context = event_put_isr(&e);
}
if (!this->state->bg_pending) {
this->state->bg_pending = true;
system_task_add_callback_from_isr(prv_dispatch_samples_bg, (void *)this,
&system_task_switch_context);
}
PROFILER_NODE_STOP(mic);
return system_task_switch_context || main_switch_context;
}
extern MicDevice * const MIC;
bool mic_selftest(void) {
stop_mode_disable(InhibitorMic);
// Configure mic serial data pin with pull-down so that the GPIO does
// not float if there is an open-circuit condition on the pin.
prv_enable_gpio(MIC, GPIO_PuPd_DOWN);
// Configure the DFSDM peripheral with short-circuit detection.
prv_enable_clocks(MIC);
// Set the short-circut threshold length to its maximum value.
DFSDM_ConfigShortCircuitThreshold(DFSDM_Channel2, 255);
prv_dfsdm_configure(MIC);
// Start DFSDM conversion without DMA. Throw out the samples; we only
// care about the short-circuit detection.
DFSDM_Cmd(ENABLE);
DFSDM_ChannelCmd(DFSDM_Channel2, ENABLE);
DFSDM_FilterCmd(DFSDM_Filter0, ENABLE);
DFSDM_RegularContinuousModeCmd(DFSDM_Filter0, ENABLE);
DFSDM_StartSoftwareRegularConversion(DFSDM_Filter0);
// Wait until the microphone wakes up. The datasheet max wakeup time
// for the two microphones we may use on Silk is around 30 ms. Add in
// some extra margin in case we make a running change and use another
// microphone that is even slower to wake up.
psleep(50);
// Run the actual test.
DFSDM_ClearShortCircuitFlag(DFSDM_CLEARF_SCD_Channel2);
psleep(10);
const bool test_pass = DFSDM_GetShortCircuitFlagStatus(DFSDM_IT_SCD_Channel2) != SET;
prv_dfsdm_disable(MIC);
prv_disable_gpio(MIC);
stop_mode_enable(InhibitorMic);
return test_pass;
}

View file

@ -0,0 +1,91 @@
/*
* Copyright 2024 Google LLC
*
* Licensed 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.
*/
#pragma once
#include "board/board.h"
#include "drivers/mic.h"
#include <os/mutex.h>
#include <util/circular_buffer.h>
#include <stdbool.h>
#include <stdint.h>
#define DFSDM_BUFFER_LENGTH (256) // Each buffer fills up 64 times per second
#define DFSDM_CIRC_BUFFER_SIZE (DFSDM_BUFFER_LENGTH * sizeof(uint16_t) * 2)
// Note: If the MCU has cache, this needs to be placed in DMA_BSS.
typedef struct MicState {
int32_t in_buffer[2][DFSDM_BUFFER_LENGTH]; // 2k
uint8_t circ_buf_store[DFSDM_CIRC_BUFFER_SIZE]; // 1k
CircularBuffer circ_buffer;
bool initialized;
bool running;
bool main_pending;
bool bg_pending;
uint32_t overflow_cnt;
uint32_t bytes_received;
int samples_to_discard;
int64_t hpf_y1; // Previous value of HPF output
int16_t prev_r; // Previous random number generated for dithering
uint16_t volume;
// A mutex is needed to protect against a race condition between
// mic_stop and the dispatch routine potentially resulting in the
// deallocation of the subscriber module's receive buffer while the
// dispatch routine is still running.
PebbleRecursiveMutex *mic_mutex;
struct MicSubscriber {
MicDataHandlerCB callback;
int16_t *buffer;
void *context;
size_t size;
size_t idx;
} subscriber;
} MicDeviceState;
typedef const struct MicDevice {
MicDeviceState *state;
DFSDM_TypeDef *filter;
DFSDM_Channel_TypeDef *channel;
uint32_t extremes_detector_channel;
uint32_t regular_channel;
uint32_t pdm_frequency;
uint32_t rcc_apb_periph;
DMARequest *dma;
AfConfig ck_gpio;
AfConfig sd_gpio;
int power_on_delay_ms;
int settling_delay_ms;
// Volume scalar (max 256)
uint16_t default_volume;
// Final right shift after applying gain control. It should be adjusted per watch family such that
// a volume of 128 provides approximate half of full-scale deflection for normal speech on a
// bigboard
int final_right_shift;
//! Function pointer to power the microphone hardware on or off.
//! May be NULL.
void (*mic_power_state_fn)(bool enabled);
} MicDevice;

View file

@ -0,0 +1,420 @@
/*
* Copyright 2024 Google LLC
*
* Licensed 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.
*/
#include "drivers/mic.h"
#include "board/board.h"
#include "drivers/accessory.h"
#include "drivers/dma.h"
#include "drivers/gpio.h"
#include "drivers/periph_config.h"
#include "drivers/pmic.h"
#include "kernel/events.h"
#include "kernel/pbl_malloc.h"
#include "kernel/util/stop.h"
#include "mfg/mfg_mode/mfg_factory_mode.h"
#include "services/common/system_task.h"
#if RECOVERY_FW
#include "services/prf/accessory/accessory_manager.h"
#else
#include "services/normal/accessory/accessory_manager.h"
#endif
#include "system/logging.h"
#include "os/mutex.h"
#include "system/passert.h"
#include "system/profiler.h"
#include "util/circular_buffer.h"
#include "util/legacy_checksum.h"
#include "util/math.h"
#include "util/net.h"
#include "util/size.h"
#define STM32F4_COMPATIBLE
#include <mcu.h>
#include "vendor/ST-libPDM/pdm_filter.h"
#define DECIMATION_FACTOR (64)
#define IN_BUF_BATCH_SIZE (8)
#define CIRCULAR_BUF_BATCH_SIZE (10)
// This gives two 1K byte buffers for DMA, which will each fill in about 1/64s
#define IN_BUFFER_LENGTH (DECIMATION_FACTOR * IN_BUF_BATCH_SIZE)
static uint16_t s_in_buffer[2][IN_BUFFER_LENGTH];
static uint8_t s_circ_buf_store[(MIC_SAMPLE_RATE / 1000) * CIRCULAR_BUF_BATCH_SIZE * 4
* sizeof(uint16_t)];
static PDMFilter_InitStruct s_pdm_filter;
static uint16_t s_volume;
static bool s_running = false;
static bool s_main_pending = false;
static bool s_bg_pending = false;
static int s_overflow_cnt;
static bool s_initialized = false;
static uint8_t s_discarded;
static CircularBuffer s_circ_buffer;
// A mutex is needed to protect against a race condition between mic_stop and the dispatch routine
// potentially resulting in the deallocation of the subscriber module's receive buffer while the
// dispatch routine is still running.
static PebbleRecursiveMutex *s_mic_mutex;
static struct Subscriber {
MicDataHandlerCB callback;
int16_t *buffer;
void *context;
size_t size;
size_t idx;
} s_subscriber;
static bool prv_dma_handler(DMARequest *request, void *context, bool is_complete);
//! Initialize power management for the microphone. Note that different boards have different
//! ways of configuring power to the mic.
static void prv_mic_power_init(void) {
if (BOARD_CONFIG.mic_config.mic_gpio_power.gpio) {
gpio_use(BOARD_CONFIG.mic_config.mic_gpio_power.gpio);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
GPIO_InitStructure.GPIO_Pin = BOARD_CONFIG.mic_config.mic_gpio_power.gpio_pin;
GPIO_Init(GPIOF, &GPIO_InitStructure);
gpio_output_set(&BOARD_CONFIG.mic_config.mic_gpio_power, false);
gpio_release(BOARD_CONFIG.mic_config.mic_gpio_power.gpio);
}
}
static void prv_i2s_gpio_init(void) {
// Enable the SPI clock
periph_config_acquire_lock();
// [AS] TODO: If I2S is moved to SPI1, this RCC function needs to be abstracted to board.h
PBL_ASSERTN(BOARD_CONFIG.mic_config.spi_clock_ctrl != RCC_APB2Periph_SPI1);
periph_config_enable(BOARD_CONFIG.mic_config.spi,
BOARD_CONFIG.mic_config.spi_clock_ctrl);
periph_config_release_lock();
gpio_use(BOARD_CONFIG.mic_config.i2s_ck.gpio);
gpio_use(BOARD_CONFIG.mic_config.i2s_sd.gpio);
// Configure pins as SPI/I2S pins
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;
// I2S CK
GPIO_InitStructure.GPIO_Pin = BOARD_CONFIG.mic_config.i2s_ck.gpio_pin;
GPIO_Init(BOARD_CONFIG.mic_config.i2s_ck.gpio, &GPIO_InitStructure);
GPIO_PinAFConfig(BOARD_CONFIG.mic_config.i2s_ck.gpio,
BOARD_CONFIG.mic_config.i2s_ck.gpio_pin_source, BOARD_CONFIG.mic_config.i2s_ck.gpio_af);
// I2S SD
GPIO_InitStructure.GPIO_Pin = BOARD_CONFIG.mic_config.i2s_sd.gpio_pin;
GPIO_Init(BOARD_CONFIG.mic_config.i2s_sd.gpio, &GPIO_InitStructure);
GPIO_PinAFConfig(BOARD_CONFIG.mic_config.i2s_sd.gpio,
BOARD_CONFIG.mic_config.i2s_sd.gpio_pin_source, BOARD_CONFIG.mic_config.i2s_sd.gpio_af);
gpio_release(BOARD_CONFIG.mic_config.i2s_ck.gpio);
gpio_release(BOARD_CONFIG.mic_config.i2s_sd.gpio);
// I2S configuration
SPI_I2S_DeInit(BOARD_CONFIG.mic_config.spi);
I2S_InitTypeDef I2S_InitStructure;
I2S_InitStructure.I2S_AudioFreq = I2S_AudioFreq_32k;
I2S_InitStructure.I2S_Standard = I2S_Standard_LSB;
I2S_InitStructure.I2S_DataFormat = I2S_DataFormat_16b;
I2S_InitStructure.I2S_CPOL = I2S_CPOL_High;
I2S_InitStructure.I2S_Mode = I2S_Mode_MasterRx;
I2S_InitStructure.I2S_MCLKOutput = I2S_MCLKOutput_Disable;
I2S_Init(BOARD_CONFIG.mic_config.spi, &I2S_InitStructure);
periph_config_acquire_lock();
periph_config_disable(BOARD_CONFIG.mic_config.spi,
BOARD_CONFIG.mic_config.spi_clock_ctrl);
periph_config_release_lock();
}
static void prv_mic_power_enable(void) {
if (BOARD_CONFIG.mic_config.mic_gpio_power.gpio) {
gpio_output_set(&BOARD_CONFIG.mic_config.mic_gpio_power, true);
} else {
set_ldo3_power_state(true);
}
}
static void prv_mic_power_disable(void) {
if (BOARD_CONFIG.mic_config.mic_gpio_power.gpio) {
gpio_output_set(&BOARD_CONFIG.mic_config.mic_gpio_power, false);
} else {
set_ldo3_power_state(false);
}
}
void mic_init(MicDevice *this) {
PBL_ASSERTN(!s_initialized);
s_main_pending = false;
s_bg_pending = false;
s_running = false;
s_subscriber = (struct Subscriber){0};
s_volume = BOARD_CONFIG.mic_config.gain;
s_pdm_filter.Fs = MIC_SAMPLE_RATE;
s_pdm_filter.LP_HZ = 8000;
s_pdm_filter.HP_HZ = 10;
s_pdm_filter.Out_MicChannels = 1;
s_pdm_filter.In_MicChannels = 1;
dma_request_init(MIC_I2S_RX_DMA);
prv_i2s_gpio_init();
prv_mic_power_init();
s_mic_mutex = mutex_create_recursive();
s_initialized = true;
}
void mic_set_volume(MicDevice *this, uint16_t volume) {
s_volume = volume;
}
bool mic_start(MicDevice *mic, MicDataHandlerCB data_handler, void *context,
int16_t *audio_buffer, size_t audio_buffer_len) {
PBL_ASSERTN(s_initialized);
mutex_lock_recursive(s_mic_mutex);
bool success = false;
if (s_running) {
goto unlock;
}
prv_mic_power_enable();
circular_buffer_init(&s_circ_buffer, s_circ_buf_store, sizeof(s_circ_buf_store));
s_subscriber = (struct Subscriber) {
.callback = data_handler,
.buffer = audio_buffer,
.context = context,
.size = audio_buffer_len,
.idx = 0
};
s_overflow_cnt = 0;
s_discarded = 0;
// The filter library checks that the CRC is present on the platform. Yay DRM
periph_config_enable(CRC, RCC_AHB1Periph_CRC);
CRC_ResetDR();
PDM_Filter_Init(&s_pdm_filter);
periph_config_disable(CRC, RCC_AHB1Periph_CRC);
//Enable I2S PLL
RCC_PLLI2SCmd(ENABLE);
while (RCC_GetFlagStatus(RCC_FLAG_PLLI2SRDY) == RESET) {}
// Enable I2S clock
periph_config_acquire_lock();
periph_config_enable(BOARD_CONFIG.mic_config.spi, BOARD_CONFIG.mic_config.spi_clock_ctrl);
periph_config_release_lock();
// Configure I2S to use DMA
SPI_I2S_DMACmd(BOARD_CONFIG.mic_config.spi, SPI_I2S_DMAReq_Rx, ENABLE);
// Enable I2S
I2S_Cmd(BOARD_CONFIG.mic_config.spi, ENABLE);
// DMA config - use single buffer circular mode. Pointer to buffer is a 2-D array for ease of
// access
void *periph_addr = (void *)&BOARD_CONFIG.mic_config.spi->DR;
stop_mode_disable(InhibitorMic);
dma_request_start_circular(MIC_I2S_RX_DMA, s_in_buffer, periph_addr, sizeof(s_in_buffer),
prv_dma_handler, NULL);
s_running = true;
success = true;
unlock:
mutex_unlock_recursive(s_mic_mutex);
return success;
}
void mic_stop(MicDevice *this) {
mutex_lock_recursive(s_mic_mutex);
if (!s_running) {
goto unlock;
}
// Disable DMA and I2S
dma_request_stop(MIC_I2S_RX_DMA);
I2S_Cmd(BOARD_CONFIG.mic_config.spi, DISABLE);
// Disable I2S clock
periph_config_acquire_lock();
periph_config_disable(BOARD_CONFIG.mic_config.spi,
BOARD_CONFIG.mic_config.spi_clock_ctrl);
periph_config_release_lock();
// Disable I2S PLL
RCC_PLLI2SCmd(DISABLE);
prv_mic_power_disable();
stop_mode_enable(InhibitorMic);
s_running = false;
PBL_LOG(LOG_LEVEL_DEBUG, "Stopped microphone, dropped samples: %d", s_overflow_cnt);
unlock:
mutex_unlock_recursive(s_mic_mutex);
}
bool mic_is_running(MicDevice *this) {
return s_running;
}
static void prv_dispatch_samples_common(void) {
mutex_lock_recursive(s_mic_mutex);
// if s_running is set to false (mic_stop is called) while the loop is running, the
// remaining samples must be discarded. If mic_stop is called from the subscriber callback,
// no more samples must be read into the subscriber buffer (it is assumed to be invalid memory at
// that point)
while (s_running) {
uint16_t size = circular_buffer_copy(&s_circ_buffer,
(uint8_t *) &s_subscriber.buffer[s_subscriber.idx],
((s_subscriber.size - s_subscriber.idx) * sizeof(int16_t)));
if (size == 0) {
break;
}
// Only call the subscriber when the buffer is full. This takes away the
// overhead of handling this in the subscriber module
s_subscriber.idx += (size / sizeof(int16_t));
if (s_subscriber.idx == s_subscriber.size) {
s_subscriber.callback(s_subscriber.buffer, s_subscriber.idx, s_subscriber.context);
s_subscriber.idx = 0;
}
// Make sure to maintain correct alignment when consuming bytes
size -= size % sizeof(int16_t);
circular_buffer_consume(&s_circ_buffer, size);
}
mutex_unlock_recursive(s_mic_mutex);
}
static void prv_dispatch_samples_main(void* data) {
// Setting this to false before we process the data means that we'll have at most 2 callbacks on
// the queue. Putting it after the processing step means that there is a possible race
// condition with setting and clearing the flag that could result in overflow
s_main_pending = false;
prv_dispatch_samples_common();
}
static void prv_dispatch_samples_bg(void* data) {
// Setting this to false before we process the data means that we'll have at most 2 callbacks on
// the queue. Putting it after the processing step means that there is a possible race
// condition with setting and clearing the flag that could result in overflow
s_bg_pending = false;
prv_dispatch_samples_common();
}
// Interrupt functions
////////////////////////////////////////////////////////////////////////////////////////////////
static bool prv_dma_handler(DMARequest *request, void *context, bool is_complete) {
const uint8_t MS_TO_SETTLE = 100;
PROFILER_NODE_START(mic);
uint16_t *pdm_buffer = s_in_buffer[is_complete ? 1 : 0];
// byte endianness needs to be swapped for the filter library
for (size_t i = 0; i < ARRAY_LENGTH(s_in_buffer[0]); i++) {
pdm_buffer[i] = htons(pdm_buffer[i]);
}
bool overflow = false;
uint16_t pcm16_buffer[MIC_SAMPLE_RATE / 1000]; // Store enough for 1 millisecond of data
uint16_t *pdm_end = pdm_buffer + ARRAY_LENGTH(s_in_buffer[0]);
while (pdm_buffer < pdm_end) {
// Process one millisecond of data per call
PDM_Filter_64_LSB((uint8_t *)pdm_buffer, pcm16_buffer, s_volume,
(PDMFilter_InitStruct *)&s_pdm_filter);
pdm_buffer += DECIMATION_FACTOR;
// while the filter is settling discard samples (about 100 ms)
// each iteration of this loop writes 1 ms of data to the buffer
if (s_discarded < MS_TO_SETTLE) {
s_discarded++;
} else if (!circular_buffer_write(&s_circ_buffer, (const uint8_t *) pcm16_buffer,
sizeof(pcm16_buffer))) {
overflow = true;
}
}
// Only count one overflow per interrupt
if (overflow) {
s_overflow_cnt++;
}
// We post an event to both KernelMain and KernelBG. It is critical that the microphone
// data be processed quickly so that we don't encounter buffer overruns. Occasionally
// KernelMain can be busy for long periods of time (24ms to do a display DMA for example)
// so we also post to KernelBG. Whichever happens to get to the event first will process the
// buffer and the other task will quickly find that the buffer has already been emptied.
bool main_switch_context = false;
bool system_task_switch_context = false;
if (!s_main_pending) {
// Only post a callback event if one is not already pending
PebbleEvent e = {
.type = PEBBLE_CALLBACK_EVENT,
.callback = {
.callback = prv_dispatch_samples_main,
.data = NULL
}
};
s_main_pending = true;
main_switch_context = event_put_isr(&e);
}
if (!s_bg_pending) {
// Only post a callback event if one is not already pending
s_bg_pending = true;
system_task_add_callback_from_isr(prv_dispatch_samples_bg, NULL,
&system_task_switch_context);
}
PROFILER_NODE_STOP(mic);
return main_switch_context || system_task_switch_context;
}

View file

@ -761,11 +761,32 @@ if mcu_family in ('STM32F2', 'STM32F4', 'STM32F7'):
],
)
if bld.capability('HAS_MICROPHONE'):
if bld.is_snowy_compatible():
bld.objects(
name='driver_mic',
source=[
'mic/mic_command.c',
'mic/stm32/pdm.c',
],
use=[
'PDMFilter_CM4_GCC',
'driver_accessory',
'driver_dma',
'driver_gpio',
'driver_periph_config',
'driver_pmic',
'fw_includes',
'root_includes',
'freertos',
'stm32_stdlib',
],
)
elif bld.is_silk() or bld.is_cutts() or bld.is_robert():
bld.objects(
name='driver_mic',
source=[
'mic/mic_command.c',
'mic/stm32/dfsdm.c',
],
use=[
'driver_accessory',

View file

@ -50,6 +50,7 @@ bool accel_manager_run_selftest(void) WEAK;
bool gyro_manager_run_selftest(void) WEAK;
bool mag3110_check_whoami(void) WEAK;
bool snowy_mag3110_query_whoami(void) WEAK;
bool mic_selftest(void) WEAK;
// NULL function pointer means test is not implemented
static const struct SelfTestCase s_test_cases[] = {
@ -68,6 +69,7 @@ static const struct SelfTestCase s_test_cases[] = {
#endif
{ "BT Module", bt_driver_test_selftest },
{ "Flash Comm", flash_check_whoami },
{ "Mic", mic_selftest },
{ "Buttons", button_selftest },
};

View file

@ -543,8 +543,7 @@
"spalding",
"silk",
"robert"
],
"ifdefs": ["CAPABILITY_HAS_MICROPHONE=1"]
]
},
{
"id": -91,