l-proger/MotorDriver.Research
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

*

Browse Source
This commit is contained in:
l-proger
2025-05-13 01:34:53 +03:00
parent 427735e23d
commit 83f3f1c7d4
945 changed files with 633484 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_gpio.cpp
+234
View File
@@ -0,0 +1,234 @@
#include "stm32_gpio.hpp"
#define N_EXTI 16
struct subscription_t {
GPIO_TypeDef* port = nullptr;
void (*callback)(void*) = nullptr;
void* ctx = nullptr;
} subscriptions[N_EXTI];
const Stm32Gpio Stm32Gpio::none{nullptr, 0};
/**
* @brief Returns the IRQ number associated with a certain pin.
* Note that all GPIOs with the same pin number map to the same IRQn,
* no matter which port they belong to.
*/
static inline IRQn_Type get_irq_number(uint16_t pin_number) {
switch (pin_number) {
case 0: return EXTI0_IRQn;
case 1: return EXTI1_IRQn;
case 2: return EXTI2_IRQn;
case 3: return EXTI3_IRQn;
case 4: return EXTI4_IRQn;
case 5:
case 6:
case 7:
case 8:
case 9: return EXTI9_5_IRQn;
case 10:
case 11:
case 12:
case 13:
case 14:
case 15: return EXTI15_10_IRQn;
default: return (IRQn_Type)0; // impossible
}
}
#define GPIO_MODE 0x00000003U
#define GPIO_OUTPUT_TYPE 0x00000010U
bool Stm32Gpio::config(uint32_t mode, uint32_t pull, uint32_t speed) {
if (port_ == GPIOA) {
__HAL_RCC_GPIOA_CLK_ENABLE();
} else if (port_ == GPIOB) {
__HAL_RCC_GPIOB_CLK_ENABLE();
} else if (port_ == GPIOC) {
__HAL_RCC_GPIOC_CLK_ENABLE();
} else if (port_ == GPIOD) {
__HAL_RCC_GPIOD_CLK_ENABLE();
} else if (port_ == GPIOE) {
__HAL_RCC_GPIOE_CLK_ENABLE();
} else if (port_ == GPIOF) {
__HAL_RCC_GPIOF_CLK_ENABLE();
} else if (port_ == GPIOG) {
__HAL_RCC_GPIOG_CLK_ENABLE();
} else if (port_ == GPIOH) {
__HAL_RCC_GPIOH_CLK_ENABLE();
} else {
return false;
}
size_t position = get_pin_number();
// The following code is mostly taken from HAL_GPIO_Init
/* Configure IO Direction mode (Input, Output, Alternate or Analog) */
uint32_t temp = port_->MODER;
temp &= ~(GPIO_MODER_MODER0 << (position * 2U));
temp |= ((mode & GPIO_MODE) << (position * 2U));
port_->MODER = temp;
/* In case of Output or Alternate function mode selection */
if((mode == GPIO_MODE_OUTPUT_PP) || (mode == GPIO_MODE_AF_PP) ||
(mode == GPIO_MODE_OUTPUT_OD) || (mode == GPIO_MODE_AF_OD))
{
/* Check the Speed parameter */
assert_param(IS_GPIO_SPEED(speed));
/* Configure the IO Speed */
temp = port_->OSPEEDR;
temp &= ~(GPIO_OSPEEDER_OSPEEDR0 << (position * 2U));
temp |= (speed << (position * 2U));
port_->OSPEEDR = temp;
/* Configure the IO Output Type */
temp = port_->OTYPER;
temp &= ~(GPIO_OTYPER_OT_0 << position) ;
temp |= (((mode & GPIO_OUTPUT_TYPE) >> 4U) << position);
port_->OTYPER = temp;
}
/* Activate the Pull-up or Pull down resistor for the current IO */
temp = port_->PUPDR;
temp &= ~(GPIO_PUPDR_PUPDR0 << (position * 2U));
temp |= ((pull) << (position * 2U));
port_->PUPDR = temp;
return true;
}
bool Stm32Gpio::subscribe(bool rising_edge, bool falling_edge, void (*callback)(void*), void* ctx) {
uint32_t pin_number = get_pin_number();
if (pin_number >= N_EXTI) {
return false; // invalid pin number
}
struct subscription_t& subscription = subscriptions[pin_number];
GPIO_TypeDef* no_port = nullptr;
if (!__atomic_compare_exchange_n(&subscription.port, &no_port, port_, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)) {
return false; // already in use
}
// The following code is mostly taken from HAL_GPIO_Init
__HAL_RCC_SYSCFG_CLK_ENABLE();
uint32_t temp = SYSCFG->EXTICR[pin_number >> 2U];
temp &= ~(0x0FU << (4U * (pin_number & 0x03U)));
temp |= ((uint32_t)(GPIO_GET_INDEX(port_)) << (4U * (pin_number & 0x03U)));
SYSCFG->EXTICR[pin_number >> 2U] = temp;
if (rising_edge) {
EXTI->RTSR |= (uint32_t)pin_mask_;
} else {
EXTI->RTSR &= ~((uint32_t)pin_mask_);
}
if (falling_edge) {
EXTI->FTSR |= (uint32_t)pin_mask_;
} else {
EXTI->FTSR &= ~((uint32_t)pin_mask_);
}
EXTI->EMR &= ~((uint32_t)pin_mask_);
EXTI->IMR |= (uint32_t)pin_mask_;
// Clear any previous triggers
__HAL_GPIO_EXTI_CLEAR_IT(pin_mask_);
subscription.ctx = ctx;
subscription.callback = callback;
return true;
}
void Stm32Gpio::unsubscribe() {
uint32_t pin_number = get_pin_number();
if (pin_number >= N_EXTI) {
return; // invalid pin number
}
struct subscription_t& subscription = subscriptions[pin_number];
if (subscription.port != port_) {
return; // the subscription was not for this GPIO
}
EXTI->IMR |= (uint32_t)pin_mask_;
__HAL_GPIO_EXTI_CLEAR_IT(pin_mask_);
// At this point no more interrupts will be triggered for this GPIO
subscription.callback = nullptr;
subscription.ctx = nullptr;
subscription.port = nullptr; // after this line, the subscription can be reused (possibly by another thread)
}
void maybe_handle(uint16_t exti_number) {
if(__HAL_GPIO_EXTI_GET_IT(1 << exti_number) == RESET) {
return; // This interrupt source did not trigger the interrupt line
}
__HAL_GPIO_EXTI_CLEAR_IT(1 << exti_number);
if (exti_number >= N_EXTI) {
return;
}
subscription_t& subscription = subscriptions[exti_number];
if (subscription.callback) {
(*subscription.callback)(subscription.ctx);
}
}
extern "C" {
/** @brief Entrypoint for the EXTI line 0 interrupt. */
void EXTI0_IRQHandler(void) {
maybe_handle(0);
}
/** @brief Entrypoint for the EXTI line 1 interrupt. */
void EXTI1_IRQHandler(void) {
maybe_handle(1);
}
/** @brief Entrypoint for the EXTI line 2 interrupt. */
void EXTI2_IRQHandler(void) {
maybe_handle(2);
}
/** @brief Entrypoint for the EXTI line 3 interrupt. */
void EXTI3_IRQHandler(void) {
maybe_handle(3);
}
/** @brief Entrypoint for the EXTI line 4 interrupt. */
void EXTI4_IRQHandler(void) {
maybe_handle(4);
}
/** @brief Entrypoint for the EXTI lines 5-9 interrupt. */
void EXTI9_5_IRQHandler(void) {
maybe_handle(5);
maybe_handle(6);
maybe_handle(7);
maybe_handle(8);
maybe_handle(9);
}
/** @brief This function handles EXTI lines 10-15 interrupt. */
void EXTI15_10_IRQHandler(void) {
maybe_handle(10);
maybe_handle(11);
maybe_handle(12);
maybe_handle(13);
maybe_handle(14);
maybe_handle(15);
}
}
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_gpio.hpp
+82
View File
@@ -0,0 +1,82 @@
#ifndef __STM32_GPIO_HPP
#define __STM32_GPIO_HPP
#include <gpio.h>
class Stm32Gpio {
public:
static const Stm32Gpio none;
Stm32Gpio() : port_(nullptr), pin_mask_(0) {}
Stm32Gpio(GPIO_TypeDef* port, uint16_t pin) : port_(port), pin_mask_(pin) {}
operator bool() const { return port_ && pin_mask_; }
/**
* @brief Configures the GPIO with the specified parameters.
*
* This can be done regardless of the current state of the GPIO.
*
* If any subscription is in place, it is not disabled by this function.
*/
bool config(uint32_t mode, uint32_t pull, uint32_t speed = GPIO_SPEED_FREQ_LOW);
void write(bool state) {
if (port_) {
HAL_GPIO_WritePin(port_, pin_mask_, state ? GPIO_PIN_SET : GPIO_PIN_RESET);
}
}
bool read() {
return port_ && (port_->IDR & pin_mask_);
}
/**
* @brief Subscribes to external interrupts on the specified GPIO.
*
* Before calling this function the gpio should most likely be configured as
* input (however this is not mandatory, the interrupt works in output mode
* too).
* Also you need to enable the EXTIx_IRQn interrupt vectors in the NVIC,
* otherwise the subscription won't have any effect.
*
* Only one subscription is allowed per pin number. I.e. it is not possible
* to set up a subscription for both PA0 and PB0 at the same time.
*
* This function is thread-safe with respect to all other public functions
* of this class.
*
* Returns true if the subscription was set up successfully or false otherwise.
*/
bool subscribe(bool rising_edge, bool falling_edge, void (*callback)(void*), void* ctx);
/**
* @brief Unsubscribes from external interrupt on the specified GPIO.
*
* If no subscription was active for this GPIO, calling this function has no
* effect.
*
* This function is thread-safe with respect to all other public functions
* of this class, however it must not be called from an interrupt routine
* running at a higher priority than the interrupt that is being unsubscribed.
*
* After this function returns the callback given to subscribe() will no
* longer be invoked.
*/
void unsubscribe();
uint16_t get_pin_number() {
uint16_t pin_number = 0;
uint16_t pin_mask = pin_mask_ >> 1;
while (pin_mask) {
pin_mask >>= 1;
pin_number++;
}
return pin_number;
}
GPIO_TypeDef* port_;
uint16_t pin_mask_; // TODO: store pin_number_ instead of pin_mask_
};
#endif // __STM32_GPIO_HPP
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_nvm.c
+499
View File
@@ -0,0 +1,499 @@
/*
* Flash-based Non-Volatile Memory (NVM)
*
* This file supports storing and loading persistent configuration based on
* the STM32 builtin flash memory.
*
* The STM32F405xx has 12 flash sectors of heterogeneous size. We use the last
* two sectors for configuration data. These pages have a size of 128kB each.
* Setting any bit in these sectors to 0 is always possible, but setting them
* to 1 requires erasing the whole sector.
*
* We consider each sector as an array of 64-bit fields except the first N bytes, which we
* instead use as an allocation block. The allocation block is a compact bit-field (2 bit per entry)
* that keeps track of the state of each field (erased, invalid, valid).
*
* One sector is always considered the valid (read) sector and the other one is the
* target for the next write access: they can be considered to be ping-pong or double buffred.
*
* When writing a block of data, instead of always erasing the whole writable sector the
* new data is appended in the erased area. This presumably increases flash life span.
* The writable sector is only erased if there is not enough space for the new data.
*
* On startup, if there is exactly one sector
* whose last non-erased value has the state "valid" that sector is considered
* the valid sector. In any other case the selection is undefined.
*
*
* To write a new block of data atomically we first mark all associated fields
* as "invalid" (in the allocation table) then write the data and then mark the
* fields as "valid" (in the direction of increasing address).
*/
#include "stm32_nvm.h"
#include <string.h>
#if defined(STM32F405xx)
#include <stm32f405xx.h>
#include <stm32f4xx_hal.h>
// refer to page 75 of datasheet:
// http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
#define FLASH_SECTOR_A FLASH_SECTOR_10
#define FLASH_SECTOR_A_BASE (const volatile uint8_t*)0x80C0000UL
#define FLASH_SECTOR_A_SIZE 0x20000UL
#define FLASH_SECTOR_B FLASH_SECTOR_11
#define FLASH_SECTOR_B_BASE (const volatile uint8_t*)0x80E0000UL
#define FLASH_SECTOR_B_SIZE 0x20000UL
#elif defined(STM32F722xx)
#include <stm32f722xx.h>
#include <stm32f7xx_hal.h>
// refer to page 68 of datasheet:
// https://www.st.com/resource/en/reference_manual/dm00305990-stm32f72xxx-and-stm32f73xxx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
#define FLASH_SECTOR_A FLASH_SECTOR_1
#define FLASH_SECTOR_A_BASE (const volatile uint8_t*)0x8004000UL
#define FLASH_SECTOR_A_SIZE 0x4000UL
#define FLASH_SECTOR_B FLASH_SECTOR_2
#define FLASH_SECTOR_B_BASE (const volatile uint8_t*)0x8008000UL
#define FLASH_SECTOR_B_SIZE 0x4000UL
#else
#error "unknown flash sector size"
#endif
typedef enum {
VALID = 0,
INVALID = 1,
ERASED = 3
} field_state_t;
typedef struct {
size_t index; //!< next field to be written to (can be equal to n_data)
const uint32_t sector_id; //!< HAL ID of this sector
const size_t n_data; //!< number of 64-bit fields in this sector
const size_t n_reserved; //!< number of 64-bit fields in this sector that are reserved for the allocation table
const volatile uint8_t* const alloc_table;
const volatile uint64_t* const data;
} sector_t;
sector_t sectors[] = { {
.sector_id = FLASH_SECTOR_A,
.n_data = FLASH_SECTOR_A_SIZE >> 3,
.n_reserved = (FLASH_SECTOR_A_SIZE >> 3) >> 5,
.alloc_table = FLASH_SECTOR_A_BASE,
.data = (uint64_t *)FLASH_SECTOR_A_BASE
}, {
.sector_id = FLASH_SECTOR_B,
.n_data = FLASH_SECTOR_B_SIZE >> 3,
.n_reserved = (FLASH_SECTOR_B_SIZE >> 3) >> 5,
.alloc_table = FLASH_SECTOR_B_BASE,
.data = (uint64_t *)FLASH_SECTOR_B_BASE
}};
uint8_t read_sector_; // 0 or 1 to indicate which sector to read from and which to write to
size_t n_staging_area_; // number of 64-bit values that were reserved using NVM_start_write
size_t n_valid_; // number of 64-bit fields that can be read
static const uint32_t FLASH_ERR_FLAGS =
#if defined(FLASH_FLAG_EOP)
FLASH_FLAG_EOP |
#endif
#if defined(FLASH_FLAG_OPERR)
FLASH_FLAG_OPERR |
#endif
#if defined(FLASH_FLAG_WRPERR)
FLASH_FLAG_WRPERR |
#endif
#if defined(FLASH_FLAG_PGAERR)
FLASH_FLAG_PGAERR |
#endif
#if defined(FLASH_FLAG_PGSERR)
FLASH_FLAG_PGSERR |
#endif
#if defined(FLASH_FLAG_PGPERR)
FLASH_FLAG_PGPERR |
#endif
0;
static void HAL_FLASH_ClearError() {
__HAL_FLASH_CLEAR_FLAG(FLASH_ERR_FLAGS);
}
// @brief Erases a flash sector. This sets all bits in the sector to 1.
// The sector's current index is reset to the minimum value (n_reserved).
// @returns 0 on success or a non-zero error code otherwise
int erase(sector_t *sector) {
FLASH_EraseInitTypeDef erase_struct = {
.TypeErase = FLASH_TYPEERASE_SECTORS,
#if defined(FLASH_OPTCR_nDBANK)
.Banks = 0, // only used for mass erase
#endif
.Sector = sector->sector_id,
.NbSectors = 1,
.VoltageRange = FLASH_VOLTAGE_RANGE_3
};
HAL_FLASH_Unlock();
HAL_FLASH_ClearError();
uint32_t sector_error;
if (HAL_FLASHEx_Erase(&erase_struct, &sector_error) != HAL_OK)
goto fail;
sector->index = sector->n_reserved;
HAL_FLASH_Lock();
return 0;
fail:
HAL_FLASH_Lock();
//printf("erase failed: %u \r\n", HAL_FLASH_GetError());
return HAL_FLASH_GetError(); // non-zero
}
// @brief Writes states into the allocation table.
// The write operation goes in the direction of increasing indices.
// @param state: 11: erased, 10: writing, 00: valid data
// @returns 0 on success or a non-zero error code otherwise
int set_allocation_state(sector_t *sector, size_t index, size_t count, field_state_t state) {
if (index < sector->n_reserved)
return -1;
if (index + count >= sector->n_data)
return -1;
// expand state to state for 4 values
const uint8_t states = (state << 0) | (state << 2) | (state << 4) | (state << 6);
// handle unaligned start
uint8_t mask = ~(0xff << ((index & 0x3) << 1));
count += index & 0x3;
index -= index & 0x3;
HAL_FLASH_Unlock();
HAL_FLASH_ClearError();
// write states
for (; count >= 4; count -= 4, index += 4) {
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, (uintptr_t)&sector->alloc_table[index >> 2], states | mask) != HAL_OK)
goto fail;
mask = 0;
}
// handle unaligned end
if (count) {
mask |= ~(0xff >> ((4 - count) << 1));
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, (uintptr_t)&sector->alloc_table[index >> 2], states | mask) != HAL_OK)
goto fail;
}
HAL_FLASH_Lock();
return 0;
fail:
HAL_FLASH_Lock();
return HAL_FLASH_GetError(); // non-zero
}
// @brief Reads the allocation table from behind to determine how many fields match the
// reference state.
// @param sector: The sector on which to perform the search
// @param max_index: The maximum index that should be considered
// @param ref_state: The reference state
// @param state: Set to the first encountered state that is unequal to ref_state.
// Set to ref_state if all encountered states are equal to ref_state.
// @returns The smallest index that points to a field with ref_state.
// This value is at least sector->n_reserved and at most max_index.
size_t scan_allocation_table(sector_t *sector, size_t max_index, field_state_t ref_state, field_state_t *state) {
const uint8_t ref_states = (ref_state << 0) | (ref_state << 2) | (ref_state << 4) | (ref_state << 6);
size_t index = (((max_index + 3) >> 2) << 2); // start at the max index but round up to a multiple of 4
size_t ignore = index - max_index;
uint8_t states = ref_states;
//printf("scan from %08x to %08x for %02x\r\n", index, sector->n_reserved, ref_states); osDelay(5);
// read 4 states at a time
for (; index >= (sector->n_reserved + 4); index -= 4) {
states = sector->alloc_table[(index - 1) >> 2];
if (ignore) { // ignore the upper 1, 2 or 3 states if max_index was unaligned
uint8_t ignore_mask = ~(0xff >> (ignore << 1));
states = (states & ~ignore_mask) | (ref_states & ignore_mask);
ignore = 0;
}
if (states != ref_states)
break;
}
// once we encounterd a byte with any state mismatch determine which of the 4 states it is
for (; ((states >> 6) == (ref_states & 0x3)) && (index > sector->n_reserved); index--) {
states <<= 2;
}
*state = states >> 6;
//printf("(it's %02x)\r\n", index); osDelay(5);
return index;
}
// Loads the head of the NVM data.
// If this function fails subsequent calls to NVM functions (other than NVM_init or NVM_erase)
// cause undefined behavior.
// @returns 0 on success or a non-zero error code otherwise
int NVM_init(void) {
field_state_t sector0_state, sector1_state;
sectors[0].index = scan_allocation_table(&sectors[0], sectors[0].n_data,
ERASED, &sector0_state);
sectors[1].index = scan_allocation_table(&sectors[1], sectors[1].n_data,
ERASED, &sector1_state);
//printf("sector states: %02x, %02x\r\n", sector0_state, sector1_state); osDelay(5);
// Select valid sector on a best effort basis
// (in unfortunate cases valid_sector might actually point
// to an invalid or erased sector)
read_sector_ = 0;
if (sector1_state == VALID)
read_sector_ = 1;
// count the number of valid fields
sector_t *read_sector = &sectors[read_sector_];
uint8_t first_nonvalid_state;
size_t min_valid_index = scan_allocation_table(read_sector, read_sector->index,
VALID, &first_nonvalid_state);
n_valid_ = read_sector->index - min_valid_index;
n_staging_area_ = 0;
int status = 0;
/*// bring non-valid sectors into a known state
this is not absolutely required
if (sector0_state != VALID)
status |= erase(&sectors[0]);
if (sector1_state != VALID)
status |= erase(&sectors[1]);
*/
return status;
}
// @brief Erases all data in the NVM.
//
// If this function fails subsequent calls to NVM functions (other than NVM_init or NVM_erase)
// cause undefined behavior.
// Caution: this function may take a long time (like 1 second)
//
// @returns 0 on success or a non-zero error code otherwise
int NVM_erase(void) {
read_sector_ = 0;
sectors[0].index = sectors[0].n_reserved;
sectors[1].index = sectors[1].n_reserved;
int state = 0;
state |= erase(&sectors[0]);
state |= erase(&sectors[1]);
return state;
}
// @brief Returns the maximum number of bytes that can be read using NVM_read.
// This holds until NVM_commit is called.
size_t NVM_get_max_read_length(void) {
return n_valid_ << 3;
}
// @brief Returns the maximum length (in bytes) that can passed to NVM_start_write.
// This holds until NVM_commit is called.
size_t NVM_get_max_write_length(void) {
sector_t *target = &sectors[1 - read_sector_];
return (target->n_data - target->n_reserved) << 3;
}
// @brief Reads from the latest committed block in the non-volatile memory.
// The function either succeeds or leaves the provided buffer unmodified.
// @param offset: offset in bytes (0 meaning the beginning of the valid area)
// @param data: buffer to write to
// @param length: length in bytes (if (offset + length) is out of range, the function fails)
// @returns 0 on success or a non-zero error code otherwise
int NVM_read(size_t offset, uint8_t *data, size_t length) {
if (offset + length > (n_valid_ << 3))
return -1;
sector_t *read_sector = &sectors[read_sector_];
const uint8_t *src_ptr = ((const uint8_t *)&read_sector->data[read_sector->index - n_valid_]) + offset;
memcpy(data, src_ptr, length);
return 0;
}
// @brief Starts an atomic write operation.
//
// The most recent valid NVM data is not modified or invalidated until NVM_commit is called.
// The length must be at most equal to the size indicated by NVM_get_max_write_length().
//
// @param length: Length of the staging block that should be created
int NVM_start_write(size_t length) {
int status = 0;
sector_t *target = &sectors[1 - read_sector_];
length = (length + 7) >> 3; // round to multiple of 64 bit
if (length > target->n_data - target->n_reserved)
return -1;
// make room for the new data
if (length > target->n_data - target->index)
if ((status = erase(target)))
return status;
// invalidate the fields we're about to write
status = set_allocation_state(target, target->index, length, INVALID);
if (status)
return status;
n_staging_area_ = length;
return 0;
}
// @brief Writes to the current data block that was opened with NVM_start_write.
//
// The operation fails if (offset + length) is larger than the length passed to NVM_start_write.
// The most recent valid NVM data is not modified or invalidated until NVM_commit is called.
// Warning: Writing different data to the same area multiple times during a single transaction
// will cause data corruption.
//
// @param offset: The offset in bytes, 0 being the beginning of the staging block.
// @param data: Pointer to the data that should be written
// @param length: Data length in bytes
int NVM_write(size_t offset, uint8_t *data, size_t length) {
if (offset + length > (n_staging_area_ << 3))
return -1;
sector_t *target = &sectors[1 - read_sector_];
HAL_FLASH_Unlock();
HAL_FLASH_ClearError();
// handle unaligned start
for (; (offset & 0x3) && length; ++data, ++offset, --length)
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE,
((uintptr_t)&target->data[target->index]) + offset, *data) != HAL_OK)
goto fail;
// write 32-bit values (64-bit doesn't work)
for (; length >= 4; data += 4, offset += 4, length -=4)
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
((uintptr_t)&target->data[target->index]) + offset, *(uint32_t*)data) != HAL_OK)
goto fail;
// handle unaligned end
for (; length; ++data, ++offset, --length)
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE,
((uintptr_t)&target->data[target->index]) + offset, *data) != HAL_OK)
goto fail;
HAL_FLASH_Lock();
return 0;
fail:
HAL_FLASH_Lock();
return HAL_FLASH_GetError(); // non-zero
}
// @brief Commits the new data to NVM atomically.
int NVM_commit(void) {
sector_t *read_sector = &sectors[read_sector_];
sector_t *write_sector = &sectors[1 - read_sector_];
// mark the newly-written fields as valid
int status = set_allocation_state(write_sector, write_sector->index, n_staging_area_, VALID);
if (status)
return status;
write_sector->index += n_staging_area_;
n_valid_ = n_staging_area_;
n_staging_area_ = 0;
read_sector_ = 1 - read_sector_;
// invalidate the other sector
if (read_sector->index < read_sector->n_data) {
status = set_allocation_state(read_sector, read_sector->index, 1, INVALID);
read_sector->index += 1;
} else {
status = erase(read_sector);
}
return status;
}
#include <cmsis_os.h>
#include <stdio.h>
/** @brief Call this at startup to test/demo the NVM driver
Expected output when starting with a fully erased NVM
[1st boot]
=== NVM TEST ===
NVM is empty
write 0x00, ..., 0x25 to NVM
new data committed to NVM
[2nd boot]
=== NVM TEST ===
NVM contains 40 valid bytes:
00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f
10 11 12 13 14 15 16 17 18 19 1a 1b 1c 1d 1e 1f
20 21 22 23 24 25 ff ff
write 0xbd, ..., 0xe2 to NVM
new data committed to NVM
[3rd boot]
=== NVM TEST ===
NVM contains 40 valid bytes:
bd be bf c0 c1 c2 c3 c4 c5 c6 c7 c8 c9 ca cb cc
cd ce cf d0 d1 d2 d3 d4 d5 d6 d7 d8 d9 da db dc
dd de df e0 e1 e2 ff ff
write 0xcb, ..., 0xf0 to NVM
new data committed to NVM
*/
void NVM_demo(void) {
const size_t len = 38;
uint8_t data[len];
int progress = 0;
uint8_t seed = 0;
osDelay(100);
printf("=== NVM TEST ===\r\n"); osDelay(5);
//NVM_erase();
if (progress++, NVM_init() != 0)
goto fail;
// load bytes from NVM and print them
size_t available = NVM_get_max_read_length();
if (available) {
printf("NVM contains %d valid bytes:\r\n", available); osDelay(5);
uint8_t buf[available];
if (progress++, NVM_read(0, buf, available) != 0)
goto fail;
for (size_t pos = 0; pos < available; ++pos) {
seed += buf[pos];
printf(" %02x", buf[pos]);
if ((((pos + 1) % 16) == 0) || ((pos + 1) == available))
printf("\r\n");
osDelay(2);
}
} else {
printf("NVM is empty\r\n"); osDelay(5);
}
// store new bytes in NVM (data based on seed)
printf("write 0x%02x, ..., 0x%02x to NVM\r\n", seed, seed + len - 1); osDelay(5);
for (size_t i = 0; i < len; i++)
data[i] = seed++;
if (progress++, NVM_start_write(len) != 0)
goto fail;
if (progress++, NVM_write(0, data, len / 2))
goto fail;
if (progress++, NVM_write(len / 2, &data[len / 2], len - (len / 2)))
goto fail;
if (progress++, NVM_commit())
goto fail;
printf("new data committed to NVM\r\n"); osDelay(5);
return;
fail:
printf("NVM test failed at %d!\r\n", progress);
}
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_nvm.h
+33
View File
@@ -0,0 +1,33 @@
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __NVM_H
#define __NVM_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <stdint.h>
#include <stdlib.h>
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
int NVM_init(void);
int NVM_erase(void);
size_t NVM_get_max_read_length(void);
size_t NVM_get_max_write_length(void);
int NVM_read(size_t offset, uint8_t *data, size_t length);
int NVM_start_write(size_t length);
int NVM_write(size_t offset, uint8_t *data, size_t length);
int NVM_commit(void);
void NVM_demo(void);
#ifdef __cplusplus
}
#endif
#endif //__NVM_H
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp
+129
View File
@@ -0,0 +1,129 @@
#include "stm32_spi_arbiter.hpp"
#include "stm32_system.h"
#include "utils.hpp"
#include <cmsis_os.h>
bool equals(const SPI_InitTypeDef& lhs, const SPI_InitTypeDef& rhs) {
return (lhs.Mode == rhs.Mode)
&& (lhs.Direction == rhs.Direction)
&& (lhs.DataSize == rhs.DataSize)
&& (lhs.CLKPolarity == rhs.CLKPolarity)
&& (lhs.CLKPhase == rhs.CLKPhase)
&& (lhs.NSS == rhs.NSS)
&& (lhs.BaudRatePrescaler == rhs.BaudRatePrescaler)
&& (lhs.FirstBit == rhs.FirstBit)
&& (lhs.TIMode == rhs.TIMode)
&& (lhs.CRCCalculation == rhs.CRCCalculation)
&& (lhs.CRCPolynomial == rhs.CRCPolynomial);
}
bool Stm32SpiArbiter::acquire_task(SpiTask* task) {
return !__atomic_exchange_n(&task->is_in_use, true, __ATOMIC_SEQ_CST);
}
void Stm32SpiArbiter::release_task(SpiTask* task) {
task->is_in_use = false;
}
bool Stm32SpiArbiter::start() {
if (!task_list_) {
return false;
}
SpiTask& task = *task_list_;
if (!equals(task.config, hspi_->Init)) {
HAL_SPI_DeInit(hspi_);
hspi_->Init = task.config;
HAL_SPI_Init(hspi_);
__HAL_SPI_ENABLE(hspi_);
}
task.ncs_gpio.write(false);
HAL_StatusTypeDef status = HAL_ERROR;
if (hspi_->hdmatx->State != HAL_DMA_STATE_READY || hspi_->hdmarx->State != HAL_DMA_STATE_READY) {
// This can happen if the DMA or interrupt priorities are not configured properly.
status = HAL_BUSY;
} else if (task.tx_buf && task.rx_buf) {
status = HAL_SPI_TransmitReceive_DMA(hspi_, (uint8_t*)task.tx_buf, task.rx_buf, task.length);
} else if (task.tx_buf) {
status = HAL_SPI_Transmit_DMA(hspi_, (uint8_t*)task.tx_buf, task.length);
} else if (task.rx_buf) {
status = HAL_SPI_Receive_DMA(hspi_, task.rx_buf, task.length);
}
if (status != HAL_OK) {
task.ncs_gpio.write(true);
}
return status == HAL_OK;
}
void Stm32SpiArbiter::transfer_async(SpiTask* task) {
task->next = nullptr;
// Append new task to task list.
// We could try to do this lock free but we could also use our time for useful things.
SpiTask** ptr = &task_list_;
CRITICAL_SECTION() {
while (*ptr)
ptr = &(*ptr)->next;
*ptr = task;
}
// If the list was empty before, kick off the SPI arbiter now
if (ptr == &task_list_) {
if (!start()) {
if (task->on_complete) {
(*task->on_complete)(task->on_complete_ctx, false);
}
}
}
}
// TODO: this currently only works when called in a CMSIS thread.
bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const uint8_t* tx_buf, uint8_t* rx_buf, size_t length, uint32_t timeout_ms) {
volatile uint8_t result = 0xff;
SpiTask task = {
.config = config,
.ncs_gpio = ncs_gpio,
.tx_buf = tx_buf,
.rx_buf = rx_buf,
.length = length,
.on_complete = [](void* ctx, bool success) { *(volatile uint8_t*)ctx = success ? 1 : 0; },
.on_complete_ctx = (void*)&result,
.is_in_use = false,
.next = nullptr
};
transfer_async(&task);
while (result == 0xff) {
osDelay(1); // TODO: honor timeout
}
return result;
}
void Stm32SpiArbiter::on_complete() {
if (!task_list_) {
return; // this should not happen
}
// Wrap up transfer
task_list_->ncs_gpio.write(true);
if (task_list_->on_complete) {
(*task_list_->on_complete)(task_list_->on_complete_ctx, true);
}
// Start next task if any
SpiTask* next = nullptr;
CRITICAL_SECTION() {
next = task_list_ = task_list_->next;
}
if (next) {
start();
}
}
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp
+94
View File
@@ -0,0 +1,94 @@
#ifndef __STM32_SPI_ARBITER_HPP
#define __STM32_SPI_ARBITER_HPP
#include "stm32_gpio.hpp"
#include <spi.h>
class Stm32SpiArbiter {
public:
struct SpiTask {
SPI_InitTypeDef config;
Stm32Gpio ncs_gpio;
const uint8_t* tx_buf;
uint8_t* rx_buf;
size_t length;
void (*on_complete)(void*, bool);
void* on_complete_ctx;
bool is_in_use = false;
struct SpiTask* next;
};
Stm32SpiArbiter(SPI_HandleTypeDef* hspi): hspi_(hspi) {}
/**
* Reserves the task for the caller if it's not in use currently.
*
* This can be used by the caller to ensure that the task structure is not
* overwritten while it's in use in a preceding transfer.
*
* Example:
*
* if (acquire_task(&task)) {
* transfer_async(&task)
* }
*
* A call to release_task() makes the task available for use again.
*/
static bool acquire_task(SpiTask* task);
/**
* Releases the task so that the next call to `acquire_task()` returns true.
* This should usually be called inside the on_complete() callback after
* the rx buffer has been processed.
*/
static void release_task(SpiTask* task);
/**
* @brief Enqueues a non-blocking transfer.
*
* Once the transfer completes, fails or is aborted, the callback is invoked.
*
* This function is thread-safe with respect to all other public functions
* of this class.
*
* @param task: Contains all configuration data for this transfer.
* The struct pointed to by this argument must remain valid and
* unmodified until the completion callback is invoked.
*/
void transfer_async(SpiTask* task);
/**
* @brief Executes a blocking transfer.
*
* If the SPI is busy this function waits until it becomes available or
* the specified timeout passes, whichever comes first.
*
* Returns true on successful transfer or false otherwise.
*
* This function is thread-safe with respect to all other public functions
* of this class.
*
* @param config: The SPI configuration to apply for this transfer.
* @param ncs_gpio: The active low GPIO to actuate during this transfer.
* @param tx_buf: Buffer for the outgoing data to be sent. Can be null unless
* rx_buf is null too.
* @param rx_buf: Buffer for the incoming data to be sent. Can be null unless
* tx_buf is null too.
*/
bool transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const uint8_t* tx_buf, uint8_t* rx_buf, size_t length, uint32_t timeout_ms);
/**
* @brief Completion method to be called from HAL_SPI_TxCpltCallback,
* HAL_SPI_RxCpltCallback and HAL_SPI_TxRxCpltCallback.
*/
void on_complete();
private:
bool start();
SPI_HandleTypeDef* hspi_;
SpiTask* task_list_ = nullptr;
};
#endif // __STM32_SPI_ARBITER_HPP
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_system.cpp
+4
View File
@@ -0,0 +1,4 @@
#include "stm32_system.h"
uint32_t irq_counters[254]; // 14 core interrupts, 240 NVIC interrupts
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_system.h
+70
View File
@@ -0,0 +1,70 @@
#ifndef __STM32_SYSTEM_H
#define __STM32_SYSTEM_H
#if defined(STM32F405xx)
#include <stm32f405xx.h>
#elif defined(STM32F722xx)
#include <stm32f722xx.h>
#else
#error "unknown STM32 microcontroller"
#endif
// C/C++ definitions
#ifdef __cplusplus
extern "C" {
#endif
// Uncomment the following line to sacrifice 1kB of RAM for the ability to
// monitor the number of times each interrupt fires.
//#define ENABLE_IRQ_COUNTER
#ifdef ENABLE_IRQ_COUNTER
extern uint32_t irq_counters[];
#define COUNT_IRQ(irqn) (++irq_counters[irqn + 14])
#define GET_IRQ_COUNTER(irqn) irq_counters[irqn + 14]
#else
#define COUNT_IRQ(irqn) ((void)0)
#define GET_IRQ_COUNTER(irqn) 0
#endif
static inline uint32_t cpu_enter_critical() {
uint32_t primask = __get_PRIMASK();
__disable_irq();
return primask;
}
static inline void cpu_exit_critical(uint32_t priority_mask) {
__set_PRIMASK(priority_mask);
}
#ifdef __cplusplus
}
#endif
// C++ only definitions
#ifdef __cplusplus
struct CriticalSectionContext {
CriticalSectionContext(const CriticalSectionContext&) = delete;
CriticalSectionContext(const CriticalSectionContext&&) = delete;
void operator=(const CriticalSectionContext&) = delete;
void operator=(const CriticalSectionContext&&) = delete;
operator bool() { return true; };
CriticalSectionContext() : mask_(cpu_enter_critical()) {}
~CriticalSectionContext() { cpu_exit_critical(mask_); }
uint32_t mask_;
bool exit_ = false;
};
#ifdef __clang__
#define CRITICAL_SECTION() for (CriticalSectionContext __critical_section_context; !__critical_section_context.exit_; __critical_section_context.exit_ = true)
#else
#define CRITICAL_SECTION() if (CriticalSectionContext __critical_section_context{})
#endif
#endif
#endif // __STM32_SYSTEM_H
ODrive-fw-v0.5.6/Firmware/Drivers/STM32/stm32_timer.hpp
+83
View File
@@ -0,0 +1,83 @@
#ifndef __STM32_TIMER_HPP
#define __STM32_TIMER_HPP
#include "stm32_system.h"
#include <tim.h>
#include <array>
class Stm32Timer {
public:
/**
* @brief Starts multiple timers deterministically and synchronously from the
* specified offset.
*
* All timers are atomically (*) put into the following state (regardless of
* their previous state/configuration):
* - TIMx_CNT will be initialized according to the corresponding counter[i] parameter.
* - If the timer is in center-aligned mode, it will be set to up-counting direction.
* - The update repetition counter is reset to TIMx_RCR (if applicable).
* - The prescaler counter is reset.
* - Update interrupts are disabled.
* - The counter put into running state.
*
* This function is implemented by generating an update event on all selected timers.
* That means as a side effect all things that are connected to the update event
* except the interrupt routine itself (i.e. ADCs, DMAs, slave timers, etc) will
* be triggered.
*
* Also you probably want to disable any connected PWM outputs to prevent glitches.
*
* (*) Best-effort atomically. There will be skew of a handful of clock cycles
* but it's always the same given the compiler version and configuration.
*/
template<size_t I>
static void start_synchronously(std::array<TIM_HandleTypeDef*, I> timers, std::array<size_t, I> counters) {
start_synchronously_impl(timers, counters, std::make_index_sequence<I>());
}
private:
#pragma GCC push_options
#pragma GCC optimize (3)
template<size_t I, size_t ... Is>
static void start_synchronously_impl(std::array<TIM_HandleTypeDef*, I> timers, std::array<size_t, I> counters, std::index_sequence<Is...>) {
for (size_t i = 0; i < I; ++i) {
TIM_HandleTypeDef* htim = timers[i];
// Stop the timer so we can start all of them later more atomically.
htim->Instance->CR1 &= ~TIM_CR1_CEN;
// Generate update event to force all of the timer's registers into
// a known state.
__HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE);
htim->Instance->EGR |= TIM_EGR_UG;
__HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
// Load counter with the desired value.
htim->Instance->CNT = counters[i];
}
register volatile uint32_t* cr_addr[I];
register uint32_t cr_val[I];
for (size_t i = 0; i < I; ++i) {
cr_addr[i] = &timers[i]->Instance->CR1;
cr_val[i] = timers[i]->Instance->CR1 | TIM_CR1_CEN;
}
// Restart all timers as atomically as possible.
// By inspection we find that this is compiled to the following code:
// f7ff faa0 bl 800bdd0 <cpu_enter_critical()>
// f8c9 6000 str.w r6, [r9]
// f8c8 5000 str.w r5, [r8]
// 603c str r4, [r7, #0]
// f7ff fa9d bl 800bdd8 <cpu_exit_critical(unsigned long)>
uint32_t mask = cpu_enter_critical();
int dummy[I] = {(*cr_addr[Is] = cr_val[Is], 0)...};
(void)dummy;
cpu_exit_critical(mask);
}
#pragma GCC pop_options
};
#endif // __STM32_TIMER_HPP