diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index 1b77fe3367..8b42565cdd 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -102,7 +102,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBCDC_DIR)/Inc \ $(CMSIS_DIR)/Include \ $(CMSIS_DIR)/Device/ST/STM32F4xx/Include \ - $(ROOT)/src/main/vcp_hal + $(ROOT)/src/main/drivers/stm32/vcp_hal else CMSIS_SRC := $(notdir $(wildcard $(CMSIS_DIR)/CoreSupport/*.c \ $(ROOT)/lib/main/STM32F4/Drivers/CMSIS/Device/ST/STM32F4xx/*.c)) @@ -116,7 +116,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBMSC_DIR)/inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32F4/Drivers/CMSIS/Device/ST/STM32F4xx \ - $(ROOT)/src/main/vcpf4 + $(ROOT)/src/main/drivers/stm32/vcpf4 endif #Flags @@ -146,42 +146,48 @@ endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DSTM32 MCU_COMMON_SRC = \ - startup/system_stm32f4xx.c \ drivers/accgyro/accgyro_mpu.c \ + drivers/dshot_bitbang_decode.c \ + drivers/inverter.c \ + drivers/pwm_output_dshot_shared.c \ + drivers/stm32/pwm_output_dshot.c \ drivers/stm32/adc_stm32f4xx.c \ drivers/stm32/bus_i2c_stm32f4xx.c \ drivers/stm32/bus_spi_stdperiph.c \ drivers/stm32/dma_stm32f4xx.c \ - drivers/dshot_bitbang.c \ - drivers/dshot_bitbang_decode.c \ + drivers/stm32/dshot_bitbang.c \ drivers/stm32/dshot_bitbang_stdperiph.c \ - drivers/inverter.c \ + drivers/stm32/exti.c \ + drivers/stm32/io_stm32.c \ drivers/stm32/light_ws2811strip_stdperiph.c \ - drivers/stm32/transponder_ir_io_stdperiph.c \ - drivers/pwm_output_dshot.c \ - drivers/pwm_output_dshot_shared.c \ + drivers/stm32/persistent.c \ + drivers/stm32/pwm_output.c \ + drivers/stm32/rcc_stm32.c \ + drivers/stm32/sdio_f4xx.c \ drivers/stm32/serial_uart_stdperiph.c \ drivers/stm32/serial_uart_stm32f4xx.c \ drivers/stm32/system_stm32f4xx.c \ + drivers/stm32/timer_stdperiph.c \ drivers/stm32/timer_stm32f4xx.c \ - drivers/persistent.c \ - drivers/stm32/sdio_f4xx.c + drivers/stm32/transponder_ir_io_stdperiph.c \ + startup/system_stm32f4xx.c ifeq ($(PERIPH_DRIVER), HAL) VCP_SRC = \ - vcp_hal/usbd_desc.c \ - vcp_hal/usbd_conf.c \ - vcp_hal/usbd_cdc_interface.c \ - drivers/serial_usb_vcp.c \ + drivers/stm32/vcp_hal/usbd_desc.c \ + drivers/stm32/vcp_hal/usbd_conf.c \ + drivers/stm32/vcp_hal/usbd_cdc_interface.c \ + drivers/stm32/serial_usb_vcp.c \ drivers/usb_io.c else VCP_SRC = \ - vcpf4/stm32f4xx_it.c \ - vcpf4/usb_bsp.c \ - vcpf4/usbd_desc.c \ - vcpf4/usbd_usr.c \ - vcpf4/usbd_cdc_vcp.c \ - drivers/serial_usb_vcp.c \ + drivers/stm32/vcpf4/stm32f4xx_it.c \ + drivers/stm32/vcpf4/usb_bsp.c \ + drivers/stm32/vcpf4/usbd_desc.c \ + drivers/stm32/vcpf4/usbd_usr.c \ + drivers/stm32/vcpf4/usbd_cdc_vcp.c \ + drivers/stm32/vcpf4/usb_cdc_hid.c \ + drivers/stm32/serial_usb_vcp.c \ drivers/usb_io.c endif diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index e4c09731cf..ee9268c38e 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -108,7 +108,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBMSC_DIR)/Inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32F7/Drivers/CMSIS/Device/ST/STM32F7xx/Include \ - $(ROOT)/src/main/vcp_hal + $(ROOT)/src/main/drivers/stm32/vcp_hal #Flags ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant @@ -147,41 +147,44 @@ endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DSTM32 VCP_SRC = \ - vcp_hal/usbd_desc.c \ - vcp_hal/usbd_conf_stm32f7xx.c \ - vcp_hal/usbd_cdc_hid.c \ - vcp_hal/usbd_cdc_interface.c \ - drivers/serial_usb_vcp.c \ + drivers/stm32/vcp_hal/usbd_desc.c \ + drivers/stm32/vcp_hal/usbd_conf_stm32f7xx.c \ + drivers/stm32/vcp_hal/usbd_cdc_hid.c \ + drivers/stm32/vcp_hal/usbd_cdc_interface.c \ + drivers/stm32/serial_usb_vcp.c \ drivers/usb_io.c MCU_COMMON_SRC = \ - startup/system_stm32f7xx.c \ drivers/accgyro/accgyro_mpu.c \ + drivers/bus_i2c_timing.c \ + drivers/dshot_bitbang_decode.c \ + drivers/pwm_output_dshot_shared.c \ drivers/stm32/adc_stm32f7xx.c \ drivers/stm32/audio_stm32f7xx.c \ - drivers/stm32/bus_i2c_hal.c \ drivers/stm32/bus_i2c_hal_init.c \ - drivers/bus_i2c_timing.c \ - drivers/stm32/dma_stm32f7xx.c \ - drivers/stm32/light_ws2811strip_hal.c \ - drivers/stm32/transponder_ir_io_hal.c \ + drivers/stm32/bus_i2c_hal.c \ drivers/stm32/bus_spi_ll.c \ - drivers/persistent.c \ - drivers/dshot_bitbang.c \ - drivers/dshot_bitbang_decode.c \ + drivers/stm32/dma_stm32f7xx.c \ drivers/stm32/dshot_bitbang_ll.c \ + drivers/stm32/dshot_bitbang.c \ + drivers/stm32/exti.c \ + drivers/stm32/io_stm32.c \ + drivers/stm32/light_ws2811strip_hal.c \ + drivers/stm32/persistent.c \ + drivers/stm32/pwm_output.c \ drivers/stm32/pwm_output_dshot_hal.c \ - drivers/pwm_output_dshot_shared.c \ - drivers/stm32/timer_hal.c \ - drivers/stm32/timer_stm32f7xx.c \ - drivers/stm32/system_stm32f7xx.c \ + drivers/stm32/rcc_stm32.c \ + drivers/stm32/sdio_f7xx.c \ drivers/stm32/serial_uart_hal.c \ drivers/stm32/serial_uart_stm32f7xx.c \ - drivers/stm32/sdio_f7xx.c + drivers/stm32/system_stm32f7xx.c \ + drivers/stm32/timer_hal.c \ + drivers/stm32/timer_stm32f7xx.c \ + drivers/stm32/transponder_ir_io_hal.c \ + startup/system_stm32f7xx.c MCU_EXCLUDES = \ - drivers/bus_i2c.c \ - drivers/timer.c + drivers/bus_i2c.c MSC_SRC = \ drivers/usb_msc_common.c \ diff --git a/make/mcu/STM32G4.mk b/make/mcu/STM32G4.mk index 6e3eee4690..5dd219ff7b 100644 --- a/make/mcu/STM32G4.mk +++ b/make/mcu/STM32G4.mk @@ -111,7 +111,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBMSC_DIR)/Inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32G4/Drivers/CMSIS/Device/ST/STM32G4xx/Include \ - $(ROOT)/src/main/vcp_hal + $(ROOT)/src/main/drivers/stm32/vcp_hal #Flags ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant @@ -134,41 +134,44 @@ endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DSTM32 VCP_SRC = \ - vcp_hal/usbd_desc.c \ - vcp_hal/usbd_conf_stm32g4xx.c \ - vcp_hal/usbd_cdc_hid.c \ - vcp_hal/usbd_cdc_interface.c \ - drivers/serial_usb_vcp.c \ + drivers/stm32/vcp_hal/usbd_desc.c \ + drivers/stm32/vcp_hal/usbd_conf_stm32g4xx.c \ + drivers/stm32/vcp_hal/usbd_cdc_hid.c \ + drivers/stm32/vcp_hal/usbd_cdc_interface.c \ + drivers/stm32/serial_usb_vcp.c \ drivers/usb_io.c MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ - drivers/stm32/adc_stm32g4xx.c \ - drivers/stm32/bus_i2c_hal.c \ - drivers/stm32/bus_i2c_hal_init.c \ drivers/bus_i2c_timing.c \ + drivers/dshot_bitbang_decode.c \ + drivers/pwm_output_dshot_shared.c \ + drivers/stm32/adc_stm32g4xx.c \ + drivers/stm32/bus_i2c_hal_init.c \ + drivers/stm32/bus_i2c_hal.c \ drivers/stm32/bus_spi_ll.c \ drivers/stm32/dma_stm32g4xx.c \ - drivers/dshot_bitbang.c \ - drivers/dshot_bitbang_decode.c \ drivers/stm32/dshot_bitbang_ll.c \ + drivers/stm32/dshot_bitbang.c \ + drivers/stm32/exti.c \ + drivers/stm32/io_stm32.c \ drivers/stm32/light_ws2811strip_hal.c \ drivers/stm32/memprot_hal.c \ drivers/stm32/memprot_stm32g4xx.c \ - drivers/persistent.c \ - drivers/pwm_output_dshot_shared.c \ + drivers/stm32/persistent.c \ + drivers/stm32/pwm_output.c \ drivers/stm32/pwm_output_dshot_hal.c \ + drivers/stm32/rcc_stm32.c \ + drivers/stm32/serial_uart_hal.c \ + drivers/stm32/serial_uart_stm32g4xx.c \ + drivers/stm32/system_stm32g4xx.c \ drivers/stm32/timer_hal.c \ drivers/stm32/timer_stm32g4xx.c \ drivers/stm32/transponder_ir_io_hal.c \ - drivers/stm32/system_stm32g4xx.c \ - drivers/stm32/serial_uart_stm32g4xx.c \ - drivers/stm32/serial_uart_hal.c \ startup/system_stm32g4xx.c MCU_EXCLUDES = \ - drivers/bus_i2c.c \ - drivers/timer.c + drivers/bus_i2c.c # G4's MSC use the same driver layer file with F7 MSC_SRC = \ diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk index c719c02ab4..e61c054da5 100644 --- a/make/mcu/STM32H7.mk +++ b/make/mcu/STM32H7.mk @@ -131,7 +131,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(USBMSC_DIR)/Inc \ $(CMSIS_DIR)/Core/Include \ $(ROOT)/lib/main/STM32H7/Drivers/CMSIS/Device/ST/STM32H7xx/Include \ - $(ROOT)/src/main/vcp_hal + $(ROOT)/src/main/drivers/stm32/vcp_hal #Flags ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant @@ -287,45 +287,48 @@ endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -DHSE_STARTUP_TIMEOUT=1000 -DSTM32 VCP_SRC = \ - drivers/serial_usb_vcp.c \ - drivers/usb_io.c \ - vcp_hal/usbd_cdc_hid.c \ - vcp_hal/usbd_cdc_interface.c \ - vcp_hal/usbd_conf_stm32h7xx.c \ - vcp_hal/usbd_desc.c + drivers/stm32/vcp_hal/usbd_desc.c \ + drivers/stm32/vcp_hal/usbd_conf_stm32h7xx.c \ + drivers/stm32/vcp_hal/usbd_cdc_hid.c \ + drivers/stm32/vcp_hal/usbd_cdc_interface.c \ + drivers/stm32/serial_usb_vcp.c \ + drivers/usb_io.c MCU_COMMON_SRC = \ - startup/system_stm32h7xx.c \ drivers/bus_i2c_timing.c \ drivers/bus_quadspi.c \ - drivers/dshot_bitbang.c \ drivers/dshot_bitbang_decode.c \ - drivers/persistent.c \ drivers/pwm_output_dshot_shared.c \ drivers/stm32/adc_stm32h7xx.c \ drivers/stm32/audio_stm32h7xx.c \ - drivers/stm32/bus_i2c_hal.c \ drivers/stm32/bus_i2c_hal_init.c \ + drivers/stm32/bus_i2c_hal.c \ drivers/stm32/bus_spi_ll.c \ drivers/stm32/bus_quadspi_hal.c \ drivers/stm32/bus_octospi_stm32h7xx.c \ drivers/stm32/dma_stm32h7xx.c \ drivers/stm32/dshot_bitbang_ll.c \ + drivers/stm32/dshot_bitbang.c \ + drivers/stm32/exti.c \ + drivers/stm32/io_stm32.c \ drivers/stm32/light_ws2811strip_hal.c \ drivers/stm32/memprot_hal.c \ drivers/stm32/memprot_stm32h7xx.c \ + drivers/stm32/persistent.c \ + drivers/stm32/pwm_output.c \ drivers/stm32/pwm_output_dshot_hal.c \ + drivers/stm32/rcc_stm32.c \ drivers/stm32/sdio_h7xx.c \ drivers/stm32/serial_uart_hal.c \ drivers/stm32/serial_uart_stm32h7xx.c \ drivers/stm32/system_stm32h7xx.c \ drivers/stm32/timer_hal.c \ drivers/stm32/timer_stm32h7xx.c \ - drivers/stm32/transponder_ir_io_hal.c + drivers/stm32/transponder_ir_io_hal.c \ + startup/system_stm32h7xx.c MCU_EXCLUDES = \ - drivers/bus_i2c.c \ - drivers/timer.c + drivers/bus_i2c.c MSC_SRC = \ drivers/stm32/usb_msc_h7xx.c \ diff --git a/make/source.mk b/make/source.mk index cf739e6923..78e1fc780a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -30,7 +30,6 @@ COMMON_SRC = \ drivers/display_canvas.c \ drivers/dma_common.c \ drivers/dma_reqmap.c \ - drivers/exti.c \ drivers/io.c \ drivers/light_led.c \ drivers/mco.c \ @@ -38,7 +37,6 @@ COMMON_SRC = \ drivers/pinio.c \ drivers/pin_pull_up_down.c \ drivers/resource.c \ - drivers/rcc.c \ drivers/serial.c \ drivers/serial_pinconfig.c \ drivers/serial_uart.c \ @@ -47,7 +45,6 @@ COMMON_SRC = \ drivers/stack_check.c \ drivers/system.c \ drivers/timer_common.c \ - drivers/timer.c \ drivers/transponder_ir_arcitimer.c \ drivers/transponder_ir_ilap.c \ drivers/transponder_ir_erlt.c \ @@ -80,7 +77,6 @@ COMMON_SRC = \ drivers/camera_control.c \ drivers/accgyro/gyro_sync.c \ drivers/pwm_esc_detect.c \ - drivers/pwm_output.c \ drivers/rx/rx_spi.c \ drivers/rx/rx_xn297.c \ drivers/rx/rx_pwm.c \ @@ -362,15 +358,11 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/display_ug2864hsweg01.c \ drivers/inverter.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ - drivers/light_ws2811strip_stdperiph.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ drivers/serial_tcp.c \ drivers/serial_uart_pinconfig.c \ drivers/serial_usb_vcp.c \ - drivers/transponder_ir_io_hal.c \ - drivers/transponder_ir_io_stdperiph.c \ drivers/vtx_rtc6705_soft_spi.c \ drivers/vtx_rtc6705.c \ drivers/vtx_common.c \ diff --git a/src/main/drivers/at32/adc_at32f43x.c b/src/main/drivers/at32/adc_at32f43x.c new file mode 100644 index 0000000000..4238e9b62c --- /dev/null +++ b/src/main/drivers/at32/adc_at32f43x.c @@ -0,0 +1,359 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ADC + +#include "build/debug.h" + +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" +#include "drivers/resource.h" +#include "drivers/dma.h" + +#include "drivers/sensor.h" + +#include "drivers/adc.h" +#include "drivers/adc_impl.h" + +#include "pg/adc.h" + + +const adcDevice_t adcHardware[ADCDEV_COUNT] = { + { + .ADCx = ADC1, + .rccADC = RCC_APB2(ADC1), + .dmaResource = NULL + }, + { + .ADCx = ADC2, + .rccADC = RCC_APB2(ADC2), + .dmaResource = NULL + + }, + { + .ADCx = ADC3, + .rccADC = RCC_APB2(ADC3), + .dmaResource = NULL + }, +}; + +adcDevice_t adcDevice[ADCDEV_COUNT]; + +#define ADC_CHANNEL_VREFINT ADC_CHANNEL_17 +#define ADC_CHANNEL_TEMPSENSOR_ADC1 ADC_CHANNEL_16 + +const adcTagMap_t adcTagMap[] = { +#ifdef USE_ADC_INTERNAL +#define ADC_TAG_MAP_VREFINT 0 +#define ADC_TAG_MAP_TEMPSENSOR 1 + { DEFIO_TAG_E__NONE, ADC_DEVICES_1, ADC_CHANNEL_VREFINT, 17 }, + { DEFIO_TAG_E__NONE, ADC_DEVICES_1, ADC_CHANNEL_TEMPSENSOR_ADC1, 16 }, +#endif + + { DEFIO_TAG_E__PA0, ADC_DEVICES_123, ADC_CHANNEL_0, 0 }, + { DEFIO_TAG_E__PA1, ADC_DEVICES_123, ADC_CHANNEL_1, 1 }, + { DEFIO_TAG_E__PA2, ADC_DEVICES_123, ADC_CHANNEL_2, 2 }, + { DEFIO_TAG_E__PA3, ADC_DEVICES_123, ADC_CHANNEL_3, 3 }, + { DEFIO_TAG_E__PA4, ADC_DEVICES_12, ADC_CHANNEL_4, 4 }, + { DEFIO_TAG_E__PA5, ADC_DEVICES_12, ADC_CHANNEL_5, 5 }, + { DEFIO_TAG_E__PA6, ADC_DEVICES_12, ADC_CHANNEL_6, 6 }, + { DEFIO_TAG_E__PA7, ADC_DEVICES_12, ADC_CHANNEL_7, 7 }, + { DEFIO_TAG_E__PB0, ADC_DEVICES_12, ADC_CHANNEL_8, 8 }, + { DEFIO_TAG_E__PB1, ADC_DEVICES_12, ADC_CHANNEL_9, 9 }, + { DEFIO_TAG_E__PC0, ADC_DEVICES_123, ADC_CHANNEL_10, 10 }, + { DEFIO_TAG_E__PC1, ADC_DEVICES_123, ADC_CHANNEL_11, 11 }, + { DEFIO_TAG_E__PC2, ADC_DEVICES_123, ADC_CHANNEL_12, 12 }, + { DEFIO_TAG_E__PC3, ADC_DEVICES_123, ADC_CHANNEL_13, 13 }, + { DEFIO_TAG_E__PC4, ADC_DEVICES_12, ADC_CHANNEL_14, 14 }, + { DEFIO_TAG_E__PC5, ADC_DEVICES_12, ADC_CHANNEL_15, 15 }, +}; + +void adcInitDevice(adcDevice_t *adcdev, int channelCount) +{ + adc_base_config_type adc_base_struct; + adc_base_default_para_init(&adc_base_struct); + adc_base_struct.sequence_mode = TRUE; + adc_base_struct.repeat_mode = TRUE; + adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT; + adc_base_struct.ordinary_channel_length = channelCount; + adc_base_config(adcdev->ADCx, &adc_base_struct); + adc_resolution_set(adcdev->ADCx, ADC_RESOLUTION_12B); +} + +int adcFindTagMapEntry(ioTag_t tag) +{ + for (int i = 0; i < ADC_TAG_MAP_COUNT; i++) { + if (adcTagMap[i].tag == tag) { + return i; + } + } + return -1; +} + +volatile DMA_DATA uint32_t adcConversionBuffer[ADC_CHANNEL_COUNT]; + +void adcInit(const adcConfig_t *config) +{ + memset(adcOperatingConfig, 0, sizeof(adcOperatingConfig)); + memcpy(adcDevice, adcHardware, sizeof(adcDevice)); + + if (config->vbat.enabled) { + adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag; + } + + if (config->rssi.enabled) { + adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; + } + + if (config->external1.enabled) { + adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; + } + + if (config->current.enabled) { + adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; + } + + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { + int map; + int dev; + + if (i == ADC_TEMPSENSOR) { + map = ADC_TAG_MAP_TEMPSENSOR; + dev = ADCDEV_1; + } else if (i == ADC_VREFINT) { + map = ADC_TAG_MAP_VREFINT; + dev = ADCDEV_1; + } else { + if (!adcOperatingConfig[i].tag) { + continue; + } + + map = adcFindTagMapEntry(adcOperatingConfig[i].tag); + if (map < 0) { + continue; + } + + for (dev = 0; dev < ADCDEV_COUNT; dev++) { +#ifndef USE_DMA_SPEC + if (!adcDevice[dev].ADCx || !adcDevice[dev].dmaResource) { + continue; + } +#else + if (!adcDevice[dev].ADCx) { + continue; + } +#endif + + if (adcTagMap[map].devices & (1 << dev)) { + break; + } + + if (dev == ADCDEV_COUNT) { + continue; + } + } + } + + adcOperatingConfig[i].adcDevice = dev; + adcOperatingConfig[i].adcChannel = adcTagMap[map].channel; + adcOperatingConfig[i].sampleTime = ADC_SAMPLING_INTERVAL_5CYCLES; + adcOperatingConfig[i].enabled = true; + + adcDevice[dev].channelBits |= (1 << adcTagMap[map].channelOrdinal); + + if (adcOperatingConfig[i].tag) { + IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0); + IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG,GPIO_DRIVE_STRENGTH_MODERATE, 0, GPIO_PULL_NONE)); + } + } + + int dmaBufferIndex = 0; + for (int dev = 0; dev < ADCDEV_COUNT; dev++) { + adcDevice_t *adc = &adcDevice[dev]; + + if (!(adc->ADCx && adc->channelBits)) { + continue; + } + + RCC_ClockCmd(adc->rccADC, ENABLE); + + adc_reset(); + if (adc->ADCx == ADC1) { + adc_common_config_type adc_common_struct; + adc_common_default_para_init(&adc_common_struct); + adc_common_struct.combine_mode = ADC_INDEPENDENT_MODE; + adc_common_struct.div = ADC_HCLK_DIV_4; + adc_common_struct.common_dma_mode = ADC_COMMON_DMAMODE_DISABLE; + adc_common_struct.common_dma_request_repeat_state = FALSE; + adc_common_struct.sampling_interval = ADC_SAMPLING_INTERVAL_5CYCLES; + adc_common_struct.tempervintrv_state = TRUE; + adc_common_struct.vbat_state = TRUE; + adc_common_config(&adc_common_struct); + } + + int configuredAdcChannels = BITCOUNT(adc->channelBits); + adcInitDevice(adc, configuredAdcChannels); + +#ifdef USE_DMA_SPEC + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, dev, config->dmaopt[dev]); + dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaSpec->ref); + if (!dmaSpec || !dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev))) { + return; + } + + dmaEnable(dmaIdentifier); + xDMA_DeInit(dmaSpec->ref); + + adc->dmaResource=dmaSpec->ref; + + dma_init_type dma_init_struct; + dma_default_para_init(&dma_init_struct); + dma_init_struct.buffer_size = BITCOUNT(adc->channelBits); + dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_WORD; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.priority = DMA_PRIORITY_MEDIUM; + dma_init_struct.loop_mode_enable = TRUE; + + dma_init_struct.memory_base_addr = (uint32_t)&(adcConversionBuffer[dmaBufferIndex]); + dma_init_struct.peripheral_base_addr = (uint32_t)&(adc->ADCx->odt); + + xDMA_Init(dmaSpec->ref, &dma_init_struct); + dmaMuxEnable(dmaIdentifier, dmaSpec->dmaMuxId); + + /* enable dma transfer complete interrupt */ + xDMA_ITConfig(dmaSpec->ref,DMA_IT_TCIF,ENABLE); + xDMA_Cmd(dmaSpec->ref,ENABLE); + + adc_dma_mode_enable(adc->ADCx, TRUE); + adc_dma_request_repeat_enable(adc->ADCx, TRUE); +#endif //end of USE_DMA_SPEC + + for (int adcChan = 0; adcChan < ADC_CHANNEL_COUNT; adcChan++) { + + if (!adcOperatingConfig[adcChan].enabled) { + continue; + } + + if (adcOperatingConfig[adcChan].adcDevice != dev) { + continue; + } + + adcOperatingConfig[adcChan].dmaIndex = dmaBufferIndex++; + adc_ordinary_channel_set(adcDevice[dev].ADCx, + adcOperatingConfig[adcChan].adcChannel, + adcOperatingConfig[adcChan].dmaIndex+1, + ADC_SAMPLETIME_92_5); + } + + + if (adc->ADCx==ADC1) { + adc_voltage_monitor_threshold_value_set(ADC1, 0x100, 0x000); + adc_voltage_monitor_single_channel_select(ADC1, adcOperatingConfig[ADC_BATTERY].adcChannel); + adc_voltage_monitor_enable(ADC1, ADC_VMONITOR_SINGLE_ORDINARY); + } + + adc_interrupt_enable(adc->ADCx, ADC_OCCO_INT, TRUE); + + adc_enable(adc->ADCx, TRUE); + while (adc_flag_get(adc->ADCx, ADC_RDY_FLAG) == RESET); + + dmaBufferIndex += BITCOUNT(adc->channelBits); + + adc_calibration_init(adc->ADCx); + while (adc_calibration_init_status_get(adc->ADCx)); + adc_calibration_start(adc->ADCx); + while (adc_calibration_status_get(adc->ADCx)); + + adc_enable(adc->ADCx, TRUE); + adc_ordinary_software_trigger_enable(adc->ADCx, TRUE); + } +} + +void adcGetChannelValues(void) +{ + for (int i = 0; i < ADC_CHANNEL_INTERNAL_FIRST_ID; i++) { + if (adcOperatingConfig[i].enabled) { + adcValues[adcOperatingConfig[i].dmaIndex] = adcConversionBuffer[adcOperatingConfig[i].dmaIndex]; + } + } +} + +#ifdef USE_ADC_INTERNAL + +bool adcInternalIsBusy(void) +{ + return false; +} + +void adcInternalStartConversion(void) +{ + return; +} + +uint16_t adcInternalRead(int channel) +{ + int dmaIndex = adcOperatingConfig[channel].dmaIndex; + return adcConversionBuffer[dmaIndex]; +} + +int adcPrivateVref = -1; +int adcPrivateTemp = -1; + +uint16_t adcInternalReadVrefint(void) +{ + uint16_t value = adcInternalRead(ADC_VREFINT); + adcPrivateVref =((double)1.2 * 4095) / value * 1000; + return adcPrivateVref; +} + +uint16_t adcInternalReadTempsensor(void) +{ + uint16_t value = adcInternalRead(ADC_TEMPSENSOR); + adcPrivateTemp = (((ADC_TEMP_BASE- value * ADC_VREF / 4096) / ADC_TEMP_SLOPE) + 25); + return adcPrivateTemp; +} + +void ADC1_2_3_IRQHandler(void) +{ + for (int dev = 0; dev < ADCDEV_COUNT; dev++) { + adcDevice_t *adc = &adcDevice[dev]; + + if (!(adc->ADCx && adc->channelBits)) { + continue; + } + if(adc_flag_get(adc->ADCx, ADC_OCCO_FLAG) != RESET) { + adc_flag_clear(adc->ADCx, ADC_OCCO_FLAG); + } + } +} + +#endif // USE_ADC_INTERNAL +#endif // USE_ADC diff --git a/src/main/drivers/at32/bus_i2c_atbsp.c b/src/main/drivers/at32/bus_i2c_atbsp.c new file mode 100644 index 0000000000..7b511585df --- /dev/null +++ b/src/main/drivers/at32/bus_i2c_atbsp.c @@ -0,0 +1,258 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_I2C) && !defined(SOFT_I2C) + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/rcc.h" + +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" + +#define I2C_TIMEOUT 0x870 //about 7 us at 288 mhz + +#ifdef USE_I2C_DEVICE_1 +void I2C1_ERR_IRQHandler(void) +{ + i2c_err_irq_handler(&i2cDevice[I2CDEV_1].handle); +} + +void I2C1_EVT_IRQHandler(void) +{ + i2c_evt_irq_handler(&i2cDevice[I2CDEV_1].handle); +} +#endif + +#ifdef USE_I2C_DEVICE_2 +void I2C2_ERR_IRQHandler(void) +{ + i2c_err_irq_handler(&i2cDevice[I2CDEV_2].handle); +} + +void I2C2_EVT_IRQHandler(void) +{ + i2c_evt_irq_handler(&i2cDevice[I2CDEV_2].handle); +} +#endif + +#ifdef USE_I2C_DEVICE_3 +void I2C3_ERR_IRQHandler(void) +{ + i2c_err_irq_handler(&i2cDevice[I2CDEV_3].handle); +} + +void I2C3_EVT_IRQHandler(void) +{ + i2c_evt_irq_handler(&i2cDevice[I2CDEV_3].handle); +} +#endif + +#ifdef USE_I2C_DEVICE_4 +void I2C4_ERR_IRQHandler(void) +{ + i2c_err_irq_handler(&i2cDevice[I2CDEV_4].handle); +} + +void I2C4_EVT_IRQHandler(void) +{ + i2c_evt_irq_handler(&i2cDevice[I2CDEV_4].handle); +} +#endif + +static volatile uint16_t i2cErrorCount = 0; + +static bool i2cHandleHardwareFailure(I2CDevice device) +{ + (void)device; + i2cErrorCount++; + return false; +} + +uint16_t i2cGetErrorCounter(void) +{ + return i2cErrorCount; +} + +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_handle_type *pHandle = &i2cDevice[device].handle; + + if (!pHandle->i2cx) { + return false; + } + + i2c_status_type status; + + if (reg_ == 0xFF) { + status = i2c_master_transmit(pHandle, addr_ << 1, &data, 1, I2C_TIMEOUT); + + if (status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + } else { + status = i2c_memory_write(pHandle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_, &data, 1, I2C_TIMEOUT_US); + + if(status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + } + + if (status != I2C_OK) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_handle_type *pHandle = &i2cDevice[device].handle; + + if (!pHandle->i2cx) { + return false; + } + + i2c_status_type status; + status = i2c_memory_write_int(pHandle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_,data, len_, I2C_TIMEOUT); + + if (status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + + if (status == I2C_ERR_STEP_1) { + return false; + } + + if (status != I2C_OK) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_handle_type *pHandle = &i2cDevice[device].handle; + + if (!pHandle->i2cx) { + return false; + } + + i2c_status_type status; + + if (reg_ == 0xFF) { + status = i2c_master_receive(pHandle ,addr_ << 1 , buf, len, I2C_TIMEOUT); + + if (status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + } else { + status = i2c_memory_read(pHandle, I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_, buf, len, I2C_TIMEOUT); + + if (status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + } + + + if (status != I2C_OK) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + i2c_handle_type *pHandle = &i2cDevice[device].handle; + + if (!pHandle->i2cx) { + return false; + } + + i2c_status_type status; + + status = i2c_memory_read_int(pHandle,I2C_MEM_ADDR_WIDIH_8, addr_ << 1, reg_,buf, len,I2C_TIMEOUT); + + if (status != I2C_OK) { + i2c_wait_flag(pHandle, I2C_STOPF_FLAG, I2C_EVENT_CHECK_NONE, I2C_TIMEOUT); + i2c_flag_clear(pHandle->i2cx, I2C_STOPF_FLAG); + } + + if (status == I2C_ERR_STEP_1) { + return false; + } + + if (status != I2C_OK) { + return i2cHandleHardwareFailure(device); + } + + return true; +} + +bool i2cBusy(I2CDevice device, bool *error) +{ + i2c_handle_type *pHandle = &i2cDevice[device].handle; + + if (error) { + *error = pHandle->error_code; + } + + if (pHandle->error_code == I2C_OK) { + if (i2c_flag_get(pHandle->i2cx, I2C_BUSYF_FLAG) == SET) { + return true; + } + return false; + } + + return true; +} + +#endif diff --git a/src/main/drivers/at32/bus_i2c_atbsp_init.c b/src/main/drivers/at32/bus_i2c_atbsp_init.c new file mode 100644 index 0000000000..980d0ba2eb --- /dev/null +++ b/src/main/drivers/at32/bus_i2c_atbsp_init.c @@ -0,0 +1,200 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_I2C) && !defined(SOFT_I2C) + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/rcc.h" + +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" +#include "drivers/bus_i2c_timing.h" + +// Number of bits in I2C protocol phase +#define LEN_ADDR 7 +#define LEN_RW 1 +#define LEN_ACK 1 + +// Clock period in us during unstick transfer +#define UNSTICK_CLK_US 10 + +// Allow 500us for clock strech to complete during unstick +#define UNSTICK_CLK_STRETCH (500/UNSTICK_CLK_US) + +static void i2cUnstick(IO_t scl, IO_t sda); + +#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_MUX , GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_OPEN_DRAIN , GPIO_PULL_UP) +#define IOCFG_I2C IO_CONFIG(GPIO_MODE_MUX , GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_OPEN_DRAIN , GPIO_PULL_NONE) + +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = I2C1, + .sclPins = { + I2CPINDEF(PB6, GPIO_MUX_4), + I2CPINDEF(PB8, GPIO_MUX_4), + }, + .sdaPins = { + I2CPINDEF(PB7, GPIO_MUX_4), + I2CPINDEF(PB9, GPIO_MUX_4), + }, + .rcc = RCC_APB1(I2C1), + .ev_irq = I2C1_EVT_IRQn, + .er_irq = I2C1_ERR_IRQn, + }, +#endif +#ifdef USE_I2C_DEVICE_2 + { + .device = I2CDEV_2, + .reg = I2C2, + .sclPins = { + I2CPINDEF(PB10, GPIO_MUX_4), + I2CPINDEF(PD12, GPIO_MUX_4), + I2CPINDEF(PH2, GPIO_MUX_4), + }, + .sdaPins = { + I2CPINDEF(PB3, GPIO_MUX_4), + I2CPINDEF(PB11, GPIO_MUX_4), + I2CPINDEF(PH3,GPIO_MUX_4), + }, + .rcc = RCC_APB1(I2C2), + .ev_irq = I2C2_EVT_IRQn, + .er_irq = I2C2_ERR_IRQn, + }, +#endif +#ifdef USE_I2C_DEVICE_3 + { + .device = I2CDEV_3, + .reg = I2C3, + .sclPins = { + I2CPINDEF(PC0, GPIO_MUX_4), + }, + .sdaPins = { + I2CPINDEF(PC1, GPIO_MUX_4), + I2CPINDEF(PB14, GPIO_MUX_4), + }, + .rcc = RCC_APB1(I2C3), + .ev_irq = I2C3_EVT_IRQn, + .er_irq = I2C3_ERR_IRQn, + }, +#endif +}; + +i2cDevice_t i2cDevice[I2CDEV_COUNT]; + +void i2cInit(I2CDevice device) +{ + if (device == I2CINVALID) { + return; + } + + i2cDevice_t *pDev = &i2cDevice[device]; + + const i2cHardware_t *hardware = pDev->hardware; + const IO_t scl = pDev->scl; + const IO_t sda = pDev->sda; + + if (!hardware || IOGetOwner(scl) || IOGetOwner(sda)) { + return; + } + + IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); + IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); + + // Enable i2c RCC + RCC_ClockCmd(hardware->rcc, ENABLE); + + i2cUnstick(scl, sda); + + // Init pins + + IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF); + IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF); + + + // Init I2C peripheral + + i2c_handle_type *pHandle = &pDev->handle; + + memset(pHandle, 0, sizeof(*pHandle)); + + pHandle->i2cx = pDev->hardware->reg; + + crm_clocks_freq_type crm_clk_freq; + crm_clocks_freq_get(&crm_clk_freq); + uint32_t i2cPclk = crm_clk_freq.apb1_freq; + + uint32_t I2Cx_CLKCTRL = i2cClockTIMINGR(i2cPclk, pDev->clockSpeed, 0); + + i2c_reset( pHandle->i2cx); + + i2c_init( pHandle->i2cx, 0x0f, I2Cx_CLKCTRL); + + i2c_own_address1_set( pHandle->i2cx, I2C_ADDRESS_MODE_7BIT, 0x0); + + nvic_irq_enable(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER)); + nvic_irq_enable(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV)); + + i2c_enable(pHandle->i2cx, TRUE); +} + +static void i2cUnstick(IO_t scl, IO_t sda) +{ + int i; + + IOHi(scl); + IOHi(sda); + + IOConfigGPIO(scl, IOCFG_OUT_OD); + IOConfigGPIO(sda, IOCFG_OUT_OD); + + for (i = 0; i < (LEN_ADDR + LEN_RW + LEN_ACK); i++) { + int timeout = UNSTICK_CLK_STRETCH; + while (!IORead(scl) && timeout) { + delayMicroseconds(UNSTICK_CLK_US); + timeout--; + } + + IOLo(scl); + delayMicroseconds(UNSTICK_CLK_US / 2); + IOHi(scl); + delayMicroseconds(UNSTICK_CLK_US / 2); + } + + IOLo(scl); + delayMicroseconds(UNSTICK_CLK_US / 2); + IOLo(sda); + delayMicroseconds(UNSTICK_CLK_US / 2); + + IOHi(scl); + delayMicroseconds(UNSTICK_CLK_US / 2); + IOHi(sda); +} +#endif diff --git a/src/main/drivers/at32/exti_at32.c b/src/main/drivers/at32/exti_at32.c new file mode 100644 index 0000000000..73f0873ec5 --- /dev/null +++ b/src/main/drivers/at32/exti_at32.c @@ -0,0 +1,186 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_EXTI + +#include "drivers/nvic.h" +#include "drivers/io_impl.h" +#include "drivers/exti.h" + +typedef struct { + extiCallbackRec_t* handler; +} extiChannelRec_t; + +extiChannelRec_t extiChannelRecs[16]; + +#define EXTI_IRQ_GROUPS 7 +// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 +static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 }; +static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS]; + +static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { + EXINT0_IRQn, + EXINT1_IRQn, + EXINT2_IRQn, + EXINT3_IRQn, + EXINT4_IRQn, + EXINT9_5_IRQn, + EXINT15_10_IRQn +}; + +static uint32_t triggerLookupTable[] = { + [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXINT_TRIGGER_RISING_EDGE, + [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXINT_TRIGGER_FALLING_EDGE, + [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXINT_TRIGGER_BOTH_EDGE +}; + +#define EXTI_REG_IMR (EXINT->inten) +#define EXTI_REG_PR (EXINT->intsts) + +void EXTIInit(void) +{ + crm_periph_clock_enable(CRM_SCFG_PERIPH_CLOCK, TRUE); + + memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); + memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); +} + +void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn) +{ + self->fn = fn; +} + +void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger) +{ + int chIdx = IO_GPIOPinIdx(io); + + if (chIdx < 0) { + return; + } + + int group = extiGroups[chIdx]; + + extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + rec->handler = cb; + + EXTIDisable(io); + + IOConfigGPIO(io, config); + + scfg_exint_line_config(IO_GPIO_PortSource(io), IO_GPIO_PinSource(io)); + + uint32_t extiLine = IO_EXTI_Line(io); + + exint_flag_clear(extiLine); + + exint_init_type exint_init_struct; + exint_default_para_init(&exint_init_struct); + exint_init_struct.line_enable = TRUE; + exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT; + exint_init_struct.line_select = extiLine; + exint_init_struct.line_polarity = triggerLookupTable[trigger]; + exint_init(&exint_init_struct); + + if (extiGroupPriority[group] > irqPriority) { + extiGroupPriority[group] = irqPriority; + + nvic_priority_group_config(NVIC_PRIORITY_GROUPING); + nvic_irq_enable(extiGroupIRQn[group], + NVIC_PRIORITY_BASE(irqPriority), + NVIC_PRIORITY_SUB(irqPriority)); + + } +} + +void EXTIRelease(IO_t io) +{ + EXTIDisable(io); + + const int chIdx = IO_GPIOPinIdx(io); + + if (chIdx < 0) { + return; + } + + extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + rec->handler = NULL; +} + +void EXTIEnable(IO_t io) +{ + uint32_t extiLine = IO_EXTI_Line(io); + + if (!extiLine) { + return; + } + + EXTI_REG_IMR |= extiLine; +} + +void EXTIDisable(IO_t io) +{ + uint32_t extiLine = IO_EXTI_Line(io); + + if (!extiLine) { + return; + } + + EXTI_REG_IMR &= ~extiLine; + EXTI_REG_PR = extiLine; +} + +#define EXTI_EVENT_MASK 0xFFFF + +void EXTI_IRQHandler(uint32_t mask) +{ + uint32_t exti_active = (EXTI_REG_IMR & EXTI_REG_PR) & mask; + + EXTI_REG_PR = exti_active; + + while (exti_active) { + unsigned idx = 31 - __builtin_clz(exti_active); + uint32_t mask = 1 << idx; + extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); + exti_active &= ~mask; + } +} + +#define _EXTI_IRQ_HANDLER(name, mask) \ + void name(void) { \ + EXTI_IRQHandler(mask & EXTI_EVENT_MASK); \ + } \ + struct dummy \ + /**/ + +_EXTI_IRQ_HANDLER(EXINT0_IRQHandler, 0x0001); +_EXTI_IRQ_HANDLER(EXINT1_IRQHandler, 0x0002); +_EXTI_IRQ_HANDLER(EXINT2_IRQHandler, 0x0004); +_EXTI_IRQ_HANDLER(EXINT3_IRQHandler, 0x0008); +_EXTI_IRQ_HANDLER(EXINT4_IRQHandler, 0x0010); +_EXTI_IRQ_HANDLER(EXINT9_5_IRQHandler, 0x03e0); +_EXTI_IRQ_HANDLER(EXINT15_10_IRQHandler, 0xfc00); + +#endif \ No newline at end of file diff --git a/src/main/drivers/at32/io_at32.c b/src/main/drivers/at32/io_at32.c new file mode 100644 index 0000000000..321c32f868 --- /dev/null +++ b/src/main/drivers/at32/io_at32.c @@ -0,0 +1,136 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" + +#include "common/utils.h" + +// io ports defs are stored in array by index now +struct ioPortDef_s { + rccPeriphTag_t rcc; +}; + +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB1(GPIOA) }, + { RCC_AHB1(GPIOB) }, + { RCC_AHB1(GPIOC) }, + { RCC_AHB1(GPIOD) }, + { RCC_AHB1(GPIOE) }, + { RCC_AHB1(GPIOF) }, + { RCC_AHB1(GPIOG) }, + { RCC_AHB1(GPIOH) } +}; + +uint32_t IO_EXTI_Line(IO_t io) +{ + if (!io) { + return 0; + } + return 1 << IO_GPIOPinIdx(io); +} + +bool IORead(IO_t io) +{ + if (!io) { + return false; + } + return (IO_GPIO(io)->idt & IO_Pin(io)); +} + +void IOWrite(IO_t io, bool hi) +{ + if (!io) { + return; + } + IO_GPIO(io)->scr = IO_Pin(io) << (hi ? 0 : 16); +} + +void IOHi(IO_t io) +{ + if (!io) { + return; + } + IO_GPIO(io)->scr = IO_Pin(io); +} + +void IOLo(IO_t io) +{ + if (!io) { + return; + } + IO_GPIO(io)->clr = IO_Pin(io); +} + +void IOToggle(IO_t io) +{ + if (!io) { + return; + } + + uint32_t mask = IO_Pin(io); + + if (IO_GPIO(io)->odt & mask) { + mask <<= 16; // bit is set, shift mask to reset half + } + IO_GPIO(io)->scr = mask; +} + +void IOConfigGPIO(IO_t io, ioConfig_t cfg) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + gpio_init_type init = { + .gpio_pins = IO_Pin(io), + .gpio_mode = (cfg >> 0) & 0x03, + .gpio_drive_strength = (cfg >> 2) & 0x03, + .gpio_out_type = (cfg >> 4) & 0x01, + .gpio_pull = (cfg >> 5) & 0x03, + }; + gpio_init(IO_GPIO(io), &init); +} + +void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + gpio_init_type init = { + .gpio_pins = IO_Pin(io), + .gpio_mode = (cfg >> 0) & 0x03, + .gpio_drive_strength = (cfg >> 2) & 0x03, + .gpio_out_type = (cfg >> 4) & 0x01, + .gpio_pull = (cfg >> 5) & 0x03, + }; + gpio_init(IO_GPIO(io), &init); + gpio_pin_mux_config(IO_GPIO(io), IO_GPIO_PinSource(io), af); +} diff --git a/src/main/drivers/at32/light_ws2811strip_at32f43x.c b/src/main/drivers/at32/light_ws2811strip_at32f43x.c new file mode 100644 index 0000000000..98b1633b05 --- /dev/null +++ b/src/main/drivers/at32/light_ws2811strip_at32f43x.c @@ -0,0 +1,204 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_LED_STRIP + +#include "build/debug.h" + +#include "common/color.h" + +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "drivers/rcc.h" +#include "drivers/timer.h" + +#include "drivers/light_ws2811strip.h" + +static IO_t ws2811IO = IO_NONE; + +static dmaResource_t *dmaRef = NULL; +static tmr_type *timer = NULL; + +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ +#if defined(USE_WS2811_SINGLE_COLOUR) + static uint32_t counter = 0; +#endif + + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { +#if defined(USE_WS2811_SINGLE_COLOUR) + counter++; + if (counter == WS2811_LED_STRIP_LENGTH) { + // Output low for 50us delay + memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); + } else if (counter == (WS2811_LED_STRIP_LENGTH + WS2811_DELAY_ITERATIONS)) { + counter = 0; + ws2811LedDataTransferInProgress = false; + xDMA_Cmd(descriptor->ref, DISABLE); + } +#else + ws2811LedDataTransferInProgress = false; + xDMA_Cmd(descriptor->ref, FALSE); +#endif + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + } +} + +bool ws2811LedStripHardwareInit(ioTag_t ioTag) +{ + if (!ioTag) { + return false; + } + + const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + + if (timerHardware == NULL) { + return false; + } + + timer = timerHardware->tim; + +#if defined(USE_DMA_SPEC) + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware); + + if (dmaSpec == NULL) { + return false; + } + + dmaRef = dmaSpec->ref; + +#else + dmaRef = timerHardware->dmaRef; +#endif + + if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) { + return false; + } + + ws2811IO = IOGetByTag(ioTag); + IOInit(ws2811IO, OWNER_LED_STRIP, 0); + + IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP, timerHardware->alternateFunction); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + + // Stop timer + tmr_counter_enable(timer, FALSE); + + /* Compute the prescaler value */ + uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, WS2811_TIMER_MHZ); + uint16_t period = timerGetPeriodByPrescaler(timer, prescaler, WS2811_CARRIER_HZ); + + BIT_COMPARE_1 = period / 3 * 2; + BIT_COMPARE_0 = period / 3; + + /* Time base configuration */ + tmr_base_init(timer,period-1,prescaler-1); + tmr_clock_source_div_set(timer,TMR_CLOCK_DIV1); + tmr_cnt_dir_set(timer,TMR_COUNT_UP); + + + tmr_output_config_type tmr_OCInitStruct; + tmr_output_default_para_init(&tmr_OCInitStruct); + tmr_OCInitStruct.oc_mode= TMR_OUTPUT_CONTROL_PWM_MODE_A; + + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + tmr_OCInitStruct.occ_output_state = TRUE; + tmr_OCInitStruct.occ_idle_state = FALSE; + tmr_OCInitStruct.occ_polarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; + } else { + tmr_OCInitStruct.oc_output_state = TRUE; + tmr_OCInitStruct.oc_idle_state = TRUE; + tmr_OCInitStruct.oc_polarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; + } + + tmr_channel_value_set(timer, (timerHardware->channel-1)*2, 0); + tmr_output_channel_config(timer,(timerHardware->channel-1)*2, &tmr_OCInitStruct); + + + tmr_period_buffer_enable(timer, TRUE); + tmr_output_channel_buffer_enable(timer, ((timerHardware->channel-1) * 2), TRUE); + + tmr_dma_request_enable(timer, timerDmaSource(timerHardware->channel), TRUE); + + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + tmr_channel_enable(timer, ((timerHardware->channel -1) * 2 + 1), TRUE); + } else { + tmr_channel_enable(timer, ((timerHardware->channel-1) * 2), TRUE); + } + + dmaEnable(dmaGetIdentifier(dmaRef)); + dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + + xDMA_Cmd(dmaRef, FALSE); + xDMA_DeInit(dmaRef); + + dma_init_type dma_init_struct; + dma_init_struct.peripheral_base_addr = (uint32_t)timerCCR(timer, timerHardware->channel); + dma_init_struct.buffer_size = WS2811_DMA_BUFFER_SIZE; + dma_init_struct.peripheral_inc_enable = FALSE; + dma_init_struct.memory_inc_enable = TRUE; + dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL; + dma_init_struct.memory_base_addr = (uint32_t)ledStripDMABuffer; + dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_WORD; + dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_WORD; + dma_init_struct.priority = DMA_PRIORITY_MEDIUM; + +#if defined(USE_WS2811_SINGLE_COLOUR) + dma_init_struct.loop_mode_enable = TRUE; +#else + dma_init_struct.loop_mode_enable = FALSE; +#endif + + xDMA_Init(dmaRef, &dma_init_struct); + +#if defined(USE_DMA_SPEC) + dmaMuxEnable(dmaGetIdentifier(dmaRef), dmaSpec->dmaMuxId); +#else + #warning "USE_DMA_SPEC DETECT FAIL IN LEDSTRIP AT32F43X NEED DMA MUX " +#endif + + xDMA_ITConfig(dmaRef, DMA_IT_TCIF, TRUE); + xDMA_Cmd(dmaRef, TRUE); + + tmr_output_enable(timer, TRUE); + tmr_counter_enable(timer, TRUE); + + return true; +} + +void ws2811LedStripDMAEnable(void) +{ + xDMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); + tmr_counter_value_set(timer, 0); + tmr_counter_enable(timer, TRUE); + xDMA_Cmd(dmaRef, TRUE); + +} +#endif diff --git a/src/main/drivers/at32/rcc_at32.c b/src/main/drivers/at32/rcc_at32.c new file mode 100644 index 0000000000..d3b2d1a4f1 --- /dev/null +++ b/src/main/drivers/at32/rcc_at32.c @@ -0,0 +1,105 @@ +/* + * This file is part of Cleanflight and Betaflight and JustFlight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * this file define the at32f435/7 crm clock enable/reset functions + * + * + */ + +#include "platform.h" +#include "drivers/rcc.h" + +void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) +{ + int tag = periphTag >> 5; + uint32_t mask = 1 << (periphTag & 0x1f); + +#define NOSUFFIX // Empty + +#define __RCC_CLK_ENABLE(bus, suffix, enbit) do { \ + __IO uint32_t tmpreg; \ + SET_BIT(CRM->bus ## en ## suffix, enbit); \ + tmpreg = READ_BIT(CRM->bus ## en ## suffix, enbit); \ + UNUSED(tmpreg); \ + } while(0) + +#define __RCC_CLK_DISABLE(bus, suffix, enbit) (CRM->bus ## en ## suffix &= ~(enbit)) + +#define __RCC_CLK(bus, suffix, enbit, newState) \ + if (newState == ENABLE) { \ + __RCC_CLK_ENABLE(bus, suffix, enbit); \ + } else { \ + __RCC_CLK_DISABLE(bus, suffix, enbit); \ + } + + switch (tag) { + case RCC_AHB1: + __RCC_CLK(ahb, 1, mask, NewState); + break; + case RCC_AHB2: + __RCC_CLK(ahb, 2, mask, NewState); + break; + case RCC_AHB3: + __RCC_CLK(ahb, 3, mask, NewState); + break; + case RCC_APB1: + __RCC_CLK(apb1, NOSUFFIX, mask, NewState); + break; + case RCC_APB2: + __RCC_CLK(apb2, NOSUFFIX, mask, NewState); + break; + } +} + +void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) +{ + int tag = periphTag >> 5; + uint32_t mask = 1 << (periphTag & 0x1f); + +#define __RCC_FORCE_RESET(bus, suffix, enbit) (CRM->bus ## rst ## suffix |= (enbit)) +#define __RCC_RELEASE_RESET(bus, suffix, enbit) (CRM->bus ## rst ## suffix &= ~(enbit)) +#define __RCC_RESET(bus, suffix, enbit, NewState) \ + if (NewState == ENABLE) { \ + __RCC_RELEASE_RESET(bus, suffix, enbit); \ + } else { \ + __RCC_FORCE_RESET(bus, suffix, enbit); \ + } + + switch (tag) { + case RCC_AHB1: + __RCC_RESET(ahb, 1, mask, NewState); + break; + + case RCC_AHB2: + __RCC_RESET(ahb, 2, mask, NewState); + break; + + case RCC_AHB3: + __RCC_RESET(ahb, 3, mask, NewState); + break; + + case RCC_APB1: + __RCC_RESET(apb1, NOSUFFIX, mask, NewState); + break; + + case RCC_APB2: + __RCC_RESET(apb2, NOSUFFIX, mask, NewState); + break; + } +} diff --git a/src/main/drivers/at32/serial_usb_vcp_at32f4.c b/src/main/drivers/at32/serial_usb_vcp_at32f4.c new file mode 100644 index 0000000000..d7467a0137 --- /dev/null +++ b/src/main/drivers/at32/serial_usb_vcp_at32f4.c @@ -0,0 +1,522 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_VCP + +#include "build/build_config.h" +#include "build/atomic.h" + +#include "common/utils.h" + +#include "drivers/io.h" +#include "drivers/usb_io.h" + +#include "pg/usb.h" + +#include "at32f435_437_clock.h" +#include "drivers/at32/usb/usb_conf.h" +#include "drivers/at32/usb/usb_core.h" +#include "drivers/at32/usb/usbd_int.h" +#include "drivers/at32/usb/cdc_class.h" +#include "drivers/at32/usb/cdc_desc.h" + +#include "drivers/time.h" +#include "drivers/serial.h" +#include "drivers/serial_usb_vcp.h" +#include "drivers/nvic.h" +#include "at32f435_437_tmr.h" + +#define USB_TIMEOUT 50 + +static vcpPort_t vcpPort; + +otg_core_type otg_core_struct; + +#define APP_RX_DATA_SIZE 2048 +#define APP_TX_DATA_SIZE 2048 + +#define APP_TX_BLOCK_SIZE 512 + +volatile uint8_t UserRxBuffer[APP_RX_DATA_SIZE];/* Received Data over USB are stored in this buffer */ +volatile uint8_t UserTxBuffer[APP_TX_DATA_SIZE];/* Received Data over UART (CDC interface) are stored in this buffer */ +uint32_t BuffLength; + +/* Increment this pointer or roll it back to start address when data are received over USART */ +volatile uint32_t UserTxBufPtrIn = 0; +/* Increment this pointer or roll it back to start address when data are sent over USB */ +volatile uint32_t UserTxBufPtrOut = 0; + +volatile uint32_t APP_Rx_ptr_out = 0; +volatile uint32_t APP_Rx_ptr_in = 0; +static uint8_t APP_Rx_Buffer[APP_RX_DATA_SIZE]; + +tmr_type * usbTxTmr= TMR20; +#define CDC_POLLING_INTERVAL 5 + +static void (*ctrlLineStateCb)(void* context, uint16_t ctrlLineState); +static void *ctrlLineStateCbContext; +static void (*baudRateCb)(void *context, uint32_t baud); +static void *baudRateCbContext; + +void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context) +{ + baudRateCbContext = context; + baudRateCb = cb; +} + +void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context) +{ + ctrlLineStateCbContext = context; + ctrlLineStateCb = cb; +} + +void usb_clock48m_select(usb_clk48_s clk_s) +{ + if(clk_s == USB_CLK_HICK) + { + crm_usb_clock_source_select(CRM_USB_CLOCK_SOURCE_HICK); + crm_periph_clock_enable(CRM_ACC_PERIPH_CLOCK, TRUE); + + acc_write_c1(7980); + acc_write_c2(8000); + acc_write_c3(8020); +#if (USB_ID == 0) + acc_sof_select(ACC_SOF_OTG1); +#else + acc_sof_select(ACC_SOF_OTG2); +#endif + acc_calibration_mode_enable(ACC_CAL_HICKTRIM, TRUE); + } else { + switch(system_core_clock) + { + /* 48MHz */ + case 48000000: + crm_usb_clock_div_set(CRM_USB_DIV_1); + break; + + /* 72MHz */ + case 72000000: + crm_usb_clock_div_set(CRM_USB_DIV_1_5); + break; + + /* 96MHz */ + case 96000000: + crm_usb_clock_div_set(CRM_USB_DIV_2); + break; + + /* 120MHz */ + case 120000000: + crm_usb_clock_div_set(CRM_USB_DIV_2_5); + break; + + /* 144MHz */ + case 144000000: + crm_usb_clock_div_set(CRM_USB_DIV_3); + break; + + /* 168MHz */ + case 168000000: + crm_usb_clock_div_set(CRM_USB_DIV_3_5); + break; + + /* 192MHz */ + case 192000000: + crm_usb_clock_div_set(CRM_USB_DIV_4); + break; + + /* 216MHz */ + case 216000000: + crm_usb_clock_div_set(CRM_USB_DIV_4_5); + break; + + /* 240MHz */ + case 240000000: + crm_usb_clock_div_set(CRM_USB_DIV_5); + break; + + /* 264MHz */ + case 264000000: + crm_usb_clock_div_set(CRM_USB_DIV_5_5); + break; + + /* 288MHz */ + case 288000000: + crm_usb_clock_div_set(CRM_USB_DIV_6); + break; + + default: + break; + + } + } +} + +void usb_gpio_config(void) +{ + gpio_init_type gpio_init_struct; + + crm_periph_clock_enable(OTG_PIN_GPIO_CLOCK, TRUE); + gpio_default_para_init(&gpio_init_struct); + + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + + gpio_init_struct.gpio_pins = OTG_PIN_DP | OTG_PIN_DM; + gpio_init(OTG_PIN_GPIO, &gpio_init_struct); + + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_DP_SOURCE, OTG_PIN_MUX); + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_DM_SOURCE, OTG_PIN_MUX); + +#ifdef USB_SOF_OUTPUT_ENABLE + crm_periph_clock_enable(OTG_PIN_SOF_GPIO_CLOCK, TRUE); + gpio_init_struct.gpio_pins = OTG_PIN_SOF; + gpio_init(OTG_PIN_SOF_GPIO, &gpio_init_struct); + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_SOF_SOURCE, OTG_PIN_MUX); +#endif + +#ifndef USB_VBUS_IGNORE + gpio_init_struct.gpio_pins = OTG_PIN_VBUS; + gpio_init_struct.gpio_pull = GPIO_PULL_DOWN; + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_VBUS_SOURCE, OTG_PIN_MUX); + gpio_init(OTG_PIN_GPIO, &gpio_init_struct); +#endif +} + +#ifdef USB_LOW_POWER_WAKUP +void usb_low_power_wakeup_config(void) +{ + exint_init_type exint_init_struct; + + crm_periph_clock_enable(CRM_SCFG_PERIPH_CLOCK, TRUE); + exint_default_para_init(&exint_init_struct); + + exint_init_struct.line_enable = TRUE; + exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT; + exint_init_struct.line_select = OTG_WKUP_EXINT_LINE; + exint_init_struct.line_polarity = EXINT_TRIGGER_RISING_EDGE; + exint_init(&exint_init_struct); + + nvic_irq_enable(OTG_WKUP_IRQ, NVIC_PRIORITY_BASE(NVIC_PRIO_USB_WUP), NVIC_PRIORITY_SUB(NVIC_PRIO_USB_WUP)); +} + +void OTG_WKUP_HANDLER(void) +{ + exint_flag_clear(OTG_WKUP_EXINT_LINE); +} + +#endif + +uint32_t CDC_Send_FreeBytes(void) +{ + uint32_t freeBytes; + + ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) { + freeBytes = ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1; + } + + return freeBytes; +} + +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) +{ + for (uint32_t i = 0; i < sendLength; i++) { + while (CDC_Send_FreeBytes() == 0) { + delay(1); + } + ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) { + UserTxBuffer[UserTxBufPtrIn] = ptrBuffer[i]; + UserTxBufPtrIn = (UserTxBufPtrIn + 1) % APP_TX_DATA_SIZE; + } + } + return sendLength; +} + +void TxTimerConfig(void) +{ + tmr_base_init(usbTxTmr, (CDC_POLLING_INTERVAL - 1), ((system_core_clock)/1000 - 1)); + tmr_clock_source_div_set(usbTxTmr, TMR_CLOCK_DIV1); + tmr_cnt_dir_set(usbTxTmr, TMR_COUNT_UP); + tmr_period_buffer_enable(usbTxTmr, TRUE); + tmr_interrupt_enable(usbTxTmr, TMR_OVF_INT, TRUE); + nvic_irq_enable(TMR20_OVF_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_USB), NVIC_PRIORITY_SUB(NVIC_PRIO_USB)); + + tmr_counter_enable(usbTxTmr,TRUE); +} + +void TMR20_OVF_IRQHandler(void) +{ + uint32_t buffsize; + static uint32_t lastBuffsize = 0; + + cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata; + + if (pcdc->g_tx_completed == 1) { + if (lastBuffsize) { + bool needZeroLengthPacket = lastBuffsize % 64 == 0; + + UserTxBufPtrOut += lastBuffsize; + if (UserTxBufPtrOut == APP_TX_DATA_SIZE) { + UserTxBufPtrOut = 0; + } + lastBuffsize = 0; + + if (needZeroLengthPacket) { + usb_vcp_send_data(&otg_core_struct.dev, (uint8_t*)&UserTxBuffer[UserTxBufPtrOut], 0); + return; + } + } + if (UserTxBufPtrOut != UserTxBufPtrIn) { + if (UserTxBufPtrOut > UserTxBufPtrIn) { + buffsize = APP_TX_DATA_SIZE - UserTxBufPtrOut; + } else { + buffsize = UserTxBufPtrIn - UserTxBufPtrOut; + } + if (buffsize > APP_TX_BLOCK_SIZE) { + buffsize = APP_TX_BLOCK_SIZE; + } + + uint32_t txed = usb_vcp_send_data(&otg_core_struct.dev,(uint8_t*)&UserTxBuffer[UserTxBufPtrOut], buffsize); + if (txed == SUCCESS) { + lastBuffsize = buffsize; + } + } + } + tmr_flag_clear(usbTxTmr, TMR_OVF_FLAG); +} + +uint8_t usbIsConnected(void) +{ + return (USB_CONN_STATE_DEFAULT != otg_core_struct.dev.conn_state); +} + +uint8_t usbIsConfigured(void) +{ + return (USB_CONN_STATE_CONFIGURED == otg_core_struct.dev.conn_state); +} + +uint8_t usbVcpIsConnected(void) +{ + return usbIsConnected(); +} + +void OTG_IRQ_HANDLER(void) +{ + usbd_irq_handler(&otg_core_struct); +} + +static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) +{ + UNUSED(instance); + UNUSED(baudRate); +} + +static void usbVcpSetMode(serialPort_t *instance, portMode_e mode) +{ + UNUSED(instance); + UNUSED(mode); +} + +static void usbVcpSetCtrlLineStateCb(serialPort_t *instance, void (*cb)(void *context, uint16_t ctrlLineState), void *context) +{ + UNUSED(instance); + CDC_SetCtrlLineStateCb((void (*)(void *context, uint16_t ctrlLineState))cb, context); +} + +static void usbVcpSetBaudRateCb(serialPort_t *instance, void (*cb)(serialPort_t *context, uint32_t baud), serialPort_t *context) +{ + UNUSED(instance); + CDC_SetBaudRateCb((void (*)(void *context, uint32_t baud))cb, (void *)context); +} + +static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +static uint32_t usbVcpAvailable(const serialPort_t *instance) +{ + UNUSED(instance); + + uint32_t available=0; + + available=APP_Rx_ptr_in-APP_Rx_ptr_out; + if(available == 0){ + cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata; + if(pcdc->g_rx_completed == 1){ + available=pcdc->g_rxlen; + } + } + return available; +} + +static uint8_t usbVcpRead(serialPort_t *instance) +{ + UNUSED(instance); + + if ((APP_Rx_ptr_in == 0) || (APP_Rx_ptr_out == APP_Rx_ptr_in)){ + APP_Rx_ptr_out = 0; + APP_Rx_ptr_in = usb_vcp_get_rxdata(&otg_core_struct.dev, APP_Rx_Buffer); + if(APP_Rx_ptr_in == 0) { + return 0; + } + } + return APP_Rx_Buffer[APP_Rx_ptr_out++]; +} + +static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count) +{ + UNUSED(instance); + + if (!(usbIsConnected() && usbIsConfigured())) { + return; + } + + uint32_t start = millis(); + const uint8_t *p = data; + while (count > 0) { + uint32_t txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; + + if (millis() - start > USB_TIMEOUT) { + break; + } + } +} + +static bool usbVcpFlush(vcpPort_t *port) +{ + uint32_t count = port->txAt; + port->txAt = 0; + + if (count == 0) { + return true; + } + + if (!usbIsConnected() || !usbIsConfigured()) { + return false; + } + + uint32_t start = millis(); + uint8_t *p = port->txBuf; + while (count > 0) { + uint32_t txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; + + if (millis() - start > USB_TIMEOUT) { + break; + } + } + return count == 0; +} +static void usbVcpWrite(serialPort_t *instance, uint8_t c) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + + port->txBuf[port->txAt++] = c; + if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) { + usbVcpFlush(port); + } +} + +static void usbVcpBeginWrite(serialPort_t *instance) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + port->buffering = true; +} + +static uint32_t usbTxBytesFree(const serialPort_t *instance) +{ + UNUSED(instance); + return CDC_Send_FreeBytes(); +} + +static void usbVcpEndWrite(serialPort_t *instance) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + port->buffering = false; + usbVcpFlush(port); +} + +static const struct serialPortVTable usbVTable[] = { + { + .serialWrite = usbVcpWrite, + .serialTotalRxWaiting = usbVcpAvailable, + .serialTotalTxFree = usbTxBytesFree, + .serialRead = usbVcpRead, + .serialSetBaudRate = usbVcpSetBaudRate, + .isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty, + .setMode = usbVcpSetMode, + .setCtrlLineStateCb = usbVcpSetCtrlLineStateCb, + .setBaudRateCb = usbVcpSetBaudRateCb, + .writeBuf = usbVcpWriteBuf, + .beginWrite = usbVcpBeginWrite, + .endWrite = usbVcpEndWrite + } +}; + +serialPort_t *usbVcpOpen(void) +{ + vcpPort_t *s; + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + usb_gpio_config(); + +#ifdef USB_LOW_POWER_WAKUP + usb_low_power_wakeup_config(); +#endif + + crm_periph_clock_enable(OTG_CLOCK, TRUE); + usb_clock48m_select(USB_CLK_HEXT); + nvic_irq_enable(OTG_IRQ, NVIC_PRIORITY_BASE(NVIC_PRIO_USB), NVIC_PRIORITY_SUB(NVIC_PRIO_USB)); + + usbGenerateDisconnectPulse(); + + usbd_init(&otg_core_struct, + USB_FULL_SPEED_CORE_ID, + USB_ID, + &cdc_class_handler, + &cdc_desc_handler); + s = &vcpPort; + s->port.vTable = usbVTable; + + TxTimerConfig(); + + return (serialPort_t *)s; +} + +uint32_t usbVcpGetBaudRate(serialPort_t *instance) +{ + UNUSED(instance); + cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata; + return pcdc->linecoding.bitrate; +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/at32/usb/cdc_class.c b/src/main/drivers/at32/usb/cdc_class.c new file mode 100644 index 0000000000..f903bbc173 --- /dev/null +++ b/src/main/drivers/at32/usb/cdc_class.c @@ -0,0 +1,443 @@ +/** + ************************************************************************** + * @file cdc_class.c + * @brief usb cdc class type + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ +#include "platform.h" + +#include "usbd_core.h" +#include "cdc_class.h" +#include "cdc_desc.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_class + * @{ + */ + +/** @defgroup USB_cdc_class + * @brief usb device class cdc demo + * @{ + */ + +/** @defgroup USB_cdc_class_private_functions + * @{ + */ + +static usb_sts_type class_init_handler(void *udev); +static usb_sts_type class_clear_handler(void *udev); +static usb_sts_type class_setup_handler(void *udev, usb_setup_type *setup); +static usb_sts_type class_ept0_tx_handler(void *udev); +static usb_sts_type class_ept0_rx_handler(void *udev); +static usb_sts_type class_in_handler(void *udev, uint8_t ept_num); +static usb_sts_type class_out_handler(void *udev, uint8_t ept_num); +static usb_sts_type class_sof_handler(void *udev); +static usb_sts_type class_event_handler(void *udev, usbd_event_type event); + +static usb_sts_type cdc_struct_init(cdc_struct_type *pcdc); +extern void usb_usart_config( linecoding_type linecoding); +static void usb_vcp_cmd_process(void *udev, uint8_t cmd, uint8_t *buff, uint16_t len); + +linecoding_type linecoding = +{ + 115200, + 0, + 0, + 8 +}; + +/* cdc data struct */ +cdc_struct_type cdc_struct; + +/* usb device class handler */ +usbd_class_handler cdc_class_handler = +{ + class_init_handler, + class_clear_handler, + class_setup_handler, + class_ept0_tx_handler, + class_ept0_rx_handler, + class_in_handler, + class_out_handler, + class_sof_handler, + class_event_handler, + &cdc_struct +}; +/** + * @brief initialize usb custom hid endpoint + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type class_init_handler(void *udev) +{ + usb_sts_type status = USB_OK; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + + /* init cdc struct */ + cdc_struct_init(pcdc); + + /* open in endpoint */ + usbd_ept_open(pudev, USBD_CDC_INT_EPT, EPT_INT_TYPE, USBD_CDC_CMD_MAXPACKET_SIZE); + + /* open in endpoint */ + usbd_ept_open(pudev, USBD_CDC_BULK_IN_EPT, EPT_BULK_TYPE, USBD_CDC_IN_MAXPACKET_SIZE); + + /* open out endpoint */ + usbd_ept_open(pudev, USBD_CDC_BULK_OUT_EPT, EPT_BULK_TYPE, USBD_CDC_OUT_MAXPACKET_SIZE); + + /* set out endpoint to receive status */ + usbd_ept_recv(pudev, USBD_CDC_BULK_OUT_EPT, pcdc->g_rx_buff, USBD_CDC_OUT_MAXPACKET_SIZE); + + return status; +} + +/** + * @brief clear endpoint or other state + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type class_clear_handler(void *udev) +{ + usb_sts_type status = USB_OK; + usbd_core_type *pudev = (usbd_core_type *)udev; + + /* close in endpoint */ + usbd_ept_close(pudev, USBD_CDC_INT_EPT); + + /* close in endpoint */ + usbd_ept_close(pudev, USBD_CDC_BULK_IN_EPT); + + /* close out endpoint */ + usbd_ept_close(pudev, USBD_CDC_BULK_OUT_EPT); + + return status; +} + +/** + * @brief usb device class setup request handler + * @param udev: to the structure of usbd_core_type + * @param setup: setup packet + * @retval status of usb_sts_type + */ +static usb_sts_type class_setup_handler(void *udev, usb_setup_type *setup) +{ + usb_sts_type status = USB_OK; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + + switch(setup->bmRequestType & USB_REQ_TYPE_RESERVED) + { + /* class request */ + case USB_REQ_TYPE_CLASS: + if(setup->wLength) + { + if(setup->bmRequestType & USB_REQ_DIR_DTH) + { + usb_vcp_cmd_process(udev, setup->bRequest, pcdc->g_cmd, setup->wLength); + usbd_ctrl_send(pudev, pcdc->g_cmd, setup->wLength); + } + else + { + pcdc->g_req = setup->bRequest; + pcdc->g_len = setup->wLength; + usbd_ctrl_recv(pudev, pcdc->g_cmd, pcdc->g_len); + + } + } + break; + /* standard request */ + case USB_REQ_TYPE_STANDARD: + switch(setup->bRequest) + { + case USB_STD_REQ_GET_DESCRIPTOR: + usbd_ctrl_unsupport(pudev); + break; + case USB_STD_REQ_GET_INTERFACE: + usbd_ctrl_send(pudev, (uint8_t *)&pcdc->alt_setting, 1); + break; + case USB_STD_REQ_SET_INTERFACE: + pcdc->alt_setting = setup->wValue; + break; + default: + break; + } + break; + default: + usbd_ctrl_unsupport(pudev); + break; + } + return status; +} + +/** + * @brief usb device endpoint 0 in status stage complete + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type class_ept0_tx_handler(void *udev) +{ + UNUSED(udev); + usb_sts_type status = USB_OK; + + /* ...user code... */ + + return status; +} + +/** + * @brief usb device endpoint 0 out status stage complete + * @param udev: usb device core handler type + * @retval status of usb_sts_type + */ +static usb_sts_type class_ept0_rx_handler(void *udev) +{ + usb_sts_type status = USB_OK; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + uint32_t recv_len = usbd_get_recv_len(pudev, 0); + /* ...user code... */ + if( pcdc->g_req == SET_LINE_CODING) + { + /* class process */ + usb_vcp_cmd_process(udev, pcdc->g_req, pcdc->g_cmd, recv_len); + } + + return status; +} + +/** + * @brief usb device transmision complete handler + * @param udev: to the structure of usbd_core_type + * @param ept_num: endpoint number + * @retval status of usb_sts_type + */ +static usb_sts_type class_in_handler(void *udev, uint8_t ept_num) +{ + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + usb_sts_type status = USB_OK; + + /* ...user code... + trans next packet data + */ + usbd_flush_tx_fifo(pudev, ept_num); + pcdc->g_tx_completed = 1; + + return status; +} + +/** + * @brief usb device endpoint receive data + * @param udev: to the structure of usbd_core_type + * @param ept_num: endpoint number + * @retval status of usb_sts_type + */ +static usb_sts_type class_out_handler(void *udev, uint8_t ept_num) +{ + usb_sts_type status = USB_OK; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + + /* get endpoint receive data length */ + pcdc->g_rxlen = usbd_get_recv_len(pudev, ept_num); + + /*set recv flag*/ + pcdc->g_rx_completed = 1; + + return status; +} + +/** + * @brief usb device sof handler + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type class_sof_handler(void *udev) +{ + UNUSED(udev); + usb_sts_type status = USB_OK; + + /* ...user code... */ + + return status; +} + +/** + * @brief usb device event handler + * @param udev: to the structure of usbd_core_type + * @param event: usb device event + * @retval status of usb_sts_type + */ +static usb_sts_type class_event_handler(void *udev, usbd_event_type event) +{ + UNUSED(udev); + + usb_sts_type status = USB_OK; + switch(event) + { + case USBD_RESET_EVENT: + + /* ...user code... */ + + break; + case USBD_SUSPEND_EVENT: + + /* ...user code... */ + + break; + case USBD_WAKEUP_EVENT: + /* ...user code... */ + + break; + case USBD_INISOINCOM_EVENT: + break; + case USBD_OUTISOINCOM_EVENT: + break; + + default: + break; + } + return status; +} + +/** + * @brief usb device cdc init + * @param pcdc: to the structure of cdc_struct + * @retval status of usb_sts_type + */ +static usb_sts_type cdc_struct_init(cdc_struct_type *pcdc) +{ + pcdc->g_tx_completed = 1; + pcdc->g_rx_completed = 0; + pcdc->alt_setting = 0; + pcdc->linecoding.bitrate = linecoding.bitrate; + pcdc->linecoding.data = linecoding.data; + pcdc->linecoding.format = linecoding.format; + pcdc->linecoding.parity = linecoding.parity; + return USB_OK; +} + +/** + * @brief usb device class rx data process + * @param udev: to the structure of usbd_core_type + * @param recv_data: receive buffer + * @retval receive data len + */ +uint16_t usb_vcp_get_rxdata(void *udev, uint8_t *recv_data) +{ + uint16_t i_index = 0; + uint16_t tmp_len = 0; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + + if(pcdc->g_rx_completed == 0) + { + return 0; + } + pcdc->g_rx_completed = 0; + tmp_len = pcdc->g_rxlen; + for(i_index = 0; i_index < pcdc->g_rxlen; i_index ++) + { + recv_data[i_index] = pcdc->g_rx_buff[i_index]; + } + + usbd_ept_recv(pudev, USBD_CDC_BULK_OUT_EPT, pcdc->g_rx_buff, USBD_CDC_OUT_MAXPACKET_SIZE); + + return tmp_len; +} + +/** + * @brief usb device class send data + * @param udev: to the structure of usbd_core_type + * @param send_data: send data buffer + * @param len: send length + * @retval error status + */ +error_status usb_vcp_send_data(void *udev, uint8_t *send_data, uint16_t len) +{ + error_status status = SUCCESS; + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + if(pcdc->g_tx_completed) + { + pcdc->g_tx_completed = 0; + usbd_ept_send(pudev, USBD_CDC_BULK_IN_EPT, send_data, len); + } + else + { + status = ERROR; + } + return status; +} + +/** + * @brief usb device function + * @param udev: to the structure of usbd_core_type + * @param cmd: request number + * @param buff: request buffer + * @param len: buffer length + * @retval none + */ +static void usb_vcp_cmd_process(void *udev, uint8_t cmd, uint8_t *buff, uint16_t len) +{ + UNUSED(len); + + usbd_core_type *pudev = (usbd_core_type *)udev; + cdc_struct_type *pcdc = (cdc_struct_type *)pudev->class_handler->pdata; + switch(cmd) + { + case SET_LINE_CODING: + pcdc->linecoding.bitrate = (uint32_t)(buff[0] | (buff[1] << 8) | (buff[2] << 16) | (buff[3] <<24)); + pcdc->linecoding.format = buff[4]; + pcdc->linecoding.parity = buff[5]; + pcdc->linecoding.data = buff[6]; +#ifdef USB_VIRTUAL_COMPORT + /* set hardware usart */ + usb_usart_config(pcdc->linecoding); +#endif + break; + + case GET_LINE_CODING: + buff[0] = (uint8_t)pcdc->linecoding.bitrate; + buff[1] = (uint8_t)(pcdc->linecoding.bitrate >> 8); + buff[2] = (uint8_t)(pcdc->linecoding.bitrate >> 16); + buff[3] = (uint8_t)(pcdc->linecoding.bitrate >> 24); + buff[4] = (uint8_t)(pcdc->linecoding.format); + buff[5] = (uint8_t)(pcdc->linecoding.parity); + buff[6] = (uint8_t)(pcdc->linecoding.data); + break; + + default: + break; + } +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/src/main/drivers/at32/usb/cdc_class.h b/src/main/drivers/at32/usb/cdc_class.h new file mode 100644 index 0000000000..f781519b88 --- /dev/null +++ b/src/main/drivers/at32/usb/cdc_class.h @@ -0,0 +1,115 @@ +/** + ************************************************************************** + * @file cdc_class.h + * @brief usb cdc class file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + + /* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __CDC_CLASS_H +#define __CDC_CLASS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb_std.h" +#include "usbd_core.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_class + * @{ + */ + +/** @addtogroup USB_cdc_class + * @{ + */ + +/** @defgroup USB_cdc_class_definition + * @{ + */ + +/** + * @brief usb cdc use endpoint define + */ +#define USBD_CDC_INT_EPT 0x82 +#define USBD_CDC_BULK_IN_EPT 0x81 +#define USBD_CDC_BULK_OUT_EPT 0x01 + +/** + * @brief usb cdc in and out max packet size define + */ +#define USBD_CDC_IN_MAXPACKET_SIZE 0x40 +#define USBD_CDC_OUT_MAXPACKET_SIZE 0x40 +#define USBD_CDC_CMD_MAXPACKET_SIZE 0x08 + +/** + * @} + */ + +/** @defgroup USB_cdc_class_exported_types + * @{ + */ + +/** + * @brief usb cdc class struct + */ +typedef struct +{ + uint32_t alt_setting; + uint8_t g_rx_buff[USBD_CDC_OUT_MAXPACKET_SIZE]; + uint8_t g_cmd[USBD_CDC_CMD_MAXPACKET_SIZE]; + uint8_t g_req; + uint16_t g_len, g_rxlen; + __IO uint8_t g_tx_completed, g_rx_completed; + linecoding_type linecoding; +}cdc_struct_type; + + +/** + * @} + */ + +/** @defgroup USB_cdc_class_exported_functions + * @{ + */ +extern usbd_class_handler cdc_class_handler; +uint16_t usb_vcp_get_rxdata(void *udev, uint8_t *recv_data); +error_status usb_vcp_send_data(void *udev, uint8_t *send_data, uint16_t len); + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif + + + + diff --git a/src/main/drivers/at32/usb/cdc_desc.c b/src/main/drivers/at32/usb/cdc_desc.c new file mode 100644 index 0000000000..b312094407 --- /dev/null +++ b/src/main/drivers/at32/usb/cdc_desc.c @@ -0,0 +1,447 @@ +/** + ************************************************************************** + * @file cdc_desc.c + * @brief usb cdc device descriptor + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ +#include "stdio.h" + +#include "platform.h" + +#include "usb_std.h" +#include "usbd_sdr.h" +#include "usbd_core.h" +#include "cdc_desc.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_class + * @{ + */ + +/** @defgroup USB_cdc_desc + * @brief usb device cdc descriptor + * @{ + */ + +/** @defgroup USB_cdc_desc_private_functions + * @{ + */ + +static usbd_desc_t *get_device_descriptor(void); +static usbd_desc_t *get_device_qualifier(void); +static usbd_desc_t *get_device_configuration(void); +static usbd_desc_t *get_device_other_speed(void); +static usbd_desc_t *get_device_lang_id(void); +static usbd_desc_t *get_device_manufacturer_string(void); +static usbd_desc_t *get_device_product_string(void); +static usbd_desc_t *get_device_serial_string(void); +static usbd_desc_t *get_device_interface_string(void); +static usbd_desc_t *get_device_config_string(void); + +static uint16_t usbd_unicode_convert(uint8_t *string, uint8_t *unicode_buf); +static void usbd_int_to_unicode (uint32_t value , uint8_t *pbuf , uint8_t len); +static void get_serial_num(void); +static uint8_t g_usbd_desc_buffer[256]; + +/** + * @brief device descriptor handler structure + */ +usbd_desc_handler cdc_desc_handler = +{ + get_device_descriptor, + get_device_qualifier, + get_device_configuration, + get_device_other_speed, + get_device_lang_id, + get_device_manufacturer_string, + get_device_product_string, + get_device_serial_string, + get_device_interface_string, + get_device_config_string, +}; + +/** + * @brief usb device standard descriptor + */ +#if defined ( __ICCARM__ ) /* iar compiler */ + #pragma data_alignment=4 +#endif +ALIGNED_HEAD static uint8_t g_usbd_descriptor[USB_DEVICE_DESC_LEN] ALIGNED_TAIL = +{ + USB_DEVICE_DESC_LEN, /* bLength */ + USB_DESCIPTOR_TYPE_DEVICE, /* bDescriptorType */ + 0x00, /* bcdUSB */ + 0x02, + 0x02, /* bDeviceClass */ + 0x00, /* bDeviceSubClass */ + 0x00, /* bDeviceProtocol */ + USB_MAX_EP0_SIZE, /* bMaxPacketSize */ + LBYTE(USBD_CDC_VENDOR_ID), /* idVendor */ + HBYTE(USBD_CDC_VENDOR_ID), /* idVendor */ + LBYTE(USBD_CDC_PRODUCT_ID), /* idProduct */ + HBYTE(USBD_CDC_PRODUCT_ID), /* idProduct */ + 0x00, /* bcdDevice rel. 2.00 */ + 0x02, + USB_MFC_STRING, /* Index of manufacturer string */ + USB_PRODUCT_STRING, /* Index of product string */ + USB_SERIAL_STRING, /* Index of serial number string */ + 1 /* bNumConfigurations */ +}; + +/** + * @brief usb configuration standard descriptor + */ +#if defined ( __ICCARM__ ) /* iar compiler */ + #pragma data_alignment=4 +#endif +ALIGNED_HEAD static uint8_t g_usbd_configuration[USBD_CDC_CONFIG_DESC_SIZE] ALIGNED_TAIL = +{ + USB_DEVICE_CFG_DESC_LEN, /* bLength: configuration descriptor size */ + USB_DESCIPTOR_TYPE_CONFIGURATION, /* bDescriptorType: configuration */ + LBYTE(USBD_CDC_CONFIG_DESC_SIZE), /* wTotalLength: bytes returned */ + HBYTE(USBD_CDC_CONFIG_DESC_SIZE), /* wTotalLength: bytes returned */ + 0x02, /* bNumInterfaces: 2 interface */ + 0x01, /* bConfigurationValue: configuration value */ + 0x00, /* iConfiguration: index of string descriptor describing + the configuration */ + 0xC0, /* bmAttributes: self powered */ + 0x32, /* MaxPower 100 mA: this current is used for detecting vbus */ + + USB_DEVICE_IF_DESC_LEN, /* bLength: interface descriptor size */ + USB_DESCIPTOR_TYPE_INTERFACE, /* bDescriptorType: interface descriptor type */ + 0x00, /* bInterfaceNumber: number of interface */ + 0x00, /* bAlternateSetting: alternate set */ + 0x01, /* bNumEndpoints: number of endpoints */ + USB_CLASS_CODE_CDC, /* bInterfaceClass: CDC class code */ + 0x02, /* bInterfaceSubClass: subclass code, Abstract Control Model*/ + 0x01, /* bInterfaceProtocol: protocol code, AT Command */ + 0x00, /* iInterface: index of string descriptor */ + + 0x05, /* bFunctionLength: size of this descriptor in bytes */ + USBD_CDC_CS_INTERFACE, /* bDescriptorType: CDC interface descriptor type */ + USBD_CDC_SUBTYPE_HEADER, /* bDescriptorSubtype: Header function Descriptor 0x00*/ + LBYTE(CDC_BCD_NUM), + HBYTE(CDC_BCD_NUM), /* bcdCDC: USB class definitions for communications */ + + 0x05, /* bFunctionLength: size of this descriptor in bytes */ + USBD_CDC_CS_INTERFACE, /* bDescriptorType: CDC interface descriptor type */ + USBD_CDC_SUBTYPE_CMF, /* bDescriptorSubtype: Call Management function descriptor subtype 0x01 */ + 0x00, /* bmCapabilities: 0x00*/ + 0x01, /* bDataInterface: interface number of data class interface optionally used for call management */ + + 0x04, /* bFunctionLength: size of this descriptor in bytes */ + USBD_CDC_CS_INTERFACE, /* bDescriptorType: CDC interface descriptor type */ + USBD_CDC_SUBTYPE_ACM, /* bDescriptorSubtype: Abstract Control Management functional descriptor subtype 0x02 */ + 0x02, /* bmCapabilities: Support Set_Line_Coding and Get_Line_Coding 0x02 */ + + 0x05, /* bFunctionLength: size of this descriptor in bytes */ + USBD_CDC_CS_INTERFACE, /* bDescriptorType: CDC interface descriptor type */ + USBD_CDC_SUBTYPE_UFD, /* bDescriptorSubtype: Union Function Descriptor subtype 0x06 */ + 0x00, /* bControlInterface: The interface number of the communications or data class interface 0x00 */ + 0x01, /* bSubordinateInterface0: interface number of first subordinate interface in the union */ + + USB_DEVICE_EPT_LEN, /* bLength: size of endpoint descriptor in bytes */ + USB_DESCIPTOR_TYPE_ENDPOINT, /* bDescriptorType: endpoint descriptor type */ + USBD_CDC_INT_EPT, /* bEndpointAddress: the address of endpoint on usb device described by this descriptor */ + USB_EPT_DESC_INTERRUPT, /* bmAttributes: endpoint attributes */ + LBYTE(USBD_CDC_CMD_MAXPACKET_SIZE), + HBYTE(USBD_CDC_CMD_MAXPACKET_SIZE), /* wMaxPacketSize: maximum packe size this endpoint */ + CDC_HID_BINTERVAL_TIME, /* bInterval: interval for polling endpoint for data transfers */ + + + USB_DEVICE_IF_DESC_LEN, /* bLength: interface descriptor size */ + USB_DESCIPTOR_TYPE_INTERFACE, /* bDescriptorType: interface descriptor type */ + 0x01, /* bInterfaceNumber: number of interface */ + 0x00, /* bAlternateSetting: alternate set */ + 0x02, /* bNumEndpoints: number of endpoints */ + USB_CLASS_CODE_CDCDATA, /* bInterfaceClass: CDC-data class code */ + 0x00, /* bInterfaceSubClass: Data interface subclass code 0x00*/ + 0x00, /* bInterfaceProtocol: data class protocol code 0x00 */ + 0x00, /* iInterface: index of string descriptor */ + + USB_DEVICE_EPT_LEN, /* bLength: size of endpoint descriptor in bytes */ + USB_DESCIPTOR_TYPE_ENDPOINT, /* bDescriptorType: endpoint descriptor type */ + USBD_CDC_BULK_IN_EPT, /* bEndpointAddress: the address of endpoint on usb device described by this descriptor */ + USB_EPT_DESC_BULK, /* bmAttributes: endpoint attributes */ + LBYTE(USBD_CDC_IN_MAXPACKET_SIZE), + HBYTE(USBD_CDC_IN_MAXPACKET_SIZE), /* wMaxPacketSize: maximum packe size this endpoint */ + 0x00, /* bInterval: interval for polling endpoint for data transfers */ + + USB_DEVICE_EPT_LEN, /* bLength: size of endpoint descriptor in bytes */ + USB_DESCIPTOR_TYPE_ENDPOINT, /* bDescriptorType: endpoint descriptor type */ + USBD_CDC_BULK_OUT_EPT, /* bEndpointAddress: the address of endpoint on usb device described by this descriptor */ + USB_EPT_DESC_BULK, /* bmAttributes: endpoint attributes */ + LBYTE(USBD_CDC_OUT_MAXPACKET_SIZE), + HBYTE(USBD_CDC_OUT_MAXPACKET_SIZE), /* wMaxPacketSize: maximum packe size this endpoint */ + 0x00, /* bInterval: interval for polling endpoint for data transfers */ +}; + +/** + * @brief usb string lang id + */ +#if defined ( __ICCARM__ ) /* iar compiler */ + #pragma data_alignment=4 +#endif +ALIGNED_HEAD static uint8_t g_string_lang_id[USBD_CDC_SIZ_STRING_LANGID] ALIGNED_TAIL = +{ + USBD_CDC_SIZ_STRING_LANGID, + USB_DESCIPTOR_TYPE_STRING, + 0x09, + 0x04, +}; + +/** + * @brief usb string serial + */ +#if defined ( __ICCARM__ ) /* iar compiler */ + #pragma data_alignment=4 +#endif +ALIGNED_HEAD static uint8_t g_string_serial[USBD_CDC_SIZ_STRING_SERIAL] ALIGNED_TAIL = +{ + USBD_CDC_SIZ_STRING_SERIAL, + USB_DESCIPTOR_TYPE_STRING, +}; + + +/* device descriptor */ +static usbd_desc_t device_descriptor = +{ + USB_DEVICE_DESC_LEN, + g_usbd_descriptor +}; + +/* config descriptor */ +static usbd_desc_t config_descriptor = +{ + USBD_CDC_CONFIG_DESC_SIZE, + g_usbd_configuration +}; + +/* langid descriptor */ +static usbd_desc_t langid_descriptor = +{ + USBD_CDC_SIZ_STRING_LANGID, + g_string_lang_id +}; + +/* serial descriptor */ +static usbd_desc_t serial_descriptor = +{ + USBD_CDC_SIZ_STRING_SERIAL, + g_string_serial +}; + +static usbd_desc_t vp_desc; + +/** + * @brief standard usb unicode convert + * @param string: source string + * @param unicode_buf: unicode buffer + * @retval length + */ +static uint16_t usbd_unicode_convert(uint8_t *string, uint8_t *unicode_buf) +{ + uint16_t str_len = 0, id_pos = 2; + uint8_t *tmp_str = string; + + while(*tmp_str != '\0') + { + str_len ++; + unicode_buf[id_pos ++] = *tmp_str ++; + unicode_buf[id_pos ++] = 0x00; + } + + str_len = str_len * 2 + 2; + unicode_buf[0] = (uint8_t)str_len; + unicode_buf[1] = USB_DESCIPTOR_TYPE_STRING; + + return str_len; +} + +/** + * @brief usb int convert to unicode + * @param value: int value + * @param pbus: unicode buffer + * @param len: length + * @retval none + */ +static void usbd_int_to_unicode (uint32_t value , uint8_t *pbuf , uint8_t len) +{ + uint8_t idx = 0; + + for( idx = 0 ; idx < len ; idx ++) + { + if( ((value >> 28)) < 0xA ) + { + pbuf[ 2 * idx] = (value >> 28) + '0'; + } + else + { + pbuf[2 * idx] = (value >> 28) + 'A' - 10; + } + + value = value << 4; + + pbuf[2 * idx + 1] = 0; + } +} + +/** + * @brief usb get serial number + * @param none + * @retval none + */ +static void get_serial_num(void) +{ + uint32_t serial0, serial1, serial2; + + serial0 = *(uint32_t*)MCU_ID1; + serial1 = *(uint32_t*)MCU_ID2; + serial2 = *(uint32_t*)MCU_ID3; + + serial0 += serial2; + + if (serial0 != 0) + { + usbd_int_to_unicode (serial0, &g_string_serial[2] ,8); + usbd_int_to_unicode (serial1, &g_string_serial[18] ,4); + } +} + +/** + * @brief get device descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_descriptor(void) +{ + return &device_descriptor; +} + +/** + * @brief get device qualifier + * @param none + * @retval usbd_desc + */ +static usbd_desc_t * get_device_qualifier(void) +{ + return NULL; +} + +/** + * @brief get config descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_configuration(void) +{ + return &config_descriptor; +} + +/** + * @brief get other speed descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_other_speed(void) +{ + return NULL; +} + +/** + * @brief get lang id descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_lang_id(void) +{ + return &langid_descriptor; +} + + +/** + * @brief get manufacturer descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_manufacturer_string(void) +{ + vp_desc.length = usbd_unicode_convert((uint8_t *)USBD_CDC_DESC_MANUFACTURER_STRING, g_usbd_desc_buffer); + vp_desc.descriptor = g_usbd_desc_buffer; + return &vp_desc; +} + +/** + * @brief get product descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_product_string(void) +{ + vp_desc.length = usbd_unicode_convert((uint8_t *)USBD_CDC_DESC_PRODUCT_STRING, g_usbd_desc_buffer); + vp_desc.descriptor = g_usbd_desc_buffer; + return &vp_desc; +} + +/** + * @brief get serial descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_serial_string(void) +{ + get_serial_num(); + return &serial_descriptor; +} + +/** + * @brief get interface descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_interface_string(void) +{ + vp_desc.length = usbd_unicode_convert((uint8_t *)USBD_CDC_DESC_INTERFACE_STRING, g_usbd_desc_buffer); + vp_desc.descriptor = g_usbd_desc_buffer; + return &vp_desc; +} + +/** + * @brief get device config descriptor + * @param none + * @retval usbd_desc + */ +static usbd_desc_t *get_device_config_string(void) +{ + vp_desc.length = usbd_unicode_convert((uint8_t *)USBD_CDC_DESC_CONFIGURATION_STRING, g_usbd_desc_buffer); + vp_desc.descriptor = g_usbd_desc_buffer; + return &vp_desc; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/src/main/drivers/at32/usb/cdc_desc.h b/src/main/drivers/at32/usb/cdc_desc.h new file mode 100644 index 0000000000..f5f80cb78f --- /dev/null +++ b/src/main/drivers/at32/usb/cdc_desc.h @@ -0,0 +1,102 @@ +/** + ************************************************************************** + * @file cdc_desc.h + * @brief usb cdc descriptor header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __CDC_DESC_H +#define __CDC_DESC_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "cdc_class.h" +#include "usbd_core.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_class + * @{ + */ + +/** @addtogroup USB_cdc_desc + * @{ + */ + +/** @defgroup USB_cdc_desc_definition + * @{ + */ +/** + * @brief usb bcd number define + */ +#define CDC_BCD_NUM 0x0110 + +/** + * @brief usb vendor id and product id define + */ +#define USBD_CDC_VENDOR_ID 0x2E3C +#define USBD_CDC_PRODUCT_ID 0x5740 + +/** + * @brief usb descriptor size define + */ +#define USBD_CDC_CONFIG_DESC_SIZE 67 +#define USBD_CDC_SIZ_STRING_LANGID 4 +#define USBD_CDC_SIZ_STRING_SERIAL 0x1A + +/** + * @brief usb string define(vendor, product configuration, interface) + */ +#define USBD_CDC_DESC_MANUFACTURER_STRING "Artery" +#define USBD_CDC_DESC_PRODUCT_STRING "AT32 Virtual Com Port " +#define USBD_CDC_DESC_CONFIGURATION_STRING "Virtual ComPort Config" +#define USBD_CDC_DESC_INTERFACE_STRING "Virtual ComPort Interface" + +/** + * @brief usb endpoint interval define + */ +#define CDC_HID_BINTERVAL_TIME 0xFF + +/** + * @brief usb mcu id address deine + */ +#define MCU_ID1 (0x1FFFF7E8) +#define MCU_ID2 (0x1FFFF7EC) +#define MCU_ID3 (0x1FFFF7F0) +/** + * @} + */ + +extern usbd_desc_handler cdc_desc_handler; + + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usb_conf.h b/src/main/drivers/at32/usb/usb_conf.h new file mode 100644 index 0000000000..40d346243b --- /dev/null +++ b/src/main/drivers/at32/usb/usb_conf.h @@ -0,0 +1,218 @@ +/** + ************************************************************************** + * @file usb_conf.h + * @version v2.0.5 + * @date 2022-02-11 + * @brief usb config header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_CONF_H +#define __USB_CONF_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "at32f435_437_usb.h" +#include "at32f435_437.h" +//#include "stdio.h" + +/** @addtogroup AT32F437_periph_examples + * @{ + */ + +/** @addtogroup 437_USB_device_vcp_loopback + * @{ + */ + +/** + * @brief enable usb device mode + */ +#define USE_OTG_DEVICE_MODE + +/** + * @brief enable usb host mode + */ +/* #define USE_OTG_HOST_MODE */ + +/** + * @brief select otgfs1 or otgfs2 define + */ + +/* use otgfs1 */ +#define OTG_USB_ID 1 + +/* use otgfs2 */ +//#define OTG_USB_ID 2 + +#if (OTG_USB_ID == 1) +#define USB_ID 0 +#define OTG_CLOCK CRM_OTGFS1_PERIPH_CLOCK +#define OTG_IRQ OTGFS1_IRQn +#define OTG_IRQ_HANDLER OTGFS1_IRQHandler +#define OTG_WKUP_IRQ OTGFS1_WKUP_IRQn +#define OTG_WKUP_HANDLER OTGFS1_WKUP_IRQHandler +#define OTG_WKUP_EXINT_LINE EXINT_LINE_18 + +#define OTG_PIN_GPIO GPIOA +#define OTG_PIN_GPIO_CLOCK CRM_GPIOA_PERIPH_CLOCK + +#define OTG_PIN_DP GPIO_PINS_12 +#define OTG_PIN_DP_SOURCE GPIO_PINS_SOURCE12 + +#define OTG_PIN_DM GPIO_PINS_11 +#define OTG_PIN_DM_SOURCE GPIO_PINS_SOURCE11 + +#define OTG_PIN_VBUS GPIO_PINS_9 +#define OTG_PIN_VBUS_SOURCE GPIO_PINS_SOURCE9 + +#define OTG_PIN_ID GPIO_PINS_10 +#define OTG_PIN_ID_SOURCE GPIO_PINS_SOURCE10 + +#define OTG_PIN_SOF_GPIO GPIOA +#define OTG_PIN_SOF_GPIO_CLOCK CRM_GPIOA_PERIPH_CLOCK +#define OTG_PIN_SOF GPIO_PINS_8 +#define OTG_PIN_SOF_SOURCE GPIO_PINS_SOURCE8 + +#define OTG_PIN_MUX GPIO_MUX_10 +#endif + +#if (OTG_USB_ID == 2) +#define USB_ID 1 +#define OTG_CLOCK CRM_OTGFS2_PERIPH_CLOCK +#define OTG_IRQ OTGFS2_IRQn +#define OTG_IRQ_HANDLER OTGFS2_IRQHandler +#define OTG_WKUP_IRQ OTGFS2_WKUP_IRQn +#define OTG_WKUP_HANDLER OTGFS2_WKUP_IRQHandler +#define OTG_WKUP_EXINT_LINE EXINT_LINE_20 + +#define OTG_PIN_GPIO GPIOB +#define OTG_PIN_GPIO_CLOCK CRM_GPIOB_PERIPH_CLOCK + +#define OTG_PIN_DP GPIO_PINS_15 +#define OTG_PIN_DP_SOURCE GPIO_PINS_SOURCE15 + +#define OTG_PIN_DM GPIO_PINS_14 +#define OTG_PIN_DM_SOURCE GPIO_PINS_SOURCE14 + +#define OTG_PIN_VBUS GPIO_PINS_13 +#define OTG_PIN_VBUS_SOURCE GPIO_PINS_SOURCE13 + +#define OTG_PIN_ID GPIO_PINS_12 +#define OTG_PIN_ID_SOURCE GPIO_PINS_SOURCE10 + +#define OTG_PIN_SOF_GPIO GPIOA +#define OTG_PIN_SOF_GPIO_CLOCK CRM_GPIOA_PERIPH_CLOCK +#define OTG_PIN_SOF GPIO_PINS_4 +#define OTG_PIN_SOF_SOURCE GPIO_PINS_SOURCE4 + +#define OTG_PIN_MUX GPIO_MUX_12 +#endif + +/** + * @brief usb device mode config + */ +#ifdef USE_OTG_DEVICE_MODE +/** + * @brief usb device mode fifo + */ +/* otg1 device fifo */ +#define USBD_RX_SIZE 128 +#define USBD_EP0_TX_SIZE 24 +#define USBD_EP1_TX_SIZE 20 +#define USBD_EP2_TX_SIZE 20 +#define USBD_EP3_TX_SIZE 20 +#define USBD_EP4_TX_SIZE 20 +#define USBD_EP5_TX_SIZE 20 +#define USBD_EP6_TX_SIZE 20 +#define USBD_EP7_TX_SIZE 20 + +/* otg2 device fifo */ +#define USBD2_RX_SIZE 128 +#define USBD2_EP0_TX_SIZE 24 +#define USBD2_EP1_TX_SIZE 20 +#define USBD2_EP2_TX_SIZE 20 +#define USBD2_EP3_TX_SIZE 20 +#define USBD2_EP4_TX_SIZE 20 +#define USBD2_EP5_TX_SIZE 20 +#define USBD2_EP6_TX_SIZE 20 +#define USBD2_EP7_TX_SIZE 20 + +/** + * @brief usb endpoint max num define + */ +#ifndef USB_EPT_MAX_NUM +#define USB_EPT_MAX_NUM 8 +#endif +#endif + +/** + * @brief usb host mode config + */ +#ifdef USE_OTG_HOST_MODE +#ifndef USB_HOST_CHANNEL_NUM +#define USB_HOST_CHANNEL_NUM 16 +#endif + +/** + * @brief usb host mode fifo + */ +/* otg1 host fifo */ +#define USBH_RX_FIFO_SIZE 128 +#define USBH_NP_TX_FIFO_SIZE 96 +#define USBH_P_TX_FIFO_SIZE 96 + +/* otg2 host fifo */ +#define USBH2_RX_FIFO_SIZE 128 +#define USBH2_NP_TX_FIFO_SIZE 96 +#define USBH2_P_TX_FIFO_SIZE 96 +#endif + +/** + * @brief usb sof output enable + */ +/* #define USB_SOF_OUTPUT_ENABLE */ + +/** + * @brief usb vbus ignore, not use vbus pin + */ +#define USB_VBUS_IGNORE + +/** + * @brief usb low power wakeup handler enable + */ +/* #define USB_LOW_POWER_WAKUP */ + +void usb_delay_ms(uint32_t ms); +void usb_delay_us(uint32_t us); +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usb_core.c b/src/main/drivers/at32/usb/usb_core.c new file mode 100644 index 0000000000..a1d7fbedb0 --- /dev/null +++ b/src/main/drivers/at32/usb/usb_core.c @@ -0,0 +1,189 @@ +/** + ************************************************************************** + * @file usb_core.c + * @brief usb driver + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usb_core.h" + +/** @addtogroup AT32F435_437_middlewares_usb_drivers + * @{ + */ + +/** @defgroup USB_drivers_core + * @brief usb global drivers core + * @{ + */ + +/** @defgroup USB_core_private_functions + * @{ + */ + +usb_sts_type usb_core_config(otg_core_type *udev, uint8_t core_id); + +/** + * @brief usb core config + * @param otgdev: to the structure of otg_core_type + * @param core_id: usb core id number (USB_FULL_SPEED_CORE_ID) + * @retval usb_sts_type + */ +usb_sts_type usb_core_config(otg_core_type *otgdev, uint8_t core_id) +{ + /* set usb speed and core id */ + otgdev->cfg.speed = core_id; + otgdev->cfg.core_id = core_id; + + /* default sof out and vbus ignore */ + otgdev->cfg.sof_out = FALSE; + otgdev->cfg.vbusig = FALSE; + + /* set max size */ + otgdev->cfg.max_size = 64; + + /* set support number of channel and endpoint */ +#ifdef USE_OTG_HOST_MODE + otgdev->cfg.hc_num = USB_HOST_CHANNEL_NUM; +#endif +#ifdef USE_OTG_DEVICE_MODE + otgdev->cfg.ept_num = USB_EPT_MAX_NUM; +#endif + otgdev->cfg.fifo_size = OTG_FIFO_SIZE; + if(core_id == USB_FULL_SPEED_CORE_ID) + { + otgdev->cfg.phy_itface = 2; + } +#ifdef USB_SOF_OUTPUT_ENABLE + otgdev->cfg.sof_out = TRUE; +#endif + +#ifdef USB_VBUS_IGNORE + otgdev->cfg.vbusig = TRUE; +#endif + + return USB_OK; +} + +#ifdef USE_OTG_DEVICE_MODE +/** + * @brief usb device initialization + * @param otgdev: to the structure of otg_core_type + * @param core_id: usb core id number (USB_FULL_SPEED_CORE_ID) + * @param usb_id: select use OTG1 or OTG2 + * this parameter can be one of the following values: + * - USB_OTG1_ID + * - USB_OTG2_ID + * @param dev_handler: usb class callback handler + * @param desc_handler: device config callback handler + * @retval usb_sts_type + */ +usb_sts_type usbd_init(otg_core_type *otgdev, + uint8_t core_id, uint8_t usb_id, + usbd_class_handler *class_handler, + usbd_desc_handler *desc_handler) +{ + usb_sts_type usb_sts = USB_OK; + + /* select use OTG1 or OTG2 */ + otgdev->usb_reg = usb_global_select_core(usb_id); + + /* usb device core config */ + usb_core_config(otgdev, core_id); + + if(otgdev->cfg.sof_out) + { + otgdev->usb_reg->gccfg_bit.sofouten = TRUE; + } + + if(otgdev->cfg.vbusig) + { + otgdev->usb_reg->gccfg_bit.vbusig = TRUE; + } + + /* usb device core init */ + usbd_core_init(&(otgdev->dev), otgdev->usb_reg, + class_handler, + desc_handler, + core_id); + + return usb_sts; +} +#endif + +#ifdef USE_OTG_HOST_MODE + +/** + * @brief usb host initialization. + * @param otgdev: to the structure of otg_core_type + * @param core_id: usb core id number (USB_FULL_SPEED_CORE_ID) + * @param usb_id: select use OTG1 or OTG2 + * this parameter can be one of the following values: + * - USB_OTG1_ID + * - USB_OTG2_ID + * @param class_handler: usb class callback handler + * @param user_handler: user callback handler + * @retval usb_sts_type + */ +usb_sts_type usbh_init(otg_core_type *otgdev, + uint8_t core_id, uint8_t usb_id, + usbh_class_handler_type *class_handler, + usbh_user_handler_type *user_handler) +{ + usb_sts_type status = USB_OK; + + /* select use otg1 or otg2 */ + otgdev->usb_reg = usb_global_select_core(usb_id); + + /* usb core config */ + usb_core_config(otgdev, core_id); + + if(otgdev->cfg.sof_out) + { + otgdev->usb_reg->gccfg_bit.sofouten = TRUE; + } + + if(otgdev->cfg.vbusig) + { + otgdev->usb_reg->gccfg_bit.vbusig = TRUE; + } + + /* usb host core init */ + usbh_core_init(&otgdev->host, otgdev->usb_reg, + class_handler, + user_handler, + core_id); + + return status; +} +#endif + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/src/main/drivers/at32/usb/usb_core.h b/src/main/drivers/at32/usb/usb_core.h new file mode 100644 index 0000000000..e28f8725c4 --- /dev/null +++ b/src/main/drivers/at32/usb/usb_core.h @@ -0,0 +1,134 @@ +/** + ************************************************************************** + * @file usb_core.h + * @brief usb core header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_CORE_H +#define __USB_CORE_H + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "usb_std.h" +#include "usb_conf.h" + +#ifdef USE_OTG_DEVICE_MODE +#include "usbd_core.h" +#endif +#ifdef USE_OTG_HOST_MODE +#include "usbh_core.h" +#endif + +/** @addtogroup AT32F435_437_middlewares_usb_drivers + * @{ + */ + +/** @addtogroup USB_drivers_core + * @{ + */ + +/** @defgroup USB_core_exported_types + * @{ + */ + +/** + * @brief usb core speed select + */ +typedef enum +{ + USB_LOW_SPEED_CORE_ID, /*!< usb low speed core id */ + USB_FULL_SPEED_CORE_ID, /*!< usb full speed core id */ + USB_HIGH_SPEED_CORE_ID, /*!< usb high speed core id */ +} usb_speed_type; + +/** + * @brief usb core cofig struct + */ +typedef struct +{ + uint8_t speed; /*!< otg speed */ + uint8_t dma_en; /*!< dma enable state, not use*/ + uint8_t hc_num; /*!< the otg host support number of channel */ + uint8_t ept_num; /*!< the otg device support number of endpoint */ + + uint16_t max_size; /*!< support max packet size */ + uint16_t fifo_size; /*!< the usb otg total file size */ + uint8_t phy_itface; /*!< usb phy select */ + uint8_t core_id; /*!< the usb otg core id */ + uint8_t low_power; /*!< the usb otg low power option */ + uint8_t sof_out; /*!< the sof signal output */ + uint8_t usb_id; /*!< select otgfs1 or otgfs2 */ + uint8_t vbusig; /*!< vbus ignore */ +} usb_core_cfg; + +/** + * @brief usb otg core struct type + */ +typedef struct +{ + usb_reg_type *usb_reg; /*!< the usb otg register type */ +#ifdef USE_OTG_DEVICE_MODE + usbd_core_type dev; /*!< the usb device core type */ +#endif + +#ifdef USE_OTG_HOST_MODE + usbh_core_type host; /*!< the usb host core type */ +#endif + + usb_core_cfg cfg; /*!< the usb otg core config type */ + +} otg_core_type; + +usb_sts_type usb_core_config(otg_core_type *otgdev, uint8_t core_id); +#ifdef USE_OTG_DEVICE_MODE +usb_sts_type usbd_init(otg_core_type *udev, + uint8_t core_id, uint8_t usb_id, + usbd_class_handler *class_handler, + usbd_desc_handler *desc_handler); +#endif + +#ifdef USE_OTG_HOST_MODE +usb_sts_type usbh_init(otg_core_type *hdev, + uint8_t core_id, uint8_t usb_id, + usbh_class_handler_type *class_handler, + usbh_user_handler_type *user_handler); +#endif +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/src/main/drivers/at32/usb/usb_std.h b/src/main/drivers/at32/usb/usb_std.h new file mode 100644 index 0000000000..7e334caa5e --- /dev/null +++ b/src/main/drivers/at32/usb/usb_std.h @@ -0,0 +1,383 @@ +/** + ************************************************************************** + * @file usb_std.h + * @brief usb standard header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_STD_H +#define __USB_STD_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/* includes ------------------------------------------------------------------*/ +#include "usb_conf.h" + +/** @addtogroup AT32F435_437_middlewares_usb_drivers + * @{ + */ + +/** @addtogroup USB_standard + * @{ + */ + +/** @defgroup USB_standard_define + * @{ + */ + +/** + * @brief usb request recipient + */ +#define USB_REQ_RECIPIENT_DEVICE 0x00 /*!< usb request recipient device */ +#define USB_REQ_RECIPIENT_INTERFACE 0x01 /*!< usb request recipient interface */ +#define USB_REQ_RECIPIENT_ENDPOINT 0x02 /*!< usb request recipient endpoint */ +#define USB_REQ_RECIPIENT_OTHER 0x03 /*!< usb request recipient other */ +#define USB_REQ_RECIPIENT_MASK 0x1F /*!< usb request recipient mask */ + +/** + * @brief usb request type + */ +#define USB_REQ_TYPE_STANDARD 0x00 /*!< usb request type standard */ +#define USB_REQ_TYPE_CLASS 0x20 /*!< usb request type class */ +#define USB_REQ_TYPE_VENDOR 0x40 /*!< usb request type vendor */ +#define USB_REQ_TYPE_RESERVED 0x60 /*!< usb request type reserved */ + +/** + * @brief usb request data transfer direction + */ +#define USB_REQ_DIR_HTD 0x00 /*!< usb request data transfer direction host to device */ +#define USB_REQ_DIR_DTH 0x80 /*!< usb request data transfer direction device to host */ + +/** + * @brief usb standard device requests codes + */ +#define USB_STD_REQ_GET_STATUS 0 /*!< usb request code status */ +#define USB_STD_REQ_CLEAR_FEATURE 1 /*!< usb request code clear feature */ +#define USB_STD_REQ_SET_FEATURE 3 /*!< usb request code feature */ +#define USB_STD_REQ_SET_ADDRESS 5 /*!< usb request code address */ +#define USB_STD_REQ_GET_DESCRIPTOR 6 /*!< usb request code get descriptor */ +#define USB_STD_REQ_SET_DESCRIPTOR 7 /*!< usb request code set descriptor */ +#define USB_STD_REQ_GET_CONFIGURATION 8 /*!< usb request code get configuration */ +#define USB_STD_REQ_SET_CONFIGURATION 9 /*!< usb request code set configuration */ +#define USB_STD_REQ_GET_INTERFACE 10 /*!< usb request code get interface */ +#define USB_STD_REQ_SET_INTERFACE 11 /*!< usb request code set interface */ +#define USB_STD_REQ_SYNCH_FRAME 12 /*!< usb request code synch frame */ + +/** + * @brief usb standard device type + */ +#define USB_DESCIPTOR_TYPE_DEVICE 1 /*!< usb standard device type device */ +#define USB_DESCIPTOR_TYPE_CONFIGURATION 2 /*!< usb standard device type configuration */ +#define USB_DESCIPTOR_TYPE_STRING 3 /*!< usb standard device type string */ +#define USB_DESCIPTOR_TYPE_INTERFACE 4 /*!< usb standard device type interface */ +#define USB_DESCIPTOR_TYPE_ENDPOINT 5 /*!< usb standard device type endpoint */ +#define USB_DESCIPTOR_TYPE_DEVICE_QUALIFIER 6 /*!< usb standard device type qualifier */ +#define USB_DESCIPTOR_TYPE_OTHER_SPEED 7 /*!< usb standard device type other speed */ +#define USB_DESCIPTOR_TYPE_INTERFACE_POWER 8 /*!< usb standard device type interface power */ + +/** + * @brief usb standard string type + */ +#define USB_LANGID_STRING 0 /*!< usb standard string type lang id */ +#define USB_MFC_STRING 1 /*!< usb standard string type mfc */ +#define USB_PRODUCT_STRING 2 /*!< usb standard string type product */ +#define USB_SERIAL_STRING 3 /*!< usb standard string type serial */ +#define USB_CONFIG_STRING 4 /*!< usb standard string type config */ +#define USB_INTERFACE_STRING 5 /*!< usb standard string type interface */ + +/** + * @brief usb configuration attributes + */ +#define USB_CONF_REMOTE_WAKEUP 2 /*!< usb configuration attributes remote wakeup */ +#define USB_CONF_SELF_POWERED 1 /*!< usb configuration attributes self powered */ + +/** + * @brief usb standard feature selectors + */ +#define USB_FEATURE_EPT_HALT 0 /*!< usb standard feature selectors endpoint halt */ +#define USB_FEATURE_REMOTE_WAKEUP 1 /*!< usb standard feature selectors remote wakeup */ +#define USB_FEATURE_TEST_MODE 2 /*!< usb standard feature selectors test mode */ + +/** + * @brief usb device connect state + */ +typedef enum +{ + USB_CONN_STATE_DEFAULT =1, /*!< usb device connect state default */ + USB_CONN_STATE_ADDRESSED, /*!< usb device connect state address */ + USB_CONN_STATE_CONFIGURED, /*!< usb device connect state configured */ + USB_CONN_STATE_SUSPENDED /*!< usb device connect state suspend */ +}usbd_conn_state; + +/** + * @brief endpoint 0 state + */ +#define USB_EPT0_IDLE 0 /*!< usb endpoint state idle */ +#define USB_EPT0_SETUP 1 /*!< usb endpoint state setup */ +#define USB_EPT0_DATA_IN 2 /*!< usb endpoint state data in */ +#define USB_EPT0_DATA_OUT 3 /*!< usb endpoint state data out */ +#define USB_EPT0_STATUS_IN 4 /*!< usb endpoint state status in */ +#define USB_EPT0_STATUS_OUT 5 /*!< usb endpoint state status out */ +#define USB_EPT0_STALL 6 /*!< usb endpoint state stall */ + +/** + * @brief usb descriptor length + */ +#define USB_DEVICE_QUALIFIER_DESC_LEN 0x0A /*!< usb qualifier descriptor length */ +#define USB_DEVICE_DESC_LEN 0x12 /*!< usb device descriptor length */ +#define USB_DEVICE_CFG_DESC_LEN 0x09 /*!< usb configuration descriptor length */ +#define USB_DEVICE_IF_DESC_LEN 0x09 /*!< usb interface descriptor length */ +#define USB_DEVICE_EPT_LEN 0x07 /*!< usb endpoint descriptor length */ +#define USB_DEVICE_OTG_DESC_LEN 0x03 /*!< usb otg descriptor length */ +#define USB_DEVICE_LANGID_STR_DESC_LEN 0x04 /*!< usb lang id string descriptor length */ +#define USB_DEVICE_OTHER_SPEED_DESC_SIZ_LEN 0x09 /*!< usb other speed descriptor length */ + +/** + * @brief usb class code + */ +#define USB_CLASS_CODE_AUDIO 0x01 /*!< usb class code audio */ +#define USB_CLASS_CODE_CDC 0x02 /*!< usb class code cdc */ +#define USB_CLASS_CODE_HID 0x03 /*!< usb class code hid */ +#define USB_CLASS_CODE_PRINTER 0x07 /*!< usb class code printer */ +#define USB_CLASS_CODE_MSC 0x08 /*!< usb class code msc */ +#define USB_CLASS_CODE_HUB 0x09 /*!< usb class code hub */ +#define USB_CLASS_CODE_CDCDATA 0x0A /*!< usb class code cdc data */ +#define USB_CLASS_CODE_CCID 0x0B /*!< usb class code ccid */ +#define USB_CLASS_CODE_VIDEO 0x0E /*!< usb class code video */ +#define USB_CLASS_CODE_VENDOR 0xFF /*!< usb class code vendor */ + +/** + * @brief usb endpoint type + */ +#define USB_EPT_DESC_CONTROL 0x00 /*!< usb endpoint description type control */ +#define USB_EPT_DESC_ISO 0x01 /*!< usb endpoint description type iso */ +#define USB_EPT_DESC_BULK 0x02 /*!< usb endpoint description type bulk */ +#define USB_EPT_DESC_INTERRUPT 0x03 /*!< usb endpoint description type interrupt */ + +#define USB_EPT_DESC_NSYNC 0x00 /*!< usb endpoint description nsync */ +#define USB_ETP_DESC_ASYNC 0x04 /*!< usb endpoint description async */ +#define USB_ETP_DESC_ADAPTIVE 0x08 /*!< usb endpoint description adaptive */ +#define USB_ETP_DESC_SYNC 0x0C /*!< usb endpoint description sync */ + +#define USB_EPT_DESC_DATA_EPT 0x00 /*!< usb endpoint description data */ +#define USB_EPT_DESC_FD_EPT 0x10 /*!< usb endpoint description fd */ +#define USB_EPT_DESC_FDDATA_EPT 0x20 /*!< usb endpoint description fddata */ + +/** + * @brief usb cdc class descriptor define + */ +#define USBD_CDC_CS_INTERFACE 0x24 +#define USBD_CDC_CS_ENDPOINT 0x25 + +/** + * @brief usb cdc class sub-type define + */ +#define USBD_CDC_SUBTYPE_HEADER 0x00 +#define USBD_CDC_SUBTYPE_CMF 0x01 +#define USBD_CDC_SUBTYPE_ACM 0x02 +#define USBD_CDC_SUBTYPE_UFD 0x06 + +/** + * @brief usb cdc class request code define + */ +#define SET_LINE_CODING 0x20 +#define GET_LINE_CODING 0x21 + +/** + * @brief usb cdc class set line coding struct + */ +typedef struct +{ + uint32_t bitrate; /* line coding baud rate */ + uint8_t format; /* line coding foramt */ + uint8_t parity; /* line coding parity */ + uint8_t data; /* line coding data bit */ +}linecoding_type; + +/** + * @brief usb hid class descriptor define + */ +#define HID_CLASS_DESC_HID 0x21 +#define HID_CLASS_DESC_REPORT 0x22 +#define HID_CLASS_DESC_PHYSICAL 0x23 + +/** + * @brief usb hid class request code define + */ +#define HID_REQ_SET_PROTOCOL 0x0B +#define HID_REQ_GET_PROTOCOL 0x03 +#define HID_REQ_SET_IDLE 0x0A +#define HID_REQ_GET_IDLE 0x02 +#define HID_REQ_SET_REPORT 0x09 +#define HID_REQ_GET_REPORT 0x01 +#define HID_DESCRIPTOR_TYPE 0x21 +#define HID_REPORT_DESC 0x22 + +/** + * @brief endpoint 0 max size + */ +#define USB_MAX_EP0_SIZE 64 /*!< usb endpoint 0 max size */ + +/** + * @brief usb swap address + */ +#define SWAPBYTE(addr) (uint16_t)(((uint16_t)(*((uint8_t *)(addr)))) + \ + (((uint16_t)(*(((uint8_t *)(addr)) + 1))) << 8)) /*!< swap address */ + +/** + * @brief min and max define + */ +#define MIN(a, b) (uint16_t)(((a) < (b)) ? (a) : (b)) /*!< min define*/ +#define MAX(a, b) (uint16_t)(((a) > (b)) ? (a) : (b)) /*!< max define*/ + +/** + * @brief low byte and high byte define + */ +#define LBYTE(x) ((uint8_t)(x & 0x00FF)) /*!< low byte define */ +#define HBYTE(x) ((uint8_t)((x & 0xFF00) >>8)) /*!< high byte define*/ + +/** + * @brief usb return status + */ +typedef enum +{ + USB_OK, /*!< usb status ok */ + USB_FAIL, /*!< usb status fail */ + USB_WAIT, /*!< usb status wait */ + USB_NOT_SUPPORT, /*!< usb status not support */ + USB_ERROR, /*!< usb status error */ +}usb_sts_type; + + +/** + * @brief format of usb setup data + */ +typedef struct +{ + uint8_t bmRequestType; /*!< characteristics of request */ + uint8_t bRequest; /*!< specific request */ + uint16_t wValue; /*!< word-sized field that varies according to request */ + uint16_t wIndex; /*!< word-sized field that varies according to request + typically used to pass an index or offset */ + uint16_t wLength; /*!< number of bytes to transfer if there is a data stage */ +} usb_setup_type; + +/** + * @brief format of standard device descriptor + */ +typedef struct +{ + uint8_t bLength; /*!< size of this descriptor in bytes */ + uint8_t bDescriptorType; /*!< device descriptor type */ + uint16_t bcdUSB; /*!< usb specification release number */ + uint8_t bDeviceClass; /*!< class code (assigned by the usb-if) */ + uint8_t bDeviceSubClass; /*!< subclass code (assigned by the usb-if) */ + uint8_t bDeviceProtocol; /*!< protocol code ((assigned by the usb-if)) */ + uint8_t bMaxPacketSize0; /*!< maximum packet size for endpoint zero */ + uint16_t idVendor; /*!< verndor id ((assigned by the usb-if)) */ + uint16_t idProduct; /*!< product id ((assigned by the usb-if)) */ + uint16_t bcdDevice; /*!< device release number in binary-coded decimal */ + uint8_t iManufacturer; /*!< index of string descriptor describing manufacturer */ + uint8_t iProduct; /*!< index of string descriptor describing product */ + uint8_t iSerialNumber; /*!< index of string descriptor describing serial number */ + uint8_t bNumConfigurations; /*!< number of possible configurations */ +} usb_device_desc_type; + +/** + * @brief format of standard configuration descriptor + */ +typedef struct +{ + uint8_t bLength; /*!< size of this descriptor in bytes */ + uint8_t bDescriptorType; /*!< configuration descriptor type */ + uint16_t wTotalLength; /*!< total length of data returned for this configuration */ + uint8_t bNumInterfaces; /*!< number of interfaces supported by this configuration */ + uint8_t bConfigurationValue; /*!< value to use as an argument to the SetConfiguration() request */ + uint8_t iConfiguration; /*!< index of string descriptor describing this configuration */ + uint8_t bmAttributes; /*!< configuration characteristics + D7 reserved + D6 self-powered + D5 remote wakeup + D4~D0 reserved */ + uint8_t bMaxPower; /*!< maximum power consumption of the usb device from the bus */ + + +}usb_configuration_desc_type; + +/** + * @brief format of standard interface descriptor + */ +typedef struct +{ + uint8_t bLength; /*!< size of this descriptor in bytes */ + uint8_t bDescriptorType; /*!< interface descriptor type */ + uint8_t bInterfaceNumber; /*!< number of this interface */ + uint8_t bAlternateSetting; /*!< value used to select this alternate setting for the interface */ + uint8_t bNumEndpoints; /*!< number of endpoints used by this interface */ + uint8_t bInterfaceClass; /*!< class code (assigned by the usb-if) */ + uint8_t bInterfaceSubClass; /*!< subclass code (assigned by the usb-if) */ + uint8_t bInterfaceProtocol; /*!< protocol code (assigned by the usb-if) */ + uint8_t iInterface; /*!< index of string descriptor describing this interface */ +} usb_interface_desc_type; + +/** + * @brief format of standard endpoint descriptor + */ +typedef struct +{ + uint8_t bLength; /*!< size of this descriptor in bytes */ + uint8_t bDescriptorType; /*!< endpoint descriptor type */ + uint8_t bEndpointAddress; /*!< the address of the endpoint on the usb device described by this descriptor */ + uint8_t bmAttributes; /*!< describes the endpoints attributes when it is configured using bConfiguration value */ + uint16_t wMaxPacketSize; /*!< maximum packet size this endpoint */ + uint8_t bInterval; /*!< interval for polling endpoint for data transfers */ +} usb_endpoint_desc_type; + +/** + * @brief format of header + */ +typedef struct +{ + uint8_t bLength; /*!< size of this descriptor in bytes */ + uint8_t bDescriptorType; /*!< descriptor type */ +} usb_header_desc_type; + + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/src/main/drivers/at32/usb/usbd_core.c b/src/main/drivers/at32/usb/usbd_core.c new file mode 100644 index 0000000000..e3b8b87512 --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_core.c @@ -0,0 +1,884 @@ +/** + ************************************************************************** + * @file usbd_core.c + * @brief usb device driver + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usb_core.h" +#include "usbd_core.h" +#include "usbd_sdr.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @defgroup USBD_drivers_core + * @brief usb device drivers core + * @{ + */ + +/** @defgroup USBD_core_private_functions + * @{ + */ + +/** + * @brief usb core in transfer complete handler + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_core_in_handler(usbd_core_type *udev, uint8_t ept_addr) +{ + /* get endpoint info*/ + usb_ept_info *ept_info = &udev->ept_in[ept_addr & 0x7F]; + + if(ept_addr == 0) + { + if(udev->ept0_sts == USB_EPT0_DATA_IN) + { + if(ept_info->rem0_len > ept_info->maxpacket) + { + ept_info->rem0_len -= ept_info->maxpacket; + usbd_ept_send(udev, 0, ept_info->trans_buf, + MIN(ept_info->rem0_len, ept_info->maxpacket)); + } + /* endpoint 0 */ + else if(ept_info->last_len == ept_info->maxpacket + && ept_info->ept0_slen >= ept_info->maxpacket + && ept_info->ept0_slen < udev->ept0_wlength) + { + ept_info->last_len = 0; + usbd_ept_send(udev, 0, 0, 0); + usbd_ept_recv(udev, ept_addr, 0, 0); + } + else + { + + if(udev->class_handler->ept0_tx_handler != 0 && + udev->conn_state == USB_CONN_STATE_CONFIGURED) + { + udev->class_handler->ept0_tx_handler(udev); + } + usbd_ctrl_recv_status(udev); + + } + } + } + else if(udev->class_handler->in_handler != 0 && + udev->conn_state == USB_CONN_STATE_CONFIGURED) + { + /* other user define endpoint */ + udev->class_handler->in_handler(udev, ept_addr); + } +} + +/** + * @brief usb core out transfer complete handler + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_core_out_handler(usbd_core_type *udev, uint8_t ept_addr) +{ + /* get endpoint info*/ + usb_ept_info *ept_info = &udev->ept_out[ept_addr & 0x7F]; + + if(ept_addr == 0) + { + /* endpoint 0 */ + if(udev->ept0_sts == USB_EPT0_DATA_OUT) + { + if(ept_info->rem0_len > ept_info->maxpacket) + { + ept_info->rem0_len -= ept_info->maxpacket; + usbd_ept_recv(udev, ept_addr, ept_info->trans_buf, + MIN(ept_info->rem0_len, ept_info->maxpacket)); + } + else + { + if(udev->class_handler->ept0_rx_handler != 0) + { + udev->class_handler->ept0_rx_handler(udev); + } + usbd_ctrl_send_status(udev); + } + } + } + else if(udev->class_handler->out_handler != 0 && + udev->conn_state == USB_CONN_STATE_CONFIGURED) + { + /* other user define endpoint */ + udev->class_handler->out_handler(udev, ept_addr); + } +} + +/** + * @brief usb core setup transfer complete handler + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_core_setup_handler(usbd_core_type *udev, uint8_t ept_num) +{ + UNUSED(ept_num); + + /* setup parse */ + usbd_setup_request_parse(&udev->setup, udev->setup_buffer); + + /* set ept0 status */ + udev->ept0_sts = USB_EPT0_SETUP; + udev->ept0_wlength = udev->setup.wLength; + + switch(udev->setup.bmRequestType & USB_REQ_RECIPIENT_MASK) + { + case USB_REQ_RECIPIENT_DEVICE: + /* recipient device request */ + usbd_device_request(udev); + break; + case USB_REQ_RECIPIENT_INTERFACE: + /* recipient interface request */ + usbd_interface_request(udev); + break; + case USB_REQ_RECIPIENT_ENDPOINT: + /* recipient endpoint request */ + usbd_endpoint_request(udev); + break; + default: + break; + } +} + +/** + * @brief usb control endpoint send data + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @param buffer: send data buffer + * @param len: send data length + * @retval none + */ +void usbd_ctrl_send(usbd_core_type *udev, uint8_t *buffer, uint16_t len) +{ + usb_ept_info *ept_info = &udev->ept_in[0]; + + ept_info->ept0_slen = len; + ept_info->rem0_len = len; + udev->ept0_sts = USB_EPT0_DATA_IN; + + usbd_ept_send(udev, 0, buffer, len); +} + +/** + * @brief usb control endpoint recv data + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @param buffer: recv data buffer + * @param len: recv data length + * @retval none + */ +void usbd_ctrl_recv(usbd_core_type *udev, uint8_t *buffer, uint16_t len) +{ + usb_ept_info *ept_info = &udev->ept_out[0]; + + ept_info->ept0_slen = len; + ept_info->rem0_len = len; + udev->ept0_sts = USB_EPT0_DATA_OUT; + + usbd_ept_recv(udev, 0, buffer, len); +} + +/** + * @brief usb control endpoint send in status + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_ctrl_send_status(usbd_core_type *udev) +{ + udev->ept0_sts = USB_EPT0_STATUS_IN; + + usbd_ept_send(udev, 0, 0, 0); +} + +/** + * @brief usb control endpoint send out status + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_ctrl_recv_status(usbd_core_type *udev) +{ + udev->ept0_sts = USB_EPT0_STATUS_OUT; + + usbd_ept_recv(udev, 0, 0, 0); +} + +/** + * @brief clear endpoint stall + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_clear_stall(usbd_core_type *udev, uint8_t ept_addr) +{ + usb_ept_info *ept_info; + usb_reg_type *usbx = udev->usb_reg; + + if(ept_addr & 0x80) + { + /* in endpoint */ + ept_info = &udev->ept_in[ept_addr & 0x7F]; + } + else + { + /* out endpoint */ + ept_info = &udev->ept_out[ept_addr & 0x7F]; + } + usb_ept_clear_stall(usbx, ept_info); + ept_info->stall = 0; +} + +/** + * @brief usb set endpoint to stall status + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_set_stall(usbd_core_type *udev, uint8_t ept_addr) +{ + usb_ept_info *ept_info; + usb_reg_type *usbx = udev->usb_reg; + + if(ept_addr & 0x80) + { + /* in endpoint */ + ept_info = &udev->ept_in[ept_addr & 0x7F]; + } + else + { + /* out endpoint */ + ept_info = &udev->ept_out[ept_addr & 0x7F]; + } + usb_ept_stall(usbx, ept_info); + + ept_info->stall = 1; +} + +/** + * @brief un-support device request + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_ctrl_unsupport(usbd_core_type *udev) +{ + /* return stall status */ + usbd_set_stall(udev, 0x00); + usbd_set_stall(udev, 0x80); +} + +/** + * @brief get endpoint receive data length + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval data receive len + */ +uint32_t usbd_get_recv_len(usbd_core_type *udev, uint8_t ept_addr) +{ + usb_ept_info *ept = &udev->ept_out[ept_addr & 0x7F]; + return ept->trans_len; +} + +/** + * @brief usb open endpoint + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @param ept_type: endpoint type + * @param maxpacket: endpoint support max buffer size + * @retval none + */ +void usbd_ept_open(usbd_core_type *udev, uint8_t ept_addr, uint8_t ept_type, uint16_t maxpacket) +{ + usb_reg_type *usbx = udev->usb_reg; + usb_ept_info *ept_info; + + if((ept_addr & 0x80) == 0) + { + /* out endpoint info */ + ept_info = &udev->ept_out[ept_addr & 0x7F]; + ept_info->inout = EPT_DIR_OUT; + } + else + { + /* in endpoint info */ + ept_info = &udev->ept_in[ept_addr & 0x7F]; + ept_info->inout = EPT_DIR_IN; + } + + /* set endpoint maxpacket and type */ + ept_info->maxpacket = maxpacket; + ept_info->trans_type = ept_type; + + /* open endpoint */ + usb_ept_open(usbx, ept_info); +} + +/** + * @brief usb close endpoint + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @retval none + */ +void usbd_ept_close(usbd_core_type *udev, uint8_t ept_addr) +{ + usb_ept_info *ept_info; + if(ept_addr & 0x80) + { + /* in endpoint */ + ept_info = &udev->ept_in[ept_addr & 0x7F]; + } + else + { + /* out endpoint */ + ept_info = &udev->ept_out[ept_addr & 0x7F]; + } + + /* close endpoint */ + usb_ept_close(udev->usb_reg, ept_info); +} + +/** + * @brief usb device connect to host + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_connect(usbd_core_type *udev) +{ + usb_connect(udev->usb_reg); +} + +/** + * @brief usb device disconnect to host + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_disconnect(usbd_core_type *udev) +{ + usb_disconnect(udev->usb_reg); +} + +/** + * @brief usb device set device address. + * @param udev: to the structure of usbd_core_type + * @param address: host assignment address + * @retval none + */ +void usbd_set_device_addr(usbd_core_type *udev, uint8_t address) +{ + usb_set_address(udev->usb_reg, address); +} + +/** + * @brief usb endpoint structure initialization + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usb_ept_default_init(usbd_core_type *udev) +{ + uint8_t i_index = 0; + /* init in endpoint info structure */ + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + udev->ept_in[i_index].eptn = i_index; + udev->ept_in[i_index].ept_address = i_index; + udev->ept_in[i_index].inout = EPT_DIR_IN; + udev->ept_in[i_index].maxpacket = 0; + udev->ept_in[i_index].trans_buf = 0; + udev->ept_in[i_index].total_len = 0; + } + + /* init out endpoint info structure */ + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + udev->ept_out[i_index].eptn = i_index; + udev->ept_out[i_index].ept_address = i_index; + udev->ept_out[i_index].inout = EPT_DIR_OUT; + udev->ept_out[i_index].maxpacket = 0; + udev->ept_out[i_index].trans_buf = 0; + udev->ept_out[i_index].total_len = 0; + } +} + +/** + * @brief endpoint send data + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @param buffer: send data buffer + * @param len: send data length + * @retval none + */ +void usbd_ept_send(usbd_core_type *udev, uint8_t ept_addr, uint8_t *buffer, uint16_t len) +{ + /* get endpoint info struct and register */ + usb_reg_type *usbx = udev->usb_reg; + usb_ept_info *ept_info = &udev->ept_in[ept_addr & 0x7F]; + otg_eptin_type *ept_in = USB_INEPT(usbx, ept_info->eptn); + otg_device_type *dev = OTG_DEVICE(usbx); + uint32_t pktcnt; + + /* set send data buffer and length */ + ept_info->trans_buf = buffer; + ept_info->total_len = len; + ept_info->trans_len = 0; + + /* transfer data len is zero */ + if(ept_info->total_len == 0) + { + ept_in->dieptsiz_bit.pktcnt = 1; + ept_in->dieptsiz_bit.xfersize = 0; + } + else + { + if((ept_addr & 0x7F) == 0) // endpoint 0 + { + /* endpoint 0 */ + if(ept_info->total_len > ept_info->maxpacket) + { + ept_info->total_len = ept_info->maxpacket; + } + + /* set transfer size */ + ept_in->dieptsiz_bit.xfersize = ept_info->total_len; + + /* set packet count */ + ept_in->dieptsiz_bit.pktcnt = 1; + + ept_info->last_len = ept_info->total_len; + } + else + { + /* other endpoint */ + + /* packet count */ + pktcnt = (ept_info->total_len + ept_info->maxpacket - 1) / ept_info->maxpacket; + + /* set transfer size */ + ept_in->dieptsiz_bit.xfersize = ept_info->total_len; + + /* set packet count */ + ept_in->dieptsiz_bit.pktcnt = pktcnt; + + if(ept_info->trans_type == EPT_ISO_TYPE) + { + ept_in->dieptsiz_bit.mc = 1; + } + } + } + + if(ept_info->trans_type != EPT_ISO_TYPE) + { + if(ept_info->total_len > 0) + { + /* set in endpoint tx fifo empty interrupt mask */ + dev->diepempmsk |= 1 << ept_info->eptn; + } + } + + if(ept_info->trans_type == EPT_ISO_TYPE) + { + if((dev->dsts_bit.soffn & 0x1) == 0) + { + ept_in->diepctl_bit.setd1pid = TRUE; + } + else + { + ept_in->diepctl_bit.setd0pid = TRUE; + } + } + + /* clear endpoint nak */ + ept_in->diepctl_bit.cnak = TRUE; + + /* endpoint enable */ + ept_in->diepctl_bit.eptena = TRUE; + + if(ept_info->trans_type == EPT_ISO_TYPE) + { + /* write data to fifo */ + usb_write_packet(usbx, ept_info->trans_buf, ept_info->eptn, ept_info->total_len); + } +} + +/** + * @brief endpoint receive data + * @param udev: to the structure of usbd_core_type + * @param ept_addr: endpoint number + * @param buffer: receive data buffer + * @param len: receive data length + * @retval none + */ +void usbd_ept_recv(usbd_core_type *udev, uint8_t ept_addr, uint8_t *buffer, uint16_t len) +{ + /* get endpoint info struct and register */ + usb_reg_type *usbx = udev->usb_reg; + usb_ept_info *ept_info = &udev->ept_out[ept_addr & 0x7F]; + otg_eptout_type *ept_out = USB_OUTEPT(usbx, ept_info->eptn); + otg_device_type *dev = OTG_DEVICE(usbx); + uint32_t pktcnt; + + /* set receive data buffer and length */ + ept_info->trans_buf = buffer; + ept_info->total_len = len; + ept_info->trans_len = 0; + + if((ept_addr & 0x7F) == 0) + { + /* endpoint 0 */ + ept_info->total_len = ept_info->maxpacket; + } + + if(ept_info->total_len == 0 || ((ept_addr & 0x7F) == 0)) + { + /* set transfer size */ + ept_out->doeptsiz_bit.xfersize = ept_info->maxpacket; + + /* set packet count */ + ept_out->doeptsiz_bit.pktcnt = 1; + } + else + { + pktcnt = (ept_info->total_len + ept_info->maxpacket - 1) / ept_info->maxpacket; + + /* set transfer size */ + ept_out->doeptsiz_bit.xfersize = ept_info->maxpacket * pktcnt; + + /* set packet count */ + ept_out->doeptsiz_bit.pktcnt = pktcnt; + } + + if(ept_info->trans_type == EPT_ISO_TYPE) + { + if((dev->dsts_bit.soffn & 0x01) == 0) + { + ept_out->doepctl_bit.setd1pid = TRUE; + } + else + { + ept_out->doepctl_bit.setd0pid = TRUE; + } + } + + /* clear endpoint nak */ + ept_out->doepctl_bit.cnak = TRUE; + + /* endpoint enable */ + ept_out->doepctl_bit.eptena = TRUE; +} + +/** + * @brief get usb connect state + * @param udev: to the structure of usbd_core_type + * @retval usb connect state + */ +usbd_conn_state usbd_connect_state_get(usbd_core_type *udev) +{ + return udev->conn_state; +} + +/** + * @brief usb device remote wakeup + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_remote_wakeup(usbd_core_type *udev) +{ + /* check device is in suspend mode */ + if(usb_suspend_status_get(udev->usb_reg) == 1) + { + /* set connect state */ + udev->conn_state = udev->old_conn_state; + + /* open phy clock */ + usb_open_phy_clk(udev->usb_reg); + + /* set remote wakeup */ + usb_remote_wkup_set(udev->usb_reg); + + /* delay 10 ms */ + usb_delay_ms(10); + + /* clear remote wakup */ + usb_remote_wkup_clear(udev->usb_reg); + } +} + +/** + * @brief usb device enter suspend mode + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_enter_suspend(usbd_core_type *udev) +{ + /* check device is in suspend mode */ + if(usb_suspend_status_get(udev->usb_reg) == 1) + { + /* stop phy clk */ + usb_stop_phy_clk(udev->usb_reg); + } +} + +/** + * @brief usb device flush in endpoint fifo + * @param udev: to the structure of usbd_core_type + * @param ept_num: endpoint number + * @retval none + */ +void usbd_flush_tx_fifo(usbd_core_type *udev, uint8_t ept_num) +{ + /* flush endpoint tx fifo */ + usb_flush_tx_fifo(udev->usb_reg, ept_num & 0x1F); +} + +/** + * @brief usb device endpoint fifo alloc + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_fifo_alloc(usbd_core_type *udev) +{ + usb_reg_type *usbx = udev->usb_reg; + + if(usbx == OTG1_GLOBAL) + { + /* set receive fifo size */ + usb_set_rx_fifo(usbx, USBD_RX_SIZE); + + /* set endpoint0 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT0, USBD_EP0_TX_SIZE); + + /* set endpoint1 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT1, USBD_EP1_TX_SIZE); + + /* set endpoint2 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT2, USBD_EP2_TX_SIZE); + + /* set endpoint3 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT3, USBD_EP3_TX_SIZE); + + if(USB_EPT_MAX_NUM == 8) + { + /* set endpoint4 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT4, USBD_EP4_TX_SIZE); + + /* set endpoint5 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT5, USBD_EP5_TX_SIZE); + + /* set endpoint6 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT6, USBD_EP6_TX_SIZE); + + /* set endpoint7 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT7, USBD_EP7_TX_SIZE); + } + } +#ifdef OTG2_GLOBAL + if(usbx == OTG2_GLOBAL) + { + /* set receive fifo size */ + usb_set_rx_fifo(usbx, USBD2_RX_SIZE); + + /* set endpoint0 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT0, USBD2_EP0_TX_SIZE); + + /* set endpoint1 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT1, USBD2_EP1_TX_SIZE); + + /* set endpoint2 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT2, USBD2_EP2_TX_SIZE); + + /* set endpoint3 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT3, USBD2_EP3_TX_SIZE); + + if(USB_EPT_MAX_NUM == 8) + { + /* set endpoint4 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT4, USBD2_EP4_TX_SIZE); + + /* set endpoint5 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT5, USBD2_EP5_TX_SIZE); + + /* set endpoint6 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT6, USBD2_EP6_TX_SIZE); + + /* set endpoint7 tx fifo size */ + usb_set_tx_fifo(usbx, USB_EPT7, USBD2_EP7_TX_SIZE); + } + } +#endif +} + +/** + * @brief usb device core initialization + * @param udev: to the structure of usbd_core_type + * @param usb_reg: usb otgfs peripheral global register + * this parameter can be one of the following values: + * OTG1_GLOBAL , OTG2_GLOBAL + * @param class_handler: usb class handler + * @param desc_handler: device config handler + * @param core_id: usb core id number + * @retval usb_sts_type + */ +usb_sts_type usbd_core_init(usbd_core_type *udev, + usb_reg_type *usb_reg, + usbd_class_handler *class_handler, + usbd_desc_handler *desc_handler, + uint8_t core_id) +{ + UNUSED(core_id); + usb_reg_type *usbx; + otg_device_type *dev; + otg_eptin_type *ept_in; + otg_eptout_type *ept_out; + uint32_t i_index; + + udev->usb_reg = usb_reg; + usbx = usb_reg; + dev = OTG_DEVICE(usbx); + + /* set connect state */ + udev->conn_state = USB_CONN_STATE_DEFAULT; + + /* device class config */ + udev->device_addr = 0; + udev->class_handler = class_handler; + udev->desc_handler = desc_handler; + /* set device disconnect */ + usbd_disconnect(udev); + + /* set endpoint to default status */ + usb_ept_default_init(udev); + + /* disable usb global interrupt */ + usb_interrupt_disable(usbx); + + /* init global register */ + usb_global_init(usbx); + + /* set device mode */ + usb_global_set_mode(usbx, OTG_DEVICE_MODE); + + /* open phy clock */ + usb_open_phy_clk(udev->usb_reg); + + /* set periodic frame interval */ + dev->dcfg_bit.perfrint = DCFG_PERFRINT_80; + + /* set device speed to full-speed */ + dev->dcfg_bit.devspd = USB_DCFG_FULL_SPEED; + + /* flush all tx fifo */ + usb_flush_tx_fifo(usbx, 16); + + /* flush share rx fifo */ + usb_flush_rx_fifo(usbx); + + /* clear all endpoint interrupt flag and mask */ + dev->daint = 0xFFFFFFFF; + dev->daintmsk = 0; + dev->diepmsk = 0; + dev->doepmsk = 0; + + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + usbx->dieptxfn[i_index] = 0; + } + + /* endpoint fifo alloc */ + usbd_fifo_alloc(udev); + + /* disable all in endpoint */ + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + ept_in = USB_INEPT(usbx, i_index); + if(ept_in->diepctl_bit.eptena) + { + ept_in->diepctl = 0; + ept_in->diepctl_bit.eptdis = TRUE; + ept_in->diepctl_bit.snak = TRUE; + } + else + { + ept_in->diepctl = 0; + } + ept_in->dieptsiz = 0; + ept_in->diepint = 0xFF; + } + + /* disable all out endpoint */ + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + ept_out = USB_OUTEPT(usbx, i_index); + if(ept_out->doepctl_bit.eptena) + { + ept_out->doepctl = 0; + ept_out->doepctl_bit.eptdis = TRUE; + ept_out->doepctl_bit.snak = TRUE; + } + else + { + ept_out->doepctl = 0; + } + ept_out->doeptsiz = 0; + ept_out->doepint = 0xFF; + } + dev->diepmsk_bit.txfifoudrmsk = TRUE; + + /* clear global interrupt and mask */ + usbx->gintmsk = 0; + usbx->gintsts = 0xBFFFFFFF; + + /* enable global interrupt mask */ + usbx->gintmsk = USB_OTG_SOF_INT | USB_OTG_RXFLVL_INT | + USB_OTG_USBSUSP_INT | USB_OTG_USBRST_INT | + USB_OTG_ENUMDONE_INT | USB_OTG_IEPT_INT | + USB_OTG_OEPT_INT | USB_OTG_INCOMISOIN_INT | + USB_OTG_INCOMPIP_INCOMPISOOUT_INT | USB_OTG_WKUP_INT | + USB_OTG_OTGINT_INT; + + /* usb connect */ + usbd_connect(udev); + + /* enable global interrupt */ + usb_interrupt_enable(usbx); + + return USB_OK; + +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/src/main/drivers/at32/usb/usbd_core.h b/src/main/drivers/at32/usb/usbd_core.h new file mode 100644 index 0000000000..17d6c42861 --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_core.h @@ -0,0 +1,185 @@ +/** + ************************************************************************** + * @file usbd_core.h + * @brief usb device core header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBD_CORE_H +#define __USBD_CORE_H + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "usb_conf.h" +#include "usb_std.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @addtogroup USBD_drivers_core + * @{ + */ + +/** @defgroup USBD_core_exported_types + * @{ + */ + +#ifdef USE_OTG_DEVICE_MODE + +/** + * @brief usb device event + */ +typedef enum +{ + USBD_NOP_EVENT, /*!< usb device nop event */ + USBD_RESET_EVENT, /*!< usb device reset event */ + USBD_SUSPEND_EVENT, /*!< usb device suspend event */ + USBD_WAKEUP_EVENT, /*!< usb device wakeup event */ + USBD_DISCONNECT_EVNET, /*!< usb device disconnect event */ + USBD_INISOINCOM_EVENT, /*!< usb device inisoincom event */ + USBD_OUTISOINCOM_EVENT, /*!< usb device outisoincom event */ + USBD_ERR_EVENT /*!< usb device error event */ +}usbd_event_type; + +/** + * @brief usb device descriptor struct + */ +typedef struct +{ + uint16_t length; /*!< descriptor length */ + uint8_t *descriptor; /*!< descriptor string */ +}usbd_desc_t; + +/** + * @brief usb device descriptor handler + */ +typedef struct +{ + usbd_desc_t *(*get_device_descriptor)(void); /*!< get device descriptor callback */ + usbd_desc_t *(*get_device_qualifier)(void); /*!< get device qualifier callback */ + usbd_desc_t *(*get_device_configuration)(void); /*!< get device configuration callback */ + usbd_desc_t *(*get_device_other_speed)(void); /*!< get device other speed callback */ + usbd_desc_t *(*get_device_lang_id)(void); /*!< get device lang id callback */ + usbd_desc_t *(*get_device_manufacturer_string)(void); /*!< get device manufacturer callback */ + usbd_desc_t *(*get_device_product_string)(void); /*!< get device product callback */ + usbd_desc_t *(*get_device_serial_string)(void); /*!< get device serial callback */ + usbd_desc_t *(*get_device_interface_string)(void); /*!< get device interface string callback */ + usbd_desc_t *(*get_device_config_string)(void); /*!< get device device config callback */ +}usbd_desc_handler; + +/** + * @brief usb device class handler + */ +typedef struct +{ + usb_sts_type (*init_handler)(void *udev); /*!< usb class init handler */ + usb_sts_type (*clear_handler)(void *udev); /*!< usb class clear handler */ + usb_sts_type (*setup_handler)(void *udev, usb_setup_type *setup); /*!< usb class setup handler */ + usb_sts_type (*ept0_tx_handler)(void *udev); /*!< usb class endpoint 0 tx complete handler */ + usb_sts_type (*ept0_rx_handler)(void *udev); /*!< usb class endpoint 0 rx complete handler */ + usb_sts_type (*in_handler)(void *udev, uint8_t ept_num); /*!< usb class in transfer complete handler */ + usb_sts_type (*out_handler)(void *udev, uint8_t ept_num); /*!< usb class out transfer complete handler */ + usb_sts_type (*sof_handler)(void *udev); /*!< usb class sof handler */ + usb_sts_type (*event_handler)(void *udev, usbd_event_type event); /*!< usb class event handler */ + void *pdata; /*!< usb class data pointer */ +}usbd_class_handler; + +/** + * @brief usb device core struct type + */ +typedef struct +{ + usb_reg_type *usb_reg; /*!< usb register pointer */ + + usbd_class_handler *class_handler; /*!< usb device class handler pointer */ + usbd_desc_handler *desc_handler; /*!< usb device descriptor handler pointer */ + + usb_ept_info ept_in[USB_EPT_MAX_NUM]; /*!< usb in endpoint infomation struct */ + usb_ept_info ept_out[USB_EPT_MAX_NUM]; /*!< usb out endpoint infomation struct */ + + usb_setup_type setup; /*!< usb setup type struct */ + uint8_t setup_buffer[12]; /*!< usb setup request buffer */ + + uint8_t ept0_sts; /*!< usb control endpoint 0 state */ + uint8_t speed; /*!< usb speed */ + uint16_t ept0_wlength; /*!< usb endpoint 0 transfer length */ + + usbd_conn_state conn_state; /*!< usb current connect state */ + usbd_conn_state old_conn_state; /*!< usb save the previous connect state */ + + uint8_t device_addr; /*!< device address */ + uint8_t remote_wakup; /*!< remote wakeup state */ + uint8_t default_config; /*!< usb default config state */ + uint8_t dev_config; /*!< usb device config state */ + uint32_t config_status; /*!< usb configure status */ +}usbd_core_type; + +void usbd_core_in_handler(usbd_core_type *udev, uint8_t ept_num); +void usbd_core_out_handler(usbd_core_type *udev, uint8_t ept_num); +void usbd_core_setup_handler(usbd_core_type *udev, uint8_t ept_num); +void usbd_ctrl_unsupport(usbd_core_type *udev); +void usbd_ctrl_send(usbd_core_type *udev, uint8_t *buffer, uint16_t len); +void usbd_ctrl_recv(usbd_core_type *udev, uint8_t *buffer, uint16_t len); +void usbd_ctrl_send_status(usbd_core_type *udev); +void usbd_ctrl_recv_status(usbd_core_type *udev); +void usbd_set_stall(usbd_core_type *udev, uint8_t ept_addr); +void usbd_clear_stall(usbd_core_type *udev, uint8_t ept_addr); +void usbd_ept_open(usbd_core_type *udev, uint8_t ept_addr, uint8_t ept_type, uint16_t maxpacket); +void usbd_ept_close(usbd_core_type *udev, uint8_t ept_addr); +void usbd_ept_send(usbd_core_type *udev, uint8_t ept_num, uint8_t *buffer, uint16_t len); +void usbd_ept_recv(usbd_core_type *udev, uint8_t ept_num, uint8_t *buffer, uint16_t len); +void usbd_connect(usbd_core_type *udev); +void usbd_disconnect(usbd_core_type *udev); +void usbd_set_device_addr(usbd_core_type *udev, uint8_t address); +uint32_t usbd_get_recv_len(usbd_core_type *udev, uint8_t ept_addr); +void usb_ept_defaut_init(usbd_core_type *udev); +usbd_conn_state usbd_connect_state_get(usbd_core_type *udev); +void usbd_remote_wakeup(usbd_core_type *udev); +void usbd_enter_suspend(usbd_core_type *udev); +void usbd_flush_tx_fifo(usbd_core_type *udev, uint8_t ept_num); +void usbd_fifo_alloc(usbd_core_type *udev); +usb_sts_type usbd_core_init(usbd_core_type *udev, + usb_reg_type *usb_reg, + usbd_class_handler *class_handler, + usbd_desc_handler *desc_handler, + uint8_t core_id); +#endif + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usbd_int.c b/src/main/drivers/at32/usb/usbd_int.c new file mode 100644 index 0000000000..3b53416b78 --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_int.c @@ -0,0 +1,539 @@ +/** + ************************************************************************** + * @file usbd_int.c + * @brief usb interrupt request + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usbd_int.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @defgroup USBD_drivers_interrupt + * @brief usb device interrupt + * @{ + */ + +/** @defgroup USBD_int_private_functions + * @{ + */ + +/** + * @brief usb device interrput request handler. + * @param otgdev: to the structure of otg_core_type + * @retval none + */ +void usbd_irq_handler(otg_core_type *otgdev) +{ + otg_global_type *usbx = otgdev->usb_reg; + usbd_core_type *udev = &otgdev->dev; + uint32_t intsts = usb_global_get_all_interrupt(usbx); + + /* check current device mode */ + if(usbx->gintsts_bit.curmode == 0) + { + /* mode mismatch interrupt */ + if(intsts & USB_OTG_MODEMIS_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_MODEMIS_FLAG); + } + + /* in endpoint interrupt */ + if(intsts & USB_OTG_IEPT_FLAG) + { + usbd_inept_handler(udev); + } + + /* out endpoint interrupt */ + if(intsts & USB_OTG_OEPT_FLAG) + { + usbd_outept_handler(udev); + } + + /* usb reset interrupt */ + if(intsts & USB_OTG_USBRST_FLAG) + { + usbd_reset_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_USBRST_FLAG); + } + + /* sof interrupt */ + if(intsts & USB_OTG_SOF_FLAG) + { + usbd_sof_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_SOF_FLAG); + } + + /* enumeration done interrupt */ + if(intsts & USB_OTG_ENUMDONE_FLAG) + { + usbd_enumdone_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_ENUMDONE_FLAG); + } + + /* rx non-empty interrupt, indicates that there is at least one + data packet pending to be read in rx fifo */ + if(intsts & USB_OTG_RXFLVL_FLAG) + { + usbd_rxflvl_handler(udev); + } + + /* incomplete isochronous in transfer interrupt */ + if(intsts & USB_OTG_INCOMISOIN_FLAG) + { + usbd_incomisioin_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_INCOMISOIN_FLAG); + } + #ifndef USB_VBUS_IGNORE + /* disconnect detected interrupt */ + if(intsts & USB_OTG_OTGINT_FLAG) + { + uint32_t tmp = udev->usb_reg->gotgint; + if(udev->usb_reg->gotgint_bit.sesenddet) + usbd_discon_handler(udev); + udev->usb_reg->gotgint = tmp; + usb_global_clear_interrupt(usbx, USB_OTG_OTGINT_FLAG); + } +#endif + /* incomplete isochronous out transfer interrupt */ + if(intsts & USB_OTG_INCOMPIP_INCOMPISOOUT_FLAG) + { + usbd_incomisoout_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_INCOMPIP_INCOMPISOOUT_FLAG); + } + + /* resume/remote wakeup interrupt */ + if(intsts & USB_OTG_WKUP_FLAG) + { + usbd_wakeup_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_WKUP_FLAG); + } + + /* usb suspend interrupt */ + if(intsts & USB_OTG_USBSUSP_FLAG) + { + usbd_suspend_handler(udev); + usb_global_clear_interrupt(usbx, USB_OTG_USBSUSP_FLAG); + } + } +} + +/** + * @brief usb write tx fifo. + * @param udev: to the structure of usbd_core_type + * @param ept_num: endpoint number + * @retval none + */ +void usb_write_empty_txfifo(usbd_core_type *udev, uint32_t ept_num) +{ + otg_global_type *usbx = udev->usb_reg; + usb_ept_info *ept_info = &udev->ept_in[ept_num]; + uint32_t length = ept_info->total_len - ept_info->trans_len; + uint32_t wlen = 0; + + if(length > ept_info->maxpacket) + { + length = ept_info->maxpacket; + } + wlen = (length + 3) / 4; + + while((USB_INEPT(usbx, ept_num)->dtxfsts & USB_OTG_DTXFSTS_INEPTFSAV) > wlen && + (ept_info->trans_len < ept_info->total_len) && (ept_info->total_len != 0)) + { + length = ept_info->total_len - ept_info->trans_len; + if(length > ept_info->maxpacket) + { + length = ept_info->maxpacket; + } + wlen = (length + 3) / 4; + usb_write_packet(usbx, ept_info->trans_buf, ept_num, length); + + ept_info->trans_buf += length; + ept_info->trans_len += length; + + } + if(length <= 0) + { + OTG_DEVICE(usbx)->diepempmsk &= ~(0x1 << ept_num); + } +} + + +/** + * @brief usb in endpoint handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_inept_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + uint32_t ept_num = 0, ept_int; + uint32_t intsts; + + /*get all endpoint interrut */ + intsts = usb_get_all_in_interrupt(usbx); + while(intsts) + { + if(intsts & 0x1) + { + /* get endpoint interrupt flag */ + ept_int = usb_ept_in_interrupt(usbx, ept_num); + + /* transfer completed interrupt */ + if(ept_int & USB_OTG_DIEPINT_XFERC_FLAG) + { + OTG_DEVICE(usbx)->diepempmsk &= ~(1 << ept_num); + usb_ept_in_clear(usbx, ept_num , USB_OTG_DIEPINT_XFERC_FLAG); + usbd_core_in_handler(udev, ept_num); + } + + /* timeout condition interrupt */ + if(ept_int & USB_OTG_DIEPINT_TIMEOUT_FLAG) + { + usb_ept_in_clear(usbx, ept_num , USB_OTG_DIEPINT_TIMEOUT_FLAG); + } + + /* in token received when tx fifo is empty */ + if(ept_int & USB_OTG_DIEPINT_INTKNTXFEMP_FLAG) + { + usb_ept_in_clear(usbx, ept_num , USB_OTG_DIEPINT_INTKNTXFEMP_FLAG); + } + + /* in endpoint nak effective */ + if(ept_int & USB_OTG_DIEPINT_INEPTNAK_FLAG) + { + usb_ept_in_clear(usbx, ept_num , USB_OTG_DIEPINT_INEPTNAK_FLAG); + } + + /* endpoint disable interrupt */ + if(ept_int & USB_OTG_DIEPINT_EPTDISD_FLAG) + { + usb_ept_in_clear(usbx, ept_num , USB_OTG_DIEPINT_EPTDISD_FLAG); + } + + /* transmit fifo empty interrupt */ + if(ept_int & USB_OTG_DIEPINT_TXFEMP_FLAG) + { + usb_write_empty_txfifo(udev, ept_num); + } + } + ept_num ++; + intsts >>= 1; + } +} + +/** + * @brief usb out endpoint handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_outept_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + uint32_t ept_num = 0, ept_int; + uint32_t intsts; + + /* get all out endpoint interrupt */ + intsts = usb_get_all_out_interrupt(usbx); + + while(intsts) + { + if(intsts & 0x1) + { + /* get out endpoint interrupt */ + ept_int = usb_ept_out_interrupt(usbx, ept_num); + + /* transfer completed interrupt */ + if(ept_int & USB_OTG_DOEPINT_XFERC_FLAG) + { + usb_ept_out_clear(usbx, ept_num , USB_OTG_DOEPINT_XFERC_FLAG); + usbd_core_out_handler(udev, ept_num); + } + + /* setup phase done interrupt */ + if(ept_int & USB_OTG_DOEPINT_SETUP_FLAG) + { + usb_ept_out_clear(usbx, ept_num , USB_OTG_DOEPINT_SETUP_FLAG); + usbd_core_setup_handler(udev, ept_num); + if(udev->device_addr != 0) + { + OTG_DEVICE(udev->usb_reg)->dcfg_bit.devaddr = udev->device_addr; + udev->device_addr = 0; + } + } + + /* endpoint disable interrupt */ + if(ept_int & USB_OTG_DOEPINT_OUTTEPD_FLAG) + { + usb_ept_out_clear(usbx, ept_num , USB_OTG_DOEPINT_OUTTEPD_FLAG); + } + } + ept_num ++; + intsts >>= 1; + } +} + +/** + * @brief usb enumeration done handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_enumdone_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + + usb_ept0_setup(usbx); + + usbx->gusbcfg_bit.usbtrdtim = USB_TRDTIM_16; + + /* open endpoint 0 out */ + usbd_ept_open(udev, 0x00, EPT_CONTROL_TYPE, 0x40); + + /* open endpoint 0 in */ + usbd_ept_open(udev, 0x80, EPT_CONTROL_TYPE, 0x40); + + /* usb connect state set to default */ + udev->conn_state = USB_CONN_STATE_DEFAULT; + + /* clear callback */ + if(udev->class_handler->clear_handler != 0) + udev->class_handler->clear_handler(udev); +} + +/** + * @brief usb rx non-empty handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_rxflvl_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + uint32_t stsp; + uint32_t count; + uint32_t pktsts; + usb_ept_info *ept_info; + + /* disable rxflvl interrupt */ + usb_global_interrupt_enable(usbx, USB_OTG_RXFLVL_INT, FALSE); + + /* get rx status */ + stsp = usbx->grxstsp; + + /*get the byte count of receive */ + count = (stsp & USB_OTG_GRXSTSP_BCNT) >> 4; + + /* get packet status */ + pktsts = (stsp &USB_OTG_GRXSTSP_PKTSTS) >> 17; + + /* get endpoint infomation struct */ + ept_info = &udev->ept_out[stsp & USB_OTG_GRXSTSP_EPTNUM]; + + /* received out data packet */ + if(pktsts == USB_OUT_STS_DATA) + { + if(count != 0) + { + /* read packet to buffer */ + usb_read_packet(usbx, ept_info->trans_buf, (stsp & USB_OTG_GRXSTSP_EPTNUM), count); + ept_info->trans_buf += count; + ept_info->trans_len += count; + + } + } + /* setup data received */ + else if ( pktsts == USB_SETUP_STS_DATA) + { + /* read packet to buffer */ + usb_read_packet(usbx, udev->setup_buffer, (stsp & USB_OTG_GRXSTSP_EPTNUM), count); + ept_info->trans_len += count; + } + + /* enable rxflvl interrupt */ + usb_global_interrupt_enable(usbx, USB_OTG_RXFLVL_INT, TRUE); + +} + +/** + * @brief usb disconnect handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_discon_handler(usbd_core_type *udev) +{ + /* disconnect callback handler */ + if(udev->class_handler->event_handler != 0) + udev->class_handler->event_handler(udev, USBD_DISCONNECT_EVNET); +} + + +/** + * @brief usb incomplete out handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_incomisoout_handler(usbd_core_type *udev) +{ + if(udev->class_handler->event_handler != 0) + udev->class_handler->event_handler(udev, USBD_OUTISOINCOM_EVENT); +} + +/** + * @brief usb incomplete in handler + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_incomisioin_handler(usbd_core_type *udev) +{ + if(udev->class_handler->event_handler != 0) + udev->class_handler->event_handler(udev, USBD_INISOINCOM_EVENT); +} + +/** + * @brief usb device reset interrupt request handler. + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_reset_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + otg_device_type *dev = OTG_DEVICE(usbx); + uint32_t i_index = 0; + + /* disable remote wakeup singal */ + dev->dctl_bit.rwkupsig = FALSE; + + /* endpoint fifo alloc */ + usbd_fifo_alloc(udev); + + /* flush all tx fifo */ + usb_flush_tx_fifo(usbx, 0x10); + + /* clear in and out endpoint interrupt flag */ + for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++) + { + USB_INEPT(usbx, i_index)->diepint = 0xFF; + USB_OUTEPT(usbx, i_index)->doepint = 0xFF; + } + + /* clear endpoint flag */ + dev->daint = 0xFFFFFFFF; + + /*clear endpoint interrupt mask */ + dev->daintmsk = 0x10001; + + /* enable out endpoint xfer, eptdis, setup interrupt mask */ + dev->doepmsk_bit.xfercmsk = TRUE; + dev->doepmsk_bit.eptdismsk = TRUE; + dev->doepmsk_bit.setupmsk = TRUE; + + /* enable in endpoint xfer, eptdis, timeout interrupt mask */ + dev->diepmsk_bit.xfercmsk = TRUE; + dev->diepmsk_bit.eptdismsk = TRUE; + dev->diepmsk_bit.timeoutmsk = TRUE; + + /* set device address to 0 */ + usb_set_address(usbx, 0); + + /* enable endpoint 0 */ + usb_ept0_start(usbx); + + /* usb connect state set to default */ + udev->conn_state = USB_CONN_STATE_DEFAULT; + + /* user define reset event */ + if(udev->class_handler->event_handler) + udev->class_handler->event_handler(udev, USBD_RESET_EVENT); +} + +/** + * @brief usb device sof interrupt request handler. + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_sof_handler(usbd_core_type *udev) +{ + /* user sof handler in class define */ + if(udev->class_handler->sof_handler) + udev->class_handler->sof_handler(udev); +} + +/** + * @brief usb device suspend interrupt request handler. + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_suspend_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + + if(OTG_DEVICE(usbx)->dsts_bit.suspsts) + { + /* save connect state */ + udev->old_conn_state = udev->conn_state; + + /* set current state to suspend */ + udev->conn_state = USB_CONN_STATE_SUSPENDED; + + /* enter suspend mode */ + usbd_enter_suspend(udev); + + /* user suspend handler */ + if(udev->class_handler->event_handler != 0) + udev->class_handler->event_handler(udev, USBD_SUSPEND_EVENT); + } +} + +/** + * @brief usb device wakup interrupt request handler. + * @param udev: to the structure of usbd_core_type + * @retval none + */ +void usbd_wakeup_handler(usbd_core_type *udev) +{ + otg_global_type *usbx = udev->usb_reg; + + /* clear remote wakeup bit */ + OTG_DEVICE(usbx)->dctl_bit.rwkupsig = FALSE; + + /* exit suspend mode */ + usb_open_phy_clk(udev->usb_reg); + + /* restore connect state */ + udev->conn_state = udev->old_conn_state; + + /* user suspend handler */ + if(udev->class_handler->event_handler != 0) + udev->class_handler->event_handler(udev, USBD_WAKEUP_EVENT); +} +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/src/main/drivers/at32/usb/usbd_int.h b/src/main/drivers/at32/usb/usbd_int.h new file mode 100644 index 0000000000..64845fe749 --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_int.h @@ -0,0 +1,80 @@ +/** + ************************************************************************** + * @file usbd_int.h + * @brief usb interrupt header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBD_INT_H +#define __USBD_INT_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @addtogroup USBD_drivers_int + * @{ + */ + +/** @defgroup USBD_interrupt_exported_types + * @{ + */ +/* includes ------------------------------------------------------------------*/ +#include "usbd_core.h" +#include "usb_core.h" + +void usbd_irq_handler(otg_core_type *udev); +void usbd_ept_handler(usbd_core_type *udev); +void usbd_reset_handler(usbd_core_type *udev); +void usbd_sof_handler(usbd_core_type *udev); +void usbd_suspend_handler(usbd_core_type *udev); +void usbd_wakeup_handler(usbd_core_type *udev); +void usbd_inept_handler(usbd_core_type *udev); +void usbd_outept_handler(usbd_core_type *udev); +void usbd_enumdone_handler(usbd_core_type *udev); +void usbd_rxflvl_handler(usbd_core_type *udev); +void usbd_incomisioin_handler(usbd_core_type *udev); +void usbd_discon_handler(usbd_core_type *udev); +void usbd_incomisoout_handler(usbd_core_type *udev); +void usb_write_empty_txfifo(usbd_core_type *udev, uint32_t ept_num); + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/src/main/drivers/at32/usb/usbd_sdr.c b/src/main/drivers/at32/usb/usbd_sdr.c new file mode 100644 index 0000000000..819188d03d --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_sdr.c @@ -0,0 +1,535 @@ +/** + ************************************************************************** + * @file usbd_sdr.c + * @brief usb standard device request + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usbd_sdr.h" + +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @defgroup USBD_drivers_standard_request + * @brief usb device standard_request + * @{ + */ + +/** @defgroup USBD_sdr_private_functions + * @{ + */ + +static usb_sts_type usbd_get_descriptor(usbd_core_type *udev); +static usb_sts_type usbd_set_address(usbd_core_type *udev); +static usb_sts_type usbd_get_status(usbd_core_type *udev); +static usb_sts_type usbd_clear_feature(usbd_core_type *udev); +static usb_sts_type usbd_set_feature(usbd_core_type *udev); +static usb_sts_type usbd_get_configuration(usbd_core_type *udev); +static usb_sts_type usbd_set_configuration(usbd_core_type *udev); + +/** + * @brief usb parse standard setup request + * @param setup: setup structure + * @param buf: setup buffer + * @retval none + */ +void usbd_setup_request_parse(usb_setup_type *setup, uint8_t *buf) +{ + setup->bmRequestType = *(uint8_t *) buf; + setup->bRequest = *(uint8_t *) (buf + 1); + setup->wValue = SWAPBYTE(buf + 2); + setup->wIndex = SWAPBYTE(buf + 4); + setup->wLength = SWAPBYTE(buf + 6); +} + +/** + * @brief get usb standard device description request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_get_descriptor(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + uint16_t len = 0; + usbd_desc_t *desc = NULL; + uint8_t desc_type = udev->setup.wValue >> 8; + switch(desc_type) + { + case USB_DESCIPTOR_TYPE_DEVICE: + desc = udev->desc_handler->get_device_descriptor(); + break; + case USB_DESCIPTOR_TYPE_CONFIGURATION: + desc = udev->desc_handler->get_device_configuration(); + break; + case USB_DESCIPTOR_TYPE_STRING: + { + uint8_t str_desc = (uint8_t)udev->setup.wValue; + switch(str_desc) + { + case USB_LANGID_STRING: + desc = udev->desc_handler->get_device_lang_id(); + break; + case USB_MFC_STRING: + desc = udev->desc_handler->get_device_manufacturer_string(); + break; + case USB_PRODUCT_STRING: + desc = udev->desc_handler->get_device_product_string(); + break; + case USB_SERIAL_STRING: + desc = udev->desc_handler->get_device_serial_string(); + break; + case USB_CONFIG_STRING: + desc = udev->desc_handler->get_device_config_string(); + break; + case USB_INTERFACE_STRING: + desc = udev->desc_handler->get_device_interface_string(); + break; + default: + udev->class_handler->setup_handler(udev, &udev->setup); + return ret; + } + break; + } + case USB_DESCIPTOR_TYPE_DEVICE_QUALIFIER: + usbd_ctrl_unsupport(udev); + break; + case USB_DESCIPTOR_TYPE_OTHER_SPEED: + usbd_ctrl_unsupport(udev); + return ret; + default: + usbd_ctrl_unsupport(udev); + return ret; + } + + if(desc != NULL) + { + if((desc->length != 0) && (udev->setup.wLength != 0)) + { + len = MIN(desc->length , udev->setup.wLength); + usbd_ctrl_send(udev, desc->descriptor, len); + } + } + return ret; +} + +/** + * @brief this request sets the device address + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_set_address(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + uint8_t dev_addr; + + /* if wIndex or wLength are non-zero, then the behavior of + the device is not specified + */ + if(setup->wIndex == 0 && setup->wLength == 0) + { + dev_addr = (uint8_t)(setup->wValue) & 0x7f; + + /* device behavior when this request is received + while the device is in the configured state is not specified.*/ + if(udev->conn_state == USB_CONN_STATE_CONFIGURED ) + { + usbd_ctrl_unsupport(udev); + } + else + { + udev->device_addr = dev_addr; + + if(dev_addr != 0) + { + udev->conn_state = USB_CONN_STATE_ADDRESSED; + } + else + { + udev->conn_state = USB_CONN_STATE_DEFAULT; + } + usbd_ctrl_send_status(udev); + } + } + else + { + usbd_ctrl_unsupport(udev); + } + return ret; +} + +/** + * @brief get usb status request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_get_status(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + case USB_CONN_STATE_CONFIGURED: + if(udev->remote_wakup) + { + udev->config_status |= USB_CONF_REMOTE_WAKEUP; + } + usbd_ctrl_send(udev, (uint8_t *)(&udev->config_status), 2); + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + return ret; +} + +/** + * @brief clear usb feature request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_clear_feature(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + case USB_CONN_STATE_CONFIGURED: + if(setup->wValue == USB_FEATURE_REMOTE_WAKEUP) + { + udev->remote_wakup = 0; + udev->config_status &= ~USB_CONF_REMOTE_WAKEUP; + udev->class_handler->setup_handler(udev, &udev->setup); + usbd_ctrl_send_status(udev); + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + return ret; +} + +/** + * @brief set usb feature request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_set_feature(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + if(setup->wValue == USB_FEATURE_REMOTE_WAKEUP) + { + udev->remote_wakup = 1; + udev->class_handler->setup_handler(udev, &udev->setup); + usbd_ctrl_send_status(udev); + } + return ret; +} + +/** + * @brief get usb configuration request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_get_configuration(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + if(setup->wLength != 1) + { + usbd_ctrl_unsupport(udev); + } + else + { + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + udev->default_config = 0; + usbd_ctrl_send(udev, (uint8_t *)(&udev->default_config), 1); + break; + case USB_CONN_STATE_CONFIGURED: + usbd_ctrl_send(udev, (uint8_t *)(&udev->dev_config), 1); + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + } + return ret; +} + +/** + * @brief sets the usb device configuration request + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +static usb_sts_type usbd_set_configuration(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + static uint8_t config_value; + usb_setup_type *setup = &udev->setup; + config_value = (uint8_t)setup->wValue; + + if(setup->wIndex == 0 && setup->wLength == 0) + { + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + if(config_value) + { + udev->dev_config = config_value; + udev->conn_state = USB_CONN_STATE_CONFIGURED; + udev->class_handler->init_handler(udev); + usbd_ctrl_send_status(udev); + } + else + { + usbd_ctrl_send_status(udev); + } + + break; + case USB_CONN_STATE_CONFIGURED: + if(config_value == 0) + { + udev->conn_state = USB_CONN_STATE_ADDRESSED; + udev->dev_config = config_value; + udev->class_handler->clear_handler(udev); + usbd_ctrl_send_status(udev); + } + else if(config_value == udev->dev_config) + { + udev->class_handler->clear_handler(udev); + udev->dev_config = config_value; + udev->class_handler->init_handler(udev); + usbd_ctrl_send_status(udev); + } + else + { + usbd_ctrl_send_status(udev); + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + } + else + { + usbd_ctrl_unsupport(udev); + } + return ret; +} + +/** + * @brief standard usb device requests + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +usb_sts_type usbd_device_request(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + if((setup->bmRequestType & USB_REQ_TYPE_RESERVED) != USB_REQ_TYPE_STANDARD) + { + udev->class_handler->setup_handler(udev, &udev->setup); + return ret; + } + switch(udev->setup.bRequest) + { + case USB_STD_REQ_GET_STATUS: + usbd_get_status(udev); + break; + case USB_STD_REQ_CLEAR_FEATURE: + usbd_clear_feature(udev); + break; + case USB_STD_REQ_SET_FEATURE: + usbd_set_feature(udev); + break; + case USB_STD_REQ_SET_ADDRESS: + usbd_set_address(udev); + break; + case USB_STD_REQ_GET_DESCRIPTOR: + usbd_get_descriptor(udev); + break; + case USB_STD_REQ_GET_CONFIGURATION: + usbd_get_configuration(udev); + break; + case USB_STD_REQ_SET_CONFIGURATION: + usbd_set_configuration(udev); + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + return ret; +} + +/** + * @brief standard usb interface requests + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +usb_sts_type usbd_interface_request(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + switch(udev->conn_state) + { + case USB_CONN_STATE_CONFIGURED: + udev->class_handler->setup_handler(udev, &udev->setup); + if(setup->wLength == 0) + { + usbd_ctrl_send_status(udev); + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + return ret; +} + +/** + * @brief standard usb endpoint requests + * @param udev: to the structure of usbd_core_type + * @retval status of usb_sts_type + */ +usb_sts_type usbd_endpoint_request(usbd_core_type *udev) +{ + usb_sts_type ret = USB_OK; + usb_setup_type *setup = &udev->setup; + uint8_t ept_addr = LBYTE(setup->wIndex); + usb_ept_info *ept_info; + + if((setup->bmRequestType & USB_REQ_TYPE_RESERVED) == USB_REQ_TYPE_CLASS) + { + udev->class_handler->setup_handler(udev, &udev->setup); + } + switch(setup->bRequest) + { + case USB_STD_REQ_GET_STATUS: + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + if((ept_addr & 0x7F) != 0) + { + usbd_set_stall(udev, ept_addr); + } + break; + case USB_CONN_STATE_CONFIGURED: + { + if((ept_addr & 0x80) != 0) + { + ept_info = &udev->ept_in[ept_addr & 0x7F]; + } + else + { + ept_info = &udev->ept_out[ept_addr & 0x7F]; + } + if(ept_info->stall == 1) + { + ept_info->status = 0x0001; + } + else + { + ept_info->status = 0x0000; + } + usbd_ctrl_send(udev, (uint8_t *)(&ept_info->status), 2); + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + break; + case USB_STD_REQ_CLEAR_FEATURE: + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + if((ept_addr != 0x00) && (ept_addr != 0x80)) + { + usbd_set_stall(udev, ept_addr); + } + break; + case USB_CONN_STATE_CONFIGURED: + if(setup->wValue == USB_FEATURE_EPT_HALT) + { + if((ept_addr & 0x7F) != 0x00 ) + { + usbd_clear_stall(udev, ept_addr); + udev->class_handler->setup_handler(udev, &udev->setup); + } + usbd_ctrl_send_status(udev); + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + break; + case USB_STD_REQ_SET_FEATURE: + switch(udev->conn_state) + { + case USB_CONN_STATE_ADDRESSED: + if((ept_addr != 0x00) && (ept_addr != 0x80)) + { + usbd_set_stall(udev, ept_addr); + } + break; + case USB_CONN_STATE_CONFIGURED: + if(setup->wValue == USB_FEATURE_EPT_HALT) + { + if((ept_addr != 0x00) && (ept_addr != 0x80)) + { + usbd_set_stall(udev, ept_addr); + } + } + udev->class_handler->setup_handler(udev, &udev->setup); + usbd_ctrl_send_status(udev); + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + break; + default: + usbd_ctrl_unsupport(udev); + break; + } + return ret; +} +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/src/main/drivers/at32/usb/usbd_sdr.h b/src/main/drivers/at32/usb/usbd_sdr.h new file mode 100644 index 0000000000..212c5f162d --- /dev/null +++ b/src/main/drivers/at32/usb/usbd_sdr.h @@ -0,0 +1,71 @@ +/** + ************************************************************************** + * @file usb_sdr.h + * @brief usb header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USB_SDR_H +#define __USB_SDR_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/* includes ------------------------------------------------------------------*/ +#include "usb_conf.h" +#include "usb_core.h" +/** @addtogroup AT32F435_437_middlewares_usbd_drivers + * @{ + */ + +/** @addtogroup USBD_drivers_standard_request + * @{ + */ + +/** @defgroup USBD_sdr_exported_functions + * @{ + */ + + +void usbd_setup_request_parse(usb_setup_type *setup, uint8_t *buf); +usb_sts_type usbd_device_request(usbd_core_type *udev); +usb_sts_type usbd_interface_request(usbd_core_type *udev); +usb_sts_type usbd_endpoint_request(usbd_core_type *udev); + + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usbh_core.c b/src/main/drivers/at32/usb/usbh_core.c new file mode 100644 index 0000000000..0614b2513d --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_core.c @@ -0,0 +1,1244 @@ +/** + ************************************************************************** + * @file usbh_core.c + * @brief usb host driver + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usbh_core.h" +#include "usb_core.h" +#include "usbh_ctrl.h" +#include "usb_conf.h" + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @defgroup USBH_drivers_core + * @brief usb host drivers core + * @{ + */ + +/** @defgroup USBH_core_private_functions + * @{ + */ + +static void usbh_attached(usbh_core_type *uhost); +static void usbh_enumeration(usbh_core_type *uhost); +static void usbh_class_request(usbh_core_type *uhost); +static void usbh_class(usbh_core_type *uhost); +static void usbh_suspend(usbh_core_type *uhost); +static void usbh_wakeup(usbh_core_type *uhost); +static void usbh_disconnect(usbh_core_type *uhost); +/** + * @brief usb host free channel + * @param uhost: to the structure of usbh_core_type + * @param index: channle number + * @retval none + */ +void usbh_free_channel(usbh_core_type *uhost, uint8_t index) +{ + if(index < USB_HOST_CHANNEL_NUM) + { + /* free host channel */ + uhost->channel[index] = 0x0; + } +} + +/** + * @brief get usb host free channel + * @param uhost: to the structure of usbh_core_type + * @retval channel index + */ +uint16_t usbh_get_free_channel(usbh_core_type *uhost) +{ + uint16_t i_index = 0; + for(i_index = 0; i_index < USB_HOST_CHANNEL_NUM; i_index ++) + { + /* find unuse channel */ + if((uhost->channel[i_index] & HCH_USED) == 0) + { + /* return channel index */ + return i_index; + } + } + return HCH_ERROR; +} + + +/** + * @brief usb host set toggle + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param toggle: toggle value + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_set_toggle(usbh_core_type *uhost, uint8_t hc_num, uint8_t toggle) +{ + if(uhost->hch[hc_num].dir) + { + /* direction in */ + uhost->hch[hc_num].toggle_in = toggle; + } + else + { + /* direction out */ + uhost->hch[hc_num].toggle_out = toggle; + } + return USB_OK; +} + +/** + * @brief usb host in out request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_in_out_request(usbh_core_type *uhost, uint8_t hc_num) +{ + usb_sts_type status = USB_OK; + uint32_t n_packet = 0; + uint32_t num_words = 0; + uint32_t tmp; + otg_global_type *usbx = uhost->usb_reg; + otg_hchannel_type *ch = USB_CHL(uhost->usb_reg, hc_num); + + /* set usb request block to idle */ + uhost->urb_state[hc_num] = URB_IDLE; + uhost->hch[hc_num].state = HCH_IDLE; + + /* set usb channel transmit count to zero */ + uhost->hch[hc_num].trans_count = 0; + + /* check transmit data len */ + if(uhost->hch[hc_num].trans_len > 0) + { + /* count how many packet need to send */ + n_packet = (uhost->hch[hc_num].trans_len + \ + uhost->hch[hc_num].maxpacket - 1) / \ + uhost->hch[hc_num].maxpacket; + + /* packet count max 256 */ + if(n_packet > 256) + { + n_packet = 256; + uhost->hch[hc_num].trans_len = n_packet * uhost->hch[hc_num].maxpacket; + } + } + else + { + /* zero data len */ + n_packet = 1; + } + + /* direction is in */ + if(uhost->hch[hc_num].dir) + { + uhost->hch[hc_num].trans_len = n_packet * uhost->hch[hc_num].maxpacket; + } + + /* set transfer information to channel register */ + ch->hctsiz = (uhost->hch[hc_num].trans_len & USB_OTG_HCTSIZ_XFERSIZE) | + ((n_packet << 19) & USB_OTG_HCTSIZ_PKTCNT) | + ((uhost->hch[hc_num].data_pid << 29) & USB_OTG_HCTSIZ_PID); + + /* set odd frame */ + ch->hcchar_bit.oddfrm = !(OTG_HOST(uhost->usb_reg)->hfnum & 0x1); + + /* clear channel disable bit and enable channel */ + tmp = ch->hcchar; + tmp &= ~(USB_OTG_HCCHAR_CHDIS); + tmp |= USB_OTG_HCCHAR_CHENA; + ch->hcchar = tmp; + + /* channel direction is out and transfer len > 0 */ + if((uhost->hch[hc_num].dir == 0) && + (uhost->hch[hc_num].trans_len > 0 )) + { + switch(uhost->hch[hc_num].ept_type) + { + case EPT_CONTROL_TYPE: + case EPT_BULK_TYPE: + num_words = (uhost->hch[hc_num].trans_len + 3) / 4; + + /* non-periodic transfer */ + if(num_words > usbx->gnptxsts_bit.nptxfspcavail) + { + usbx->gintmsk_bit.nptxfempmsk = 1; + } + break; + case EPT_ISO_TYPE: + case EPT_INT_TYPE: + num_words = (uhost->hch[hc_num].trans_len + 3) / 4; + + /* periodic transfer */ + if(num_words > OTG_HOST(usbx)->hptxsts_bit.ptxfspcavil) + { + usbx->gintmsk_bit.ptxfempmsk = 1; + } + break; + default: + break; + } + /* write data to fifo */ + usb_write_packet(usbx, uhost->hch[hc_num].trans_buf, + hc_num, uhost->hch[hc_num].trans_len); + } + + return status; +} + +/** + * @brief usb host interrupt receive request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: receive buffer + * @param length: receive length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_interrupt_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is in */ + uhost->hch[hc_num].dir = 1; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + if(uhost->hch[hc_num].toggle_in == 0) + { + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + } + else + { + /* pid: data1 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + } + + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host interrupt send request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: send buffer + * @param length: send length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_interrupt_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is out */ + uhost->hch[hc_num].dir = 0; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + if(uhost->hch[hc_num].toggle_out == 0) + { + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + } + else + { + /* pid: data1 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + } + + return usbh_in_out_request(uhost, hc_num); +} + + +/** + * @brief usb host bulk receive request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: receive buffer + * @param length: receive length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_bulk_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is in */ + uhost->hch[hc_num].dir = 1; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + if(uhost->hch[hc_num].toggle_in == 0) + { + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + } + else + { + /* pid: data1 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + } + + return usbh_in_out_request(uhost, hc_num); +} + + +/** + * @brief usb host bulk send request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: receive buffer + * @param length: receive length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_bulk_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is out */ + uhost->hch[hc_num].dir = 0; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + if(uhost->hch[hc_num].toggle_out == 0) + { + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + } + else + { + /* pid: data1 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + } + + return usbh_in_out_request(uhost, hc_num); +} + + +/** + * @brief usb host iso send request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: send buffer + * @param length: send length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_isoc_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is out */ + uhost->hch[hc_num].dir = 0; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host iso receive request + * @param uhost: to the structure of usbh_core_type + * @param hc_num: channel number + * @param buffer: receive buffer + * @param length: receive length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_isoc_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length) +{ + /* set direction is in */ + uhost->hch[hc_num].dir = 1; + + /* set transfer buffer */ + uhost->hch[hc_num].trans_buf = buffer; + + /* set transfer len*/ + uhost->hch[hc_num].trans_len = length; + + /* pid: data0 */ + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host cfg default init + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_cfg_default_init(usbh_core_type *uhost) +{ + /* set global state to idle */ + uhost->global_state = USBH_IDLE; + + /* enumeration state to get description */ + uhost->enum_state = ENUM_GET_MIN_DESC; + + /* request state send */ + uhost->req_state = CMD_SEND; + + /* control transfer state is idle*/ + uhost->ctrl.state = CONTROL_IDLE; + + /* defaut endpoint 0 max size is 8byte */ + uhost->ctrl.ept0_size = 8; + + /* default device address is 0 */ + uhost->dev.address = 0; + + /* default speed is full speed */ + uhost->dev.speed = USB_FULL_SPEED_CORE_ID; + + uhost->timer = 0; + + uhost->ctrl.err_cnt = 0; + + /* free all channel */ + usbh_free_channel(uhost, uhost->ctrl.hch_in); + usbh_free_channel(uhost, uhost->ctrl.hch_out); + return USB_OK; +} + +/** + * @brief usb host enter suspend + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_enter_suspend(usbh_core_type *uhost) +{ + otg_host_type *host = OTG_HOST(uhost->usb_reg); + uint32_t hprt_val = host->hprt; + + hprt_val &= ~(USB_OTG_HPRT_PRTENA | USB_OTG_HPRT_PRTENCHNG | + USB_OTG_HPRT_PRTOVRCACT | USB_OTG_HPRT_PRTCONDET); + + /* set port suspend */ + host->hprt = hprt_val | USB_OTG_HPRT_PRTSUSP; + + /* stop phy clock */ + usb_stop_phy_clk(uhost->usb_reg); + +} + +/** + * @brief usb host resume + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_resume(usbh_core_type *uhost) +{ + otg_host_type *host = OTG_HOST(uhost->usb_reg); + uint32_t temp = host->hprt; + + /* open phy clock */ + usb_open_phy_clk(uhost->usb_reg); + + /* clear port suspend and set port resume*/ + temp &= ~(USB_OTG_HPRT_PRTENA | USB_OTG_HPRT_PRTENCHNG | + USB_OTG_HPRT_PRTOVRCACT | USB_OTG_HPRT_PRTCONDET + | USB_OTG_HPRT_PRTSUSP); + host->hprt = temp | USB_OTG_HPRT_PRTRES; + + /* delay 20 ms */ + usb_delay_ms(20); + + /*clear port resume */ + temp = host->hprt; + temp &= ~(USB_OTG_HPRT_PRTENA | USB_OTG_HPRT_PRTENCHNG | + USB_OTG_HPRT_PRTOVRCACT | USB_OTG_HPRT_PRTCONDET + | USB_OTG_HPRT_PRTRES); + host->hprt = temp; + usb_delay_ms(5); +} + +/** + * @brief usb host core initialize + * @param uhost: to the structure of usbh_core_type + * @param usb_reg: usb otgfs peripheral global register + * this parameter can be one of the following values: + * OTG1_GLOBAL , OTG2_GLOBAL + * @param class_handler: usb host class handler type pointer + * @param user_handler: usb host user handler type pointer + * @param core_id: usb core select id + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_core_init(usbh_core_type *uhost, + usb_reg_type *usb_reg, + usbh_class_handler_type *class_handler, + usbh_user_handler_type *user_handler, + uint8_t core_id) +{ + UNUSED(core_id); + + usb_sts_type status = USB_OK; + uint32_t i_index; + otg_global_type *usbx = usb_reg; + otg_host_type *host = OTG_HOST(usbx); + uhost->usb_reg = usb_reg; + + /* host class handler */ + uhost->class_handler = class_handler; + uhost->user_handler = user_handler; + + /* host user handler */ + uhost->user_handler->user_init(); + + uhost->timer = 0; + + /* usb host cfg default init */ + usbh_cfg_default_init(uhost); + + /* clear host config to default value */ + for(i_index = 0; i_index < USB_HOST_CHANNEL_NUM; i_index ++) + { + uhost->err_cnt[i_index] = 0; + uhost->xfer_cnt[i_index] = 0; + uhost->hch_state[i_index] = HCH_IDLE; + uhost->hch[0].maxpacket = 8; + } + + /* no device connect */ + uhost->conn_sts = 0; + + /* disable usb interrupt */ + usb_interrupt_disable(usbx); + + /* usb global init */ + usb_global_init(usbx); + + /* set usb host mode */ + usb_global_set_mode(usbx, OTG_HOST_MODE); + + /* open usb phy clock*/ + usb_open_phy_clk(usbx); + + /* clock select */ + usbh_fsls_clksel(usbx, USB_HCFG_CLK_48M); + + /* set support ls and fs device */ + host->hcfg_bit.fslssupp = 0; + + if(usbx == OTG1_GLOBAL) + { + /* set receive fifo size */ + usbx->grxfsiz = USBH_RX_FIFO_SIZE; + + /* set non-periodic transmit fifo start address and depth */ + usbx->gnptxfsiz_ept0tx_bit.nptxfstaddr = USBH_RX_FIFO_SIZE; + usbx->gnptxfsiz_ept0tx_bit.nptxfdep = USBH_NP_TX_FIFO_SIZE; + + /* set periodic transmit fifo start address and depth */ + usbx->hptxfsiz_bit.ptxfstaddr = USBH_RX_FIFO_SIZE + USBH_NP_TX_FIFO_SIZE; + usbx->hptxfsiz_bit.ptxfsize = USBH_P_TX_FIFO_SIZE; + } +#ifdef OTG2_GLOBAL + if(usbx == OTG2_GLOBAL) + { + /* set receive fifo size */ + usbx->grxfsiz = USBH2_RX_FIFO_SIZE; + + /* set non-periodic transmit fifo start address and depth */ + usbx->gnptxfsiz_ept0tx_bit.nptxfstaddr = USBH2_RX_FIFO_SIZE; + usbx->gnptxfsiz_ept0tx_bit.nptxfdep = USBH2_NP_TX_FIFO_SIZE; + + /* set periodic transmit fifo start address and depth */ + usbx->hptxfsiz_bit.ptxfstaddr = USBH2_RX_FIFO_SIZE + USBH2_NP_TX_FIFO_SIZE; + usbx->hptxfsiz_bit.ptxfsize = USBH2_P_TX_FIFO_SIZE; + } +#endif + /* flush tx fifo */ + usb_flush_tx_fifo(usbx, 16); + + /* flush rx fifo */ + usb_flush_rx_fifo(usbx); + + /* clear host channel interrut mask and status */ + for(i_index = 0; i_index < USB_HOST_CHANNEL_NUM; i_index ++) + { + USB_CHL(usbx, i_index)->hcintmsk = 0; + USB_CHL(usbx, i_index)->hcint = 0xFFFFFFFF; + } + + /* power on to this port */ + usb_port_power_on(usbx, TRUE); + + /* clear global interrupt mask and status */ + usbx->gintmsk = 0; + usbx->gintsts = 0xBFFFFFFF; + + /* set global interrut mask */ + usbx->gintmsk = USB_OTG_SOF_INT | USB_OTG_RXFLVL_INT | + USB_OTG_USBSUSP_INT | USB_OTG_PRT_INT | + USB_OTG_HCH_INT | USB_OTG_INCOMISOIN_INT | + USB_OTG_INCOMPIP_INCOMPISOOUT_INT | USB_OTG_WKUP_INT | + USB_OTG_DISCON_INT; + + /* enable usb global interrupt */ + usb_interrupt_enable(usbx); + + /* active vbus */ + usbh_active_vbus(uhost, TRUE); + return status; +} + +/** + * @brief usb host open channel + * @param uhost: to the structure of usbh_core_type + * @param chn: host channel number + * @param ept_num: devvice endpoint number + * @param dev_address: device address + * @param type: channel transfer type + * this parameter can be one of the following values: + * - EPT_CONTROL_TYPE + * - EPT_BULK_TYPE + * - EPT_INT_TYPE + * - EPT_ISO_TYPE + * @param maxpacket: support max packe size for this channel + * @param speed: device speed + * this parameter can be one of the following values: + * - USB_PRTSPD_FULL_SPEED + * - USB_PRTSPD_LOW_SPEED + * @param ept_addr: endpoint address + * @retval usb_sts_type + */ +void usbh_hc_open(usbh_core_type *uhost, + uint8_t chn, + uint8_t ept_num, + uint8_t dev_address, + uint8_t type, + uint16_t maxpacket, + uint8_t speed) +{ + /* device address */ + uhost->hch[chn].address = dev_address; + + /* device speed */ + uhost->hch[chn].speed = speed; + + /* endpoint transfer type */ + uhost->hch[chn].ept_type = type; + + /* endpoint support maxpacket */ + uhost->hch[chn].maxpacket = maxpacket; + + /* endpoint direction in or out */ + uhost->hch[chn].dir = (ept_num & 0x80)?1:0;; + + /* host channel number */ + uhost->hch[chn].ch_num = chn; + + /* device endpoint number */ + uhost->hch[chn].ept_num = ept_num; + + /* enable channel */ + usb_hc_enable(uhost->usb_reg, chn, + ept_num, dev_address, + type, maxpacket, speed + ); +} + +/** + * @brief disable host channel + * @param usbx: to select the otgfs peripheral. + * this parameter can be one of the following values: + * - OTG1_GLOBAL + * - OTG2_GLOBAL + * @param chn: channel number + * @retval none + */ +void usbh_ch_disable(usbh_core_type *uhost, uint8_t chn) +{ + usb_hch_halt(uhost->usb_reg, chn); +} + +/** + * @brief usb host alloc channel + * @param uhost: to the structure of usbh_core_type + * @param ept_addr: endpoint address + * @retval usb_sts_type + */ +uint16_t usbh_alloc_channel(usbh_core_type *uhost, uint8_t ept_addr) +{ + /* get one free channel */ + uint16_t ch_num = usbh_get_free_channel(uhost); + + if(ch_num == HCH_ERROR) + return USB_FAIL; + + /* set channel to used */ + uhost->channel[ch_num] = HCH_USED | ept_addr; + return ch_num; +} + +/** + * @brief usb host get urb status + * @param uhost: to the structure of usbh_core_type + * @param ch_num: channel number + * @retval urb_sts_type: urb status + */ +urb_sts_type usbh_get_urb_status(usbh_core_type *uhost, uint8_t ch_num) +{ + return uhost->urb_state[ch_num]; +} +/** + * @brief usb wait control setup complete + * @param uhost: to the structure of usbh_core_type + * @param next_ctrl_state: next ctrl state when setup complete + * @param next_enum_state: next enum state when setup complete + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_result_check(usbh_core_type *uhost, ctrl_ept0_sts_type next_ctrl_state, uint8_t next_enum_state) +{ + usb_sts_type status; + + /* control transfer loop */ + status = usbh_ctrl_transfer_loop(uhost); + + if(status == USB_OK) + { + uhost->ctrl.state = next_ctrl_state; + uhost->enum_state = next_enum_state; + uhost->req_state = CMD_SEND; + } + else if(status == USB_ERROR) + { + uhost->ctrl.state = CONTROL_IDLE; + uhost->req_state = CMD_SEND; + } + else if(status == USB_NOT_SUPPORT) + { + uhost->ctrl.state = next_ctrl_state; + uhost->enum_state = next_enum_state; + uhost->req_state = CMD_SEND; + } + return status; +} + +/** + * @brief auto alloc address (1...20) + * @param none + * @retval address (1...20) + */ +uint8_t usbh_alloc_address(void) +{ + static uint8_t address = 1; + if(address == 20) + address = 1; + return address ++; +} + + +/** + * @brief usb host enumeration handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_enum_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_WAIT; + switch(uhost->enum_state) + { + case ENUM_IDLE: + break; + case ENUM_GET_MIN_DESC: + /* get description */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_device_descriptor(uhost, 8); + } + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_FULL_DESC) == USB_OK) + { + usbh_parse_dev_desc(uhost, uhost->rx_buffer, 8); + + /* set new control endpoint maxpacket size */ + uhost->ctrl.ept0_size = (uhost->dev).dev_desc.bMaxPacketSize0; + + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_in,0x80, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_out,0x00, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + } + break; + + case ENUM_GET_FULL_DESC: + /* get description */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_device_descriptor(uhost, 18); + } + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_SET_ADDR) == USB_OK) + { + usbh_parse_dev_desc(uhost, uhost->rx_buffer, 18); + //USBH_DEBUG("VID: %xh", uhost->dev.dev_desc.idVendor); + //USBH_DEBUG("PID: %xh", uhost->dev.dev_desc.idProduct); + } + break; + + case ENUM_SET_ADDR: + /* set device address */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + uhost->dev.address = usbh_alloc_address(); + //USBH_DEBUG("Set Address: %d", uhost->dev.address); + usbh_set_address(uhost, uhost->dev.address); + } + if (usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_CFG) == USB_OK) + { + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_in,0x80, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_out,0x00, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + } + break; + + case ENUM_GET_CFG: + /* get device confiuration */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_configure_descriptor(uhost, 9); + } + + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_FULL_CFG) == USB_OK) + { + usbh_parse_configure_desc(uhost, uhost->rx_buffer, 9); + } + break; + + case ENUM_GET_FULL_CFG: + /* get device confiuration */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_configure_descriptor(uhost, uhost->dev.cfg_desc.cfg.wTotalLength); + } + + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_MFC_STRING) == USB_OK) + { + usbh_parse_configure_desc(uhost, uhost->rx_buffer, uhost->dev.cfg_desc.cfg.wTotalLength); + } + break; + + case ENUM_GET_MFC_STRING: + /* get device mfc string */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_sting_descriptor(uhost, uhost->dev.dev_desc.iManufacturer, + uhost->rx_buffer, 0xFF); + } + + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_PRODUCT_STRING) == USB_OK) + { + usbh_parse_string_desc(uhost->rx_buffer, uhost->rx_buffer, 0xFF); + uhost->user_handler->user_mfc_string(uhost->rx_buffer); + } + break; + + case ENUM_GET_PRODUCT_STRING: + /* get device product string */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_sting_descriptor(uhost, uhost->dev.dev_desc.iProduct, + uhost->rx_buffer, 0xFF); + } + + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_GET_SERIALNUM_STRING) == USB_OK) + { + usbh_parse_string_desc(uhost->rx_buffer, uhost->rx_buffer, 0xFF); + uhost->user_handler->user_product_string(uhost->rx_buffer); + } + break; + + case ENUM_GET_SERIALNUM_STRING: + /* get device serial string */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_get_sting_descriptor(uhost, uhost->dev.dev_desc.iSerialNumber, + uhost->rx_buffer, 0xFF); + } + + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_SET_CONFIG) == USB_OK) + { + usbh_parse_string_desc(uhost->rx_buffer, uhost->rx_buffer, 0xFF); + uhost->user_handler->user_serial_string(uhost->rx_buffer); + } + break; + + case ENUM_SET_CONFIG: + /* set device config */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_set_configuration(uhost, uhost->dev.cfg_desc.cfg.bConfigurationValue); + } + usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_COMPLETE); + + break; + + case ENUM_COMPLETE: + /* enum complete */ + status = USB_OK; + break; + default: + break; + } + return status; +} + +/** + * @brief active vbus. + * @param uhost: to the structure of usbh_core_type + * @param state: vbus state + * @retval none + */ +void usbh_active_vbus(usbh_core_type *uhost, confirm_state state) +{ + uhost->user_handler->user_active_vbus(uhost, state); +} + +/** + * @brief reset usb port + * @param usbx: to the structure of otg_global_type + * @retval none + */ +void usbh_reset_port(usbh_core_type *uhost) +{ + otg_host_type *usb_host = OTG_HOST(uhost->usb_reg); + uint32_t hprt_val = usb_host->hprt; + + hprt_val &= ~(USB_OTG_HPRT_PRTENA | USB_OTG_HPRT_PRTENCHNG | + USB_OTG_HPRT_PRTOVRCACT | USB_OTG_HPRT_PRTCONDET); + + /* set port reset */ + usb_host->hprt = hprt_val | USB_OTG_HPRT_PRTRST; + + usb_delay_ms(100); + + /* clear port reset */ + usb_host->hprt = hprt_val & (~USB_OTG_HPRT_PRTRST); + + usb_delay_ms(20); + +} + +/** + * @brief usb host attached + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_attached(usbh_core_type *uhost) +{ + /* get free channel */ + uhost->ctrl.hch_in = usbh_alloc_channel(uhost, 0x80); + uhost->ctrl.hch_out = usbh_alloc_channel(uhost, 0x00); + + /* user reset callback handler */ + uhost->user_handler->user_reset(); + + /* get device speed */ + uhost->dev.speed = OTG_HOST(uhost->usb_reg)->hprt_bit.prtspd; + uhost->global_state = USBH_ENUMERATION; + uhost->user_handler->user_speed(uhost->dev.speed); + + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_in,0x80, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + + /* enable channel */ + usbh_hc_open(uhost, uhost->ctrl.hch_out,0x00, + uhost->dev.address, EPT_CONTROL_TYPE, + uhost->ctrl.ept0_size, + uhost->dev.speed); + + usb_flush_tx_fifo(uhost->usb_reg, 0x10); + usb_flush_rx_fifo(uhost->usb_reg); + + /* user attached callback */ + uhost->user_handler->user_attached(); +} + + +/** + * @brief usb host enumeration + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_enumeration(usbh_core_type *uhost) +{ + /* enumeration process */ + if(usbh_enum_handler(uhost) == USB_OK) + { + /* user enumeration done callback */ + uhost->user_handler->user_enumeration_done(); + uhost->global_state = USBH_USER_HANDLER; + } +} + +/** + * @brief usb host class request + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_class_request(usbh_core_type *uhost) +{ + usb_sts_type status; + + /* class request callback */ + status = uhost->class_handler->request_handler((void *)uhost); + if(status == USB_OK) + { + uhost->global_state = USBH_CLASS; + } + else if(status == USB_ERROR || status == USB_FAIL) + { + uhost->global_state = USBH_ERROR_STATE; + } + else if(status == USB_NOT_SUPPORT) + { + uhost->global_state = USBH_ERROR_STATE; + } +} + +/** + * @brief usb host class handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_class(usbh_core_type *uhost) +{ + /* process handler */ + if(uhost->class_handler->process_handler((void *)uhost) == USB_OK) + { + } +} + +/** + * @brief usb host suspend + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_suspend(usbh_core_type *uhost) +{ + /* set device feature */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + usbh_set_feature(uhost, 0x01, 0); + } + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_IDLE) == USB_OK) + { + /* enter suspend mode */ + usb_delay_ms(3); + usbh_enter_suspend(uhost); + uhost->global_state = USBH_SUSPENDED; + + } +} + +/** + * @brief usb host wakeup + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_wakeup(usbh_core_type *uhost) +{ + /* clear device feature */ + if(uhost->ctrl.state == CONTROL_IDLE) + { + /* usb host resume */ + usbh_resume(uhost); + usbh_clear_dev_feature(uhost, 0x01, 0); + } + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_IDLE) == USB_OK) + { + uhost->global_state = USBH_CLASS_REQUEST; + //USBH_DEBUG("Remote Wakeup"); + } +} + +/** + * @brief usb host disconnect + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +static void usbh_disconnect(usbh_core_type *uhost) +{ + uint8_t i_index = 0; + + /* set host to default state */ + usbh_cfg_default_init(uhost); + + /* free host channel */ + for(i_index = 0; i_index < USB_HOST_CHANNEL_NUM; i_index ++) + { + usbh_free_channel(uhost, i_index); + } + + /* call class reset handler */ + if(uhost->class_handler->reset_handler != NULL) + { + uhost->class_handler->reset_handler(uhost); + } + + /* set global state to idle */ + uhost->global_state = USBH_IDLE; + + /*call user disconnect function */ + uhost->user_handler->user_disconnect(); +} + + +/** + * @brief usb host enum loop handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +usb_sts_type usbh_loop_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_FAIL; + + if(uhost->conn_sts == 0 && + uhost->global_state != USBH_IDLE && + uhost->global_state != USBH_DISCONNECT) + { + uhost->global_state = USBH_IDLE; + } + switch(uhost->global_state) + { + case USBH_IDLE: + if(uhost->conn_sts == 1) + { + uhost->global_state = USBH_PORT_EN; + + /* wait stable */ + usb_delay_ms(200); + + /* port reset */ + usbh_reset_port(uhost); + + /* user reset */ + uhost->user_handler->user_reset(); + } + break; + + case USBH_PORT_EN: + if(uhost->port_enable) + { + uhost->global_state = USBH_ATTACHED; + usb_delay_ms(50); + } + break; + + case USBH_ATTACHED: + usbh_attached(uhost); + break; + + case USBH_ENUMERATION: + usbh_enumeration(uhost); + break; + + case USBH_USER_HANDLER: + uhost->global_state = USBH_CLASS_REQUEST; + if( uhost->class_handler->init_handler(uhost) == USB_NOT_SUPPORT) + { + uhost->global_state = USBH_UNSUPPORT; + } + break; + + case USBH_CLASS_REQUEST: + usbh_class_request(uhost); + break; + + case USBH_CLASS: + usbh_class(uhost); + break; + + case USBH_SUSPEND: + usbh_suspend(uhost); + break; + + case USBH_SUSPENDED: + break; + + case USBH_WAKEUP: + usbh_wakeup(uhost); + break; + + case USBH_DISCONNECT: + usbh_disconnect(uhost); + break; + + case USBH_ERROR_STATE: + usbh_cfg_default_init(uhost); + uhost->class_handler->reset_handler(uhost); + uhost->user_handler->user_reset(); + break; + case USBH_UNSUPPORT: + break; + default: + break; + } + + return status; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/src/main/drivers/at32/usb/usbh_core.h b/src/main/drivers/at32/usb/usbh_core.h new file mode 100644 index 0000000000..91d6a3d909 --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_core.h @@ -0,0 +1,373 @@ +/** + ************************************************************************** + * @file usbh_core.h + * @brief usb host core header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBH_CORE_H +#define __USBH_CORE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "usb_conf.h" +#include "usb_std.h" + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @addtogroup USBH_drivers_core + * @{ + */ + +/** @defgroup USBH_core_exported_types + * @{ + */ + + +#ifdef USE_OTG_HOST_MODE + +/** + * @brief usb channel flag + */ +typedef enum +{ + HCH_IDLE, /*!< usb host channel idle */ + HCH_XFRC, /*!< usb host channel transfer completed */ + HCH_HALTED, /*!< usb host channel halted */ + HCH_NAK, /*!< usb host channel nak */ + HCH_NYET, /*!< usb host channel nyet */ + HCH_STALL, /*!< usb host channel stall */ + HCH_XACTERR, /*!< usb host channel transaction error */ + HCH_BBLERR, /*!< usb host channel babble error */ + HCH_DATATGLERR /*!< usb host channel data toggle error */ +} hch_sts_type; + +/** + * @brief usb channel state + */ +typedef enum +{ + URB_IDLE = 0, /*!< usb request idle state */ + URB_DONE, /*!< usb request done state */ + URB_NOTREADY, /*!< usb request not ready state */ + URB_NYET, /*!< usb request nyet stat e*/ + URB_ERROR, /*!< usb request error state */ + URB_STALL /*!< usb request stall state */ +} urb_sts_type; + +/** + * @brief usb control channel flag + */ +typedef enum +{ + CTRL_START = 0, /*!< usb control request start */ + CTRL_XFERC, /*!< usb control request completed */ + CTRL_HALTED, /*!< usb control request halted */ + CTRL_NAK, /*!< usb control request nak */ + CTRL_STALL, /*!< usb control request stall */ + CTRL_XACTERR, /*!< usb control request transaction error */ + CTRL_BBLERR, /*!< usb control request babble error */ + CTRL_DATATGLERR, /*!< usb control request data toggle error */ + CTRL_FAIL /*!< usb control request failed */ +} ctrl_sts_type; + +/** + * @brief usb host control state machine + */ +typedef enum +{ + CONTROL_IDLE, /*!< usb control state idle */ + CONTROL_SETUP, /*!< usb control state setup */ + CONTROL_SETUP_WAIT, /*!< usb control state setup wait */ + CONTROL_DATA_IN, /*!< usb control state data in */ + CONTROL_DATA_IN_WAIT, /*!< usb control state data in wait */ + CONTROL_DATA_OUT, /*!< usb control state data out */ + CONTROL_DATA_OUT_WAIT, /*!< usb control state data out wait */ + CONTROL_STATUS_IN, /*!< usb control state status in */ + CONTROL_STATUS_IN_WAIT, /*!< usb control state status in wait */ + CONTROL_STATUS_OUT, /*!< usb control state out */ + CONTROL_STATUS_OUT_WAIT, /*!< usb control state out wait */ + CONTROL_ERROR, /*!< usb control state error */ + CONTROL_STALL, /*!< usb control state stall */ + CONTROL_COMPLETE /*!< usb control state complete */ +} ctrl_ept0_sts_type; + +/** + * @brief usb host enumration state machine + */ +typedef enum +{ + ENUM_IDLE, /*!< usb host enumration state idle */ + ENUM_GET_MIN_DESC, /*!< usb host enumration state get descriptor 8 byte*/ + ENUM_GET_FULL_DESC, /*!< usb host enumration state get descriptor 18 byte*/ + ENUM_SET_ADDR, /*!< usb host enumration state set address */ + ENUM_GET_CFG, /*!< usb host enumration state get configuration */ + ENUM_GET_FULL_CFG, /*!< usb host enumration state get full configuration */ + ENUM_GET_MFC_STRING, /*!< usb host enumration state get manufacturer string */ + ENUM_GET_PRODUCT_STRING, /*!< usb host enumration state get product string */ + ENUM_GET_SERIALNUM_STRING, /*!< usb host enumration state get serial number string */ + ENUM_SET_CONFIG, /*!< usb host enumration state set config */ + ENUM_COMPLETE, /*!< usb host enumration state complete */ +} usbh_enum_sts_type; + +/** + * @brief usb host global state machine + */ +typedef enum +{ + USBH_IDLE, /*!< usb host global state idle */ + USBH_PORT_EN, /*!< usb host global state port enable */ + USBH_ATTACHED, /*!< usb host global state attached */ + USBH_DISCONNECT, /*!< usb host global state disconnect */ + USBH_DEV_SPEED, /*!< usb host global state device speed */ + USBH_ENUMERATION, /*!< usb host global state enumeration */ + USBH_CLASS_REQUEST, /*!< usb host global state class request */ + USBH_CLASS, /*!< usb host global state class */ + USBH_CTRL_XFER, /*!< usb host global state control transfer */ + USBH_USER_HANDLER, /*!< usb host global state user handler */ + USBH_SUSPEND, /*!< usb host global state suspend */ + USBH_SUSPENDED, /*!< usb host have in suspend mode */ + USBH_WAKEUP, /*!< usb host global state wakeup */ + USBH_UNSUPPORT, /*!< usb host global unsupport device */ + USBH_ERROR_STATE, /*!< usb host global state error */ +} usbh_gstate_type; + +/** + * @brief usb host transfer state + */ +typedef enum +{ + CMD_IDLE, /*!< usb host transfer state idle */ + CMD_SEND, /*!< usb host transfer state send */ + CMD_WAIT /*!< usb host transfer state wait */ +} cmd_sts_type; + +/** + * @brief usb host channel malloc state + */ +#define HCH_OK 0x0000 /*!< usb channel malloc state ok */ +#define HCH_USED 0x8000 /*!< usb channel had used */ +#define HCH_ERROR 0xFFFF /*!< usb channel error */ +#define HCH_USED_MASK 0x7FFF /*!< usb channel use mask */ + +/** + * @brief channel pid + */ +#define HCH_PID_DATA0 0 /*!< usb channel pid data 0 */ +#define HCH_PID_DATA2 1 /*!< usb channel pid data 2 */ +#define HCH_PID_DATA1 2 /*!< usb channel pid data 1 */ +#define HCH_PID_SETUP 3 /*!< usb channel pid setup */ + +/** + * @brief channel data transfer direction + */ +#define USB_REQUEST_DIR_MASK 0x80 /*!< usb request direction mask */ +#define USB_DIR_H2D USB_REQ_DIR_HTD /*!< usb request direction host to device */ +#define USB_DIR_D2H USB_REQ_DIR_DTH /*!< usb request direction device to host */ + +/** + * @brief request timeout + */ +#define DATA_STAGE_TIMEOUT 5000 /*!< usb data stage timeout */ +#define NODATA_STAGE_TIMEOUT 50 /*!< usb no-data stage timeout */ + +/** + * @brief max interface and endpoint + */ +#define USBH_MAX_ERROR_COUNT 2 /*!< usb support maximum error */ +#define USBH_MAX_INTERFACE 5 /*!< usb support maximum interface */ +#define USBH_MAX_ENDPOINT 5 /*!< usb support maximum endpoint */ + +/** + * @brief interface descriptor + */ +typedef struct +{ + usb_interface_desc_type interface; /*!< usb device interface descriptor structure */ + usb_endpoint_desc_type endpoint[USBH_MAX_ENDPOINT]; /*!< usb device endpoint descriptor structure array */ +} usb_itf_desc_type; + +/** + * @brief configure descriptor + */ +typedef struct +{ + usb_configuration_desc_type cfg; /*!< usb device configuration descriptor structure */ + usb_itf_desc_type interface[USBH_MAX_INTERFACE]; /*!< usb device interface descriptor structure array*/ +} usb_cfg_desc_type; + +/** + * @brief device descriptor + */ +typedef struct +{ + uint8_t address; /*!< usb device address */ + uint8_t speed; /*!< usb device speed */ + usb_device_desc_type dev_desc; /*!< usb device descriptor */ + usb_cfg_desc_type cfg_desc; /*!< usb device configuration */ +} usbh_dev_desc_type; + +/** + * @brief usb host control struct type + */ +typedef struct +{ + uint8_t hch_in; /*!< in channel number */ + uint8_t hch_out; /*!< out channel number */ + uint8_t ept0_size; /*!< endpoint 0 size */ + uint8_t *buffer; /*!< endpoint 0 transfer buffer */ + usb_setup_type setup; /*!< control setup type */ + uint16_t len; /*!< transfer length */ + uint8_t err_cnt; /*!< error counter */ + uint32_t timer; /*!< transfer timer */ + ctrl_sts_type sts; /*!< control transfer status */ + ctrl_ept0_sts_type state; /*!< endpoint 0 state */ +} usbh_ctrl_type; + +/** + * @brief host class handler type + */ +typedef struct +{ + usb_sts_type (*init_handler)(void *uhost); /*!< usb host class init handler */ + usb_sts_type (*reset_handler)(void *uhost); /*!< usb host class reset handler */ + usb_sts_type (*request_handler)(void *uhost); /*!< usb host class request handler */ + usb_sts_type (*process_handler)(void *uhost); /*!< usb host class process handler */ + void *pdata; /*!< usb host class data */ +} usbh_class_handler_type; + +/** + * @brief host user handler type + */ +typedef struct +{ + usb_sts_type (*user_init)(void); /*!< usb host user init handler */ + usb_sts_type (*user_reset)(void); /*!< usb host user reset handler */ + usb_sts_type (*user_attached)(void); /*!< usb host user attached handler */ + usb_sts_type (*user_disconnect)(void); /*!< usb host user disconnect handler */ + usb_sts_type (*user_speed)(uint8_t speed); /*!< usb host user speed handler */ + usb_sts_type (*user_mfc_string)(void *); /*!< usb host user manufacturer string handler */ + usb_sts_type (*user_product_string)(void *); /*!< usb host user product string handler */ + usb_sts_type (*user_serial_string)(void *); /*!< usb host user serial handler */ + usb_sts_type (*user_enumeration_done)(void); /*!< usb host user enumeration done handler */ + usb_sts_type (*user_application)(void); /*!< usb host user application handler */ + usb_sts_type (*user_active_vbus)(void *uhost, confirm_state state); /*!< usb host user active vbus */ + usb_sts_type (*user_not_support)(void); /*!< usb host user not support handler */ +} usbh_user_handler_type; + +/** + * @brief host host core handler type + */ +typedef struct +{ + usb_reg_type *usb_reg; /*!< usb register pointer */ + + uint8_t global_state; /*!< usb host global state machine */ + uint8_t enum_state; /*!< usb host enumeration state machine */ + uint8_t req_state; /*!< usb host request state machine */ + + usbh_dev_desc_type dev; /*!< usb device descriptor */ + usbh_ctrl_type ctrl; /*!< usb host control transfer struct */ + + usbh_class_handler_type *class_handler; /*!< usb host class handler pointer */ + usbh_user_handler_type *user_handler; /*!< usb host user handler pointer */ + + usb_hch_type hch[USB_HOST_CHANNEL_NUM]; /*!< usb host channel array */ + uint8_t rx_buffer[USB_MAX_DATA_LENGTH]; /*!< usb host rx buffer */ + + uint32_t conn_sts; /*!< connect status */ + uint32_t port_enable; /*!< port enable status */ + uint32_t timer; /*!< sof timer */ + + uint32_t err_cnt[USB_HOST_CHANNEL_NUM]; /*!< error counter */ + uint32_t xfer_cnt[USB_HOST_CHANNEL_NUM]; /*!< xfer counter */ + hch_sts_type hch_state[USB_HOST_CHANNEL_NUM];/*!< channel state */ + urb_sts_type urb_state[USB_HOST_CHANNEL_NUM];/*!< usb request state */ + uint16_t channel[USB_HOST_CHANNEL_NUM]; /*!< channel array */ +} usbh_core_type; + + +void usbh_free_channel(usbh_core_type *uhost, uint8_t index); +uint16_t usbh_get_free_channel(usbh_core_type *uhost); +usb_sts_type usbh_set_toggle(usbh_core_type *uhost, uint8_t hc_num, uint8_t toggle); +usb_sts_type usbh_in_out_request(usbh_core_type *uhost, uint8_t hc_num); +usb_sts_type usbh_interrupt_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_interrupt_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_bulk_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_bulk_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_isoc_send(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_isoc_recv(usbh_core_type *uhost, uint8_t hc_num, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_cfg_default_init(usbh_core_type *uhost); +void usbh_enter_suspend(usbh_core_type *uhost); +void usbh_resume(usbh_core_type *uhost); + +uint16_t usbh_alloc_channel(usbh_core_type *uhost, uint8_t ept_addr); +urb_sts_type usbh_get_urb_status(usbh_core_type *uhost, uint8_t ch_num); +usb_sts_type usbh_ctrl_result_check(usbh_core_type *uhost, + ctrl_ept0_sts_type next_ctrl_state, + uint8_t next_enum_state); +uint8_t usbh_alloc_address(void); +void usbh_reset_port(usbh_core_type *uhost); +usb_sts_type usbh_loop_handler(usbh_core_type *uhost); +void usbh_ch_disable(usbh_core_type *uhost, uint8_t chn); +void usbh_hc_open(usbh_core_type *uhost, + uint8_t chn, + uint8_t ept_num, + uint8_t dev_address, + uint8_t type, + uint16_t maxpacket, + uint8_t speed); +void usbh_active_vbus(usbh_core_type *uhost, confirm_state state); + +usb_sts_type usbh_core_init(usbh_core_type *uhost, + usb_reg_type *usb_reg, + usbh_class_handler_type *class_handler, + usbh_user_handler_type *user_handler, + uint8_t core_id); +#endif + + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usbh_ctrl.c b/src/main/drivers/at32/usb/usbh_ctrl.c new file mode 100644 index 0000000000..620827df01 --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_ctrl.c @@ -0,0 +1,988 @@ +/** + ************************************************************************** + * @file usbh_ctrl.c + * @brief usb host control request + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usbh_ctrl.h" +#include "usbh_core.h" +#include "usb_std.h" + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @defgroup USBH_drivers_control + * @brief usb host drivers control + * @{ + */ + +/** @defgroup USBH_ctrl_private_functions + * @{ + */ + +/* control timeout 5s */ +#define CTRL_TIMEOUT 5000 + +/** + * @brief usb host control send setup packet + * @param uhost: to the structure of usbh_core_type + * @param buffer: usb control setup send buffer + * @param hc_num: channel number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_send_setup(usbh_core_type *uhost, uint8_t *buffer, uint8_t hc_num) +{ + uhost->hch[hc_num].dir = 0; + uhost->hch[hc_num].data_pid = HCH_PID_SETUP; + uhost->hch[hc_num].trans_buf = buffer; + uhost->hch[hc_num].trans_len = 8; /*setup */ + + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host control receive data from device + * @param uhost: to the structure of usbh_core_type + * @param buffer: usb control receive data buffer + * @param length: usb control receive data length + * @param hc_num: channel number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_recv_data(usbh_core_type *uhost, uint8_t *buffer, + uint16_t length, uint16_t hc_num) +{ + uhost->hch[hc_num].dir = 1; + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + uhost->hch[hc_num].trans_buf = buffer; + uhost->hch[hc_num].trans_len = length; + + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host control send data packet + * @param uhost: to the structure of usbh_core_type + * @param buffer: usb control send data buffer + * @param length: usb control send data length + * @param hc_num: channel number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_send_data(usbh_core_type *uhost, uint8_t *buffer, + uint16_t length, uint16_t hc_num) +{ + uhost->hch[hc_num].dir = 0; + uhost->hch[hc_num].trans_buf = buffer; + uhost->hch[hc_num].trans_len = length; + + if(length == 0) + { + uhost->hch[uhost->ctrl.hch_out].toggle_out = 1; + } + if(uhost->hch[uhost->ctrl.hch_out].toggle_out == 0) + { + uhost->hch[hc_num].data_pid = HCH_PID_DATA0; + } + else + { + uhost->hch[hc_num].data_pid = HCH_PID_DATA1; + } + return usbh_in_out_request(uhost, hc_num); +} + +/** + * @brief usb host control setup request handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_setup_handler(usbh_core_type *uhost) +{ + usbh_ctrl_send_setup(uhost, (uint8_t *)(&uhost->ctrl.setup), + uhost->ctrl.hch_out); + uhost->ctrl.state = CONTROL_SETUP_WAIT; + return USB_OK; +} + +/** + * @brief usb host control setup request wait handler + * @param uhost: to the structure of usbh_core_type + * @param timeout: pointer of wait timeout + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_setup_wait_handler(usbh_core_type *uhost, uint32_t *timeout) +{ + urb_sts_type urb_state; + usb_sts_type status = USB_WAIT; + uint8_t dir; + urb_state = uhost->urb_state[uhost->ctrl.hch_out]; + if(urb_state == URB_DONE) + { + dir = uhost->ctrl.setup.bmRequestType & USB_REQUEST_DIR_MASK; + if(uhost->ctrl.setup.wLength != 0) + { + *timeout = DATA_STAGE_TIMEOUT; + if(dir == USB_DIR_D2H) //in + { + uhost->ctrl.state = CONTROL_DATA_IN; + } + else //out + { + uhost->ctrl.state = CONTROL_DATA_OUT; + } + } + else + { + *timeout = NODATA_STAGE_TIMEOUT; + if(dir == USB_DIR_D2H) //no data, send status + { + uhost->ctrl.state = CONTROL_STATUS_OUT; + } + else //out + { + uhost->ctrl.state = CONTROL_STATUS_IN; + } + } + status = USB_OK; + } + else if(urb_state == URB_ERROR || urb_state == URB_NOTREADY) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + else + { + /* wait nak timeout 5s*/ + if(uhost->timer - uhost->ctrl.timer > CTRL_TIMEOUT) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + } + return status; +} + +/** + * @brief usb host control data in request handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_data_in_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_OK; + usbh_ctrl_recv_data(uhost, uhost->ctrl.buffer, + uhost->ctrl.len, + uhost->ctrl.hch_in); + uhost->ctrl.state = CONTROL_DATA_IN_WAIT; + + return status; +} + +/** + * @brief usb host control data in wait handler + * @param uhost: to the structure of usbh_core_type + * @param timeout: wait timeout + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_data_in_wait_handler(usbh_core_type *uhost, uint32_t timeout) +{ + UNUSED(timeout); + usb_sts_type status = USB_OK; + urb_sts_type urb_state; + urb_state = uhost->urb_state[uhost->ctrl.hch_in]; + + if(urb_state == URB_DONE) + { + uhost->ctrl.state = CONTROL_STATUS_OUT; + } + else if(urb_state == URB_STALL) + { + uhost->ctrl.state = CONTROL_STALL; + } + else if(urb_state == URB_ERROR) + { + uhost->ctrl.state = CONTROL_ERROR; + } + else + { + /* wait nak timeout 5s*/ + if(uhost->timer - uhost->ctrl.timer > CTRL_TIMEOUT) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + + } + return status; +} + +/** + * @brief usb host control data out request handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_data_out_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_OK; + uhost->hch[uhost->ctrl.hch_out].toggle_out = 1; + + usbh_ctrl_send_data(uhost, uhost->ctrl.buffer, + uhost->ctrl.len, + uhost->ctrl.hch_out); + uhost->ctrl.state = CONTROL_DATA_OUT_WAIT; + + return status; +} + +/** + * @brief usb host control data out wait handler + * @param uhost: to the structure of usbh_core_type + * @param timeout: wait timeout + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_data_out_wait_handler(usbh_core_type *uhost, uint32_t timeout) +{ + UNUSED(timeout); + usb_sts_type status = USB_OK; + urb_sts_type urb_state; + urb_state = uhost->urb_state[uhost->ctrl.hch_out]; + if(urb_state == URB_DONE) + { + uhost->ctrl.state = CONTROL_STATUS_IN; + } + else if(urb_state == URB_STALL) + { + uhost->ctrl.state = CONTROL_STALL; + } + else if(urb_state == URB_ERROR) + { + uhost->ctrl.state = CONTROL_ERROR; + } + else if(urb_state == URB_NOTREADY) + { + uhost->ctrl.state = CONTROL_DATA_OUT; + } + else + { + /* wait nak timeout 5s*/ + if(uhost->timer - uhost->ctrl.timer > CTRL_TIMEOUT) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + } + return status; +} + +/** + * @brief usb host control status data in handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_status_in_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_OK; + usbh_ctrl_recv_data(uhost, 0, 0, + uhost->ctrl.hch_in); + uhost->ctrl.state = CONTROL_STATUS_IN_WAIT; + + + return status; +} + +/** + * @brief usb host control status data in wait handler + * @param uhost: to the structure of usbh_core_type + * @param timeout: wait timeout + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_status_in_wait_handler(usbh_core_type *uhost, uint32_t timeout) +{ + UNUSED(timeout); + usb_sts_type status = USB_OK; + urb_sts_type urb_state; + urb_state = uhost->urb_state[uhost->ctrl.hch_in]; + if(urb_state == URB_DONE) + { + uhost->ctrl.state = CONTROL_COMPLETE; + } + else if(urb_state == URB_STALL) + { + uhost->ctrl.state = CONTROL_STALL; + status = USB_NOT_SUPPORT; + } + else if(urb_state == URB_ERROR) + { + uhost->ctrl.state = CONTROL_ERROR; + } + else + { + /* wait nak timeout 5s*/ + if(uhost->timer - uhost->ctrl.timer > CTRL_TIMEOUT) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + } + return status; +} + +/** + * @brief usb host control status data out wait handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_status_out_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_OK; + uhost->hch[uhost->ctrl.hch_out].toggle_out ^= 1; + + usbh_ctrl_send_data(uhost, 0, 0, uhost->ctrl.hch_out); + uhost->ctrl.state = CONTROL_STATUS_OUT_WAIT; + + return status; +} + +/** + * @brief usb host control status data out wait handler + * @param uhost: to the structure of usbh_core_type + * @param timeout: wait timeout + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_status_out_wait_handler(usbh_core_type *uhost, uint32_t timeout) +{ + UNUSED(timeout); + + usb_sts_type status = USB_OK; + urb_sts_type urb_state; + urb_state = uhost->urb_state[uhost->ctrl.hch_out]; + if(urb_state == URB_DONE) + { + uhost->ctrl.state = CONTROL_COMPLETE; + } + else if(urb_state == URB_STALL) + { + uhost->ctrl.state = CONTROL_STALL; + } + else if(urb_state == URB_ERROR) + { + uhost->ctrl.state = CONTROL_ERROR; + } + else if(urb_state == URB_NOTREADY) + { + uhost->ctrl.state = CONTROL_STATUS_OUT; + } + else + { + /* wait nak timeout 5s*/ + if(uhost->timer - uhost->ctrl.timer > CTRL_TIMEOUT) + { + uhost->ctrl.state = CONTROL_ERROR; + uhost->ctrl.sts = CTRL_XACTERR; + status = USB_ERROR; + } + } + return status; +} + +/** + * @brief usb host control error handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_error_handler(usbh_core_type *uhost) +{ + usb_sts_type status = USB_WAIT; + if(++ uhost->ctrl.err_cnt <= USBH_MAX_ERROR_COUNT) + { + uhost->ctrl.state = CONTROL_SETUP; + } + else + { + uhost->ctrl.sts = CTRL_FAIL; + uhost->global_state = USBH_ERROR_STATE; + uhost->ctrl.err_cnt = 0; + //USBH_DEBUG("control error: **** Device No Response ****"); + status = USB_ERROR; + } + return status; +} + +/** + * @brief usb host control stall handler + * @param uhost: to the structure of usbh_core_type + * @retval usb_sts_type + */ +usb_sts_type usbh_ctrl_stall_handler(usbh_core_type *uhost) +{ + UNUSED(uhost); + return USB_NOT_SUPPORT; +} + +/** + * @brief usb host control complete handler + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_complete_handler(usbh_core_type *uhost) +{ + UNUSED(uhost); + return USB_OK; +} + +/** + * @brief usb host control transfer loop function + * @param uhost: to the structure of usbh_core_type + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_transfer_loop(usbh_core_type *uhost) +{ + usb_sts_type status = USB_WAIT; + static uint32_t timeout = 0; + uhost->ctrl.sts = CTRL_START; + + switch(uhost->ctrl.state) + { + case CONTROL_SETUP: + usbh_ctrl_setup_handler(uhost); + uhost->ctrl.timer = uhost->timer; + break; + + case CONTROL_SETUP_WAIT: + usbh_ctrl_setup_wait_handler(uhost, &timeout); + break; + + case CONTROL_DATA_IN: + usbh_ctrl_data_in_handler(uhost); + uhost->ctrl.timer = uhost->timer; + break; + + case CONTROL_DATA_IN_WAIT: + usbh_ctrl_data_in_wait_handler(uhost, timeout); + break; + + case CONTROL_DATA_OUT: + usbh_ctrl_data_out_handler(uhost); + uhost->ctrl.timer = uhost->timer; + break; + + case CONTROL_DATA_OUT_WAIT: + usbh_ctrl_data_out_wait_handler(uhost, timeout); + break; + + case CONTROL_STATUS_IN: + usbh_ctrl_status_in_handler(uhost); + uhost->ctrl.timer = uhost->timer; + break; + + case CONTROL_STATUS_IN_WAIT: + usbh_ctrl_status_in_wait_handler(uhost, timeout); + break; + + case CONTROL_STATUS_OUT: + usbh_ctrl_status_out_handler(uhost); + uhost->ctrl.timer = uhost->timer; + break; + + case CONTROL_STATUS_OUT_WAIT: + usbh_ctrl_status_out_wait_handler(uhost, timeout); + break; + case CONTROL_STALL: + status = usbh_ctrl_stall_handler(uhost); + break; + case CONTROL_ERROR: + status = usbh_ctrl_error_handler(uhost); + break; + case CONTROL_COMPLETE: + status = usbh_ctrl_complete_handler(uhost); + break; + + default: + break; + } + + return status; +} + +/** + * @brief usb host control request + * @param uhost: to the structure of usbh_core_type + * @param buffer: usb request buffer + * @param length: usb request length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_ctrl_request(usbh_core_type *uhost, uint8_t *buffer, uint16_t length) +{ + usb_sts_type status = USB_OK; + if(uhost->req_state == CMD_SEND) + { + uhost->req_state = CMD_WAIT; + uhost->ctrl.buffer = buffer; + uhost->ctrl.len = length; + uhost->ctrl.state = CONTROL_SETUP; + } + return status; +} + +/** + * @brief usb host get device descriptor + * @param uhost: to the structure of usbh_core_type + * @param length: get descriptor request length + * @param req_type: usb request type + * @param wvalue: usb wvalue + * @param buffer: request buffer + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_get_descriptor(usbh_core_type *uhost, uint16_t length, + uint8_t req_type, uint16_t wvalue, + uint8_t *buffer) +{ + usb_sts_type status; + uhost->ctrl.setup.bmRequestType = USB_DIR_D2H | req_type; + uhost->ctrl.setup.bRequest = USB_STD_REQ_GET_DESCRIPTOR; + uhost->ctrl.setup.wValue = wvalue; + uhost->ctrl.setup.wLength = length; + + if((wvalue & 0xFF00) == ((USB_DESCIPTOR_TYPE_STRING << 8) & 0xFF00)) + { + uhost->ctrl.setup.wIndex = 0x0409; + } + else + { + uhost->ctrl.setup.wIndex = 0; + } + + status = usbh_ctrl_request(uhost, buffer, length); + return status; +} + +/** + * @brief usb host parse device descriptor + * @param uhost: to the structure of usbh_core_type + * @param buffer: usb device descriptor buffer + * @param length: usb device descriptor length + * @retval status: usb_sts_type status + */ +void usbh_parse_dev_desc(usbh_core_type *uhost, uint8_t *buffer, uint16_t length) +{ + usbh_dev_desc_type *desc = &(uhost->dev); + + desc->dev_desc.bLength = *(uint8_t *)(buffer + 0); + desc->dev_desc.bDescriptorType = *(uint8_t *)(buffer + 1); + desc->dev_desc.bcdUSB = SWAPBYTE(buffer + 2); + desc->dev_desc.bDeviceClass = *(uint8_t *)(buffer + 4); + desc->dev_desc.bDeviceSubClass = *(uint8_t *)(buffer + 5); + desc->dev_desc.bDeviceProtocol = *(uint8_t *)(buffer + 6); + desc->dev_desc.bMaxPacketSize0 = *(uint8_t *)(buffer + 7); + + if(length > 8) + { + desc->dev_desc.idVendor = SWAPBYTE(buffer + 8); + desc->dev_desc.idProduct = SWAPBYTE(buffer + 10); + desc->dev_desc.bcdDevice = SWAPBYTE(buffer + 12); + desc->dev_desc.iManufacturer = *(uint8_t *)(buffer + 14); + desc->dev_desc.iProduct = *(uint8_t *)(buffer + 15); + desc->dev_desc.iSerialNumber = *(uint8_t *)(buffer + 16); + desc->dev_desc.bNumConfigurations = *(uint8_t *)(buffer + 17); + } +} + +/** + * @brief usb host get next header + * @param buffer: usb data buffer + * @param index_len: pointer of index len + * @retval status: usb_sts_type status + */ +usb_header_desc_type *usbh_get_next_header(uint8_t *buf, uint16_t *index_len) +{ + *index_len += ((usb_header_desc_type *)buf)->bLength; + return (usb_header_desc_type *) + ((uint8_t *)buf + ((usb_header_desc_type *)buf)->bLength); +} + +/** + * @brief usb host parse interface descriptor + * @param intf: usb interface description type + * @param buf: interface description data buffer + * @retval none + */ +void usbh_parse_interface_desc(usb_interface_desc_type *intf, uint8_t *buf) +{ + intf->bLength = *(uint8_t *)buf; + intf->bDescriptorType = *(uint8_t *)(buf + 1); + intf->bInterfaceNumber = *(uint8_t *)(buf + 2); + intf->bAlternateSetting = *(uint8_t *)(buf + 3); + intf->bNumEndpoints = *(uint8_t *)(buf + 4); + intf->bInterfaceClass = *(uint8_t *)(buf + 5); + intf->bInterfaceSubClass = *(uint8_t *)(buf + 6); + intf->bInterfaceProtocol = *(uint8_t *)(buf + 7); + intf->iInterface = *(uint8_t *)(buf + 8); +} + +/** + * @brief usb host parse endpoint descriptor + * @param ept_desc: endpoint type + * @param buf: endpoint description data buffer + * @retval none + */ +void usbh_parse_endpoint_desc(usb_endpoint_desc_type *ept_desc, uint8_t *buf) +{ + ept_desc->bLength = *(uint8_t *)(buf + 0); + ept_desc->bDescriptorType = *(uint8_t *)(buf + 1); + ept_desc->bEndpointAddress = *(uint8_t *)(buf + 2); + ept_desc->bmAttributes = *(uint8_t *)(buf + 3); + ept_desc->wMaxPacketSize = SWAPBYTE(buf + 4); + ept_desc->bInterval = *(uint8_t *)(buf + 6); +} + +/** + * @brief usb host parse configure descriptor + * @param uhost: to the structure of usbh_core_type + * @param buffer: configure buffer + * @param length: configure length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_parse_configure_desc(usbh_core_type *uhost, + uint8_t *buffer, uint16_t length) +{ + usb_cfg_desc_type *cfg_desc = &(uhost->dev.cfg_desc); + usb_interface_desc_type *intf_desc; + usb_endpoint_desc_type *ept_desc; + usb_header_desc_type *desc; + uint16_t index_len; + uint8_t index_intf = 0; + uint8_t index_ept = 0; + + desc = (usb_header_desc_type *)buffer; + cfg_desc->cfg.bLength = *(uint8_t *)buffer; + cfg_desc->cfg.bDescriptorType = *(uint8_t *)(buffer + 1); + cfg_desc->cfg.wTotalLength = SWAPBYTE(buffer + 2); + cfg_desc->cfg.bNumInterfaces = *(uint8_t *)(buffer + 4); + cfg_desc->cfg.bConfigurationValue = *(uint8_t *)(buffer + 5); + cfg_desc->cfg.iConfiguration = *(uint8_t *)(buffer + 6); + cfg_desc->cfg.bmAttributes = *(uint8_t *)(buffer + 7); + cfg_desc->cfg.bMaxPower = *(uint8_t *)(buffer + 8); + + if(length > USB_DEVICE_CFG_DESC_LEN) + { + index_len = USB_DEVICE_CFG_DESC_LEN; + + while((index_intf < USBH_MAX_INTERFACE) && index_len < cfg_desc->cfg.wTotalLength) + { + desc = usbh_get_next_header((uint8_t *)desc, &index_len); + if(desc->bDescriptorType == USB_DESCIPTOR_TYPE_INTERFACE) + { + index_ept = 0; + intf_desc = &cfg_desc->interface[index_intf].interface; + usbh_parse_interface_desc(intf_desc, (uint8_t *)desc); + + while(index_ept < intf_desc->bNumEndpoints && index_len < cfg_desc->cfg.wTotalLength) + { + desc = usbh_get_next_header((uint8_t *)desc, &index_len); + if(desc->bDescriptorType == USB_DESCIPTOR_TYPE_ENDPOINT) + { + ept_desc = &(cfg_desc->interface[index_intf].endpoint[index_ept]); + usbh_parse_endpoint_desc(ept_desc, (uint8_t *)desc); + index_ept ++; + } + } + index_intf ++; + } + } + } + return USB_OK; +} + +/** + * @brief usb host find interface + * @param uhost: to the structure of usbh_core_type + * @param class_code: class code + * @param sub_class: subclass code + * @param protocol: prtocol code + * @retval idx: interface index + */ +uint8_t usbh_find_interface(usbh_core_type *uhost, uint8_t class_code, uint8_t sub_class, uint8_t protocol) +{ + uint8_t idx = 0; + usb_itf_desc_type *usbitf; + for(idx = 0; idx < uhost->dev.cfg_desc.cfg.bNumInterfaces; idx ++) + { + usbitf = &uhost->dev.cfg_desc.interface[idx]; + if(((usbitf->interface.bInterfaceClass == class_code) || (class_code == 0xFF)) && + ((usbitf->interface.bInterfaceSubClass == sub_class) || (sub_class == 0xFF)) && + ((usbitf->interface.bInterfaceProtocol == protocol) || (protocol == 0xFF)) + ) + { + return idx; + } + } + return 0xFF; +} + +/** + * @brief usbh parse string descriptor + * @param src: string source pointer + * @param dest: string destination pointer + * @param length: string length + * @retval none + */ +void usbh_parse_string_desc(uint8_t *src, uint8_t *dest, uint16_t length) +{ + uint16_t len; + uint16_t i_index; + + if(src[1] == USB_DESCIPTOR_TYPE_STRING) + { + len = ((src[0] - 2) <= length ? (src[0] - 2) : length); + src += 2; + for(i_index = 0; i_index < len; i_index += 2) + { + *dest = src[i_index]; + dest ++; + } + *dest = 0; + } +} + +/** + * @brief usb host get device descriptor + * @param uhost: to the structure of usbh_core_type + * @param length: get device descriptor length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_get_device_descriptor(usbh_core_type *uhost, uint16_t length) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + uint16_t wvalue; + + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + wvalue = (USB_DESCIPTOR_TYPE_DEVICE << 8) & 0xFF00; + + status = usbh_get_descriptor(uhost, length, bm_req, + wvalue, uhost->rx_buffer); + return status; +} + +/** + * @brief usb host get configure descriptor + * @param uhost: to the structure of usbh_core_type + * @param length: get device configure length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_get_configure_descriptor(usbh_core_type *uhost, uint16_t length) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + uint16_t wvalue; + + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + wvalue = (USB_DESCIPTOR_TYPE_CONFIGURATION << 8) & 0xFF00; + + status = usbh_get_descriptor(uhost, length, bm_req, + wvalue, uhost->rx_buffer); + + return status; +} + +/** + * @brief usb host get string descriptor + * @param uhost: to the structure of usbh_core_type + * @param string_id: string id + * @param buffer: receive data buffer + * @param length: get device string length + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_get_sting_descriptor(usbh_core_type *uhost, uint8_t string_id, + uint8_t *buffer, uint16_t length) +{ + UNUSED(buffer); + + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + uint16_t wvalue; + + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + wvalue = (USB_DESCIPTOR_TYPE_STRING << 8) | string_id; + + status = usbh_get_descriptor(uhost, length, bm_req, + wvalue, uhost->rx_buffer); + + return status; +} + +/** + * @brief usb host set configurtion + * @param uhost: to the structure of usbh_core_type + * @param config: usb configuration + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_set_configuration(usbh_core_type *uhost, uint16_t config) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_SET_CONFIGURATION; + uhost->ctrl.setup.wValue = config; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = 0; + status = usbh_ctrl_request(uhost, 0, 0); + return status; +} + +/** + * @brief usb host set device address + * @param uhost: to the structure of usbh_core_type + * @param address: device address + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_set_address(usbh_core_type *uhost, uint8_t address) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_SET_ADDRESS; + uhost->ctrl.setup.wValue = (uint16_t)address; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = 0; + status = usbh_ctrl_request(uhost, 0, 0); + return status; +} + +/** + * @brief usb host set interface + * @param uhost: to the structure of usbh_core_type + * @param ept_num: endpoint number + * @param altsetting: alter setting + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_set_interface(usbh_core_type *uhost, uint8_t ept_num, uint8_t altsetting) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + bm_req = USB_REQ_RECIPIENT_INTERFACE | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_SET_INTERFACE; + uhost->ctrl.setup.wValue = (uint16_t)altsetting; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = ept_num; + status = usbh_ctrl_request(uhost, 0, 0); + return status; +} + +/** + * @brief usb host set feature + * @param uhost: to the structure of usbh_core_type + * @param feature: feature number + * @param index: index number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_set_feature(usbh_core_type *uhost, uint8_t feature, uint16_t index) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_SET_FEATURE; + uhost->ctrl.setup.wValue = (uint16_t)feature; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = index; + status = usbh_ctrl_request(uhost, 0, 0); + return status; +} + + +/** + * @brief usb host clear device feature + * @param uhost: to the structure of usbh_core_type + * @param feature: feature number + * @param index: index number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_clear_dev_feature(usbh_core_type *uhost, uint8_t feature, uint16_t index) +{ + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + bm_req = USB_REQ_RECIPIENT_DEVICE | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_CLEAR_FEATURE; + uhost->ctrl.setup.wValue = (uint16_t)feature; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = index; + status = usbh_ctrl_request(uhost, 0, 0); + return status; +} + +/** + * @brief usb host clear endpoint feature + * @param uhost: to the structure of usbh_core_type + * @param ept_num: endpoint number + * @param hc_num: host channel number + * @retval status: usb_sts_type status + */ +usb_sts_type usbh_clear_ept_feature(usbh_core_type *uhost, uint8_t ept_num, uint8_t hc_num) +{ + UNUSED(hc_num); + + usb_sts_type status = USB_WAIT; + uint8_t bm_req; + if(uhost->ctrl.state == CONTROL_IDLE ) + { + bm_req = USB_REQ_RECIPIENT_ENDPOINT | USB_REQ_TYPE_STANDARD; + + uhost->ctrl.setup.bmRequestType = USB_DIR_H2D | bm_req; + uhost->ctrl.setup.bRequest = USB_STD_REQ_CLEAR_FEATURE; + uhost->ctrl.setup.wValue = USB_FEATURE_EPT_HALT; + uhost->ctrl.setup.wLength = 0; + uhost->ctrl.setup.wIndex = ept_num; + usbh_ctrl_request(uhost, 0, 0); + } + if(usbh_ctrl_result_check(uhost, CONTROL_IDLE, ENUM_IDLE) == USB_OK) + { + status = USB_OK; + } + return status; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/src/main/drivers/at32/usb/usbh_ctrl.h b/src/main/drivers/at32/usb/usbh_ctrl.h new file mode 100644 index 0000000000..4d6312855c --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_ctrl.h @@ -0,0 +1,107 @@ +/** + ************************************************************************** + * @file usbh_ctrl.h + * @brief usb header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBH_CTRL_H +#define __USBH_CTRL_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/* includes ------------------------------------------------------------------*/ +#include "usb_conf.h" +#include "usbh_core.h" + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @addtogroup USBH_drivers_control + * @{ + */ + +/** @defgroup USBH_ctrl_exported_types + * @{ + */ + +usb_sts_type usbh_ctrl_send_setup(usbh_core_type *uhost, uint8_t *buffer, uint8_t hc_num); +usb_sts_type usbh_ctrl_recv_data(usbh_core_type *uhost, uint8_t *buffer, + uint16_t length, uint16_t hc_num); +usb_sts_type usbh_ctrl_send_data(usbh_core_type *uhost, uint8_t *buffer, + uint16_t length, uint16_t hc_num); +usb_sts_type usbh_ctrl_setup_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_setup_wait_handler(usbh_core_type *uhost, uint32_t *timeout); +usb_sts_type usbh_ctrl_data_in_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_data_in_wait_handler(usbh_core_type *uhost, uint32_t timeout); +usb_sts_type usbh_ctrl_data_out_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_data_out_wait_handler(usbh_core_type *uhost, uint32_t timeout); +usb_sts_type usbh_ctrl_status_in_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_status_in_wait_handler(usbh_core_type *uhost, uint32_t timeout); +usb_sts_type usbh_ctrl_status_out_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_status_out_wait_handler(usbh_core_type *uhost, uint32_t timeout); +usb_sts_type usbh_ctrl_error_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_stall_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_complete_handler(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_transfer_loop(usbh_core_type *uhost); +usb_sts_type usbh_ctrl_request(usbh_core_type *uhost, uint8_t *buffer, uint16_t length); +usb_sts_type usbh_get_descriptor(usbh_core_type *uhost, uint16_t length, + uint8_t req_type, uint16_t wvalue, + uint8_t *buffer); +void usbh_parse_dev_desc(usbh_core_type *uhost, uint8_t *buffer, uint16_t length); +usb_header_desc_type *usbh_get_next_header(uint8_t *buf, uint16_t *index_len); +void usbh_parse_interface_desc(usb_interface_desc_type *intf, uint8_t *buf); +void usbh_parse_endpoint_desc(usb_endpoint_desc_type *ept_desc, uint8_t *buf); +usb_sts_type usbh_parse_configure_desc(usbh_core_type *uhost, + uint8_t *buffer, uint16_t length); +uint8_t usbh_find_interface(usbh_core_type *uhost, uint8_t class_code, uint8_t sub_class, uint8_t protocol); +void usbh_parse_string_desc(uint8_t *src, uint8_t *dest, uint16_t length); +usb_sts_type usbh_get_device_descriptor(usbh_core_type *uhost, uint16_t length); +usb_sts_type usbh_get_configure_descriptor(usbh_core_type *uhost, uint16_t length); +usb_sts_type usbh_get_sting_descriptor(usbh_core_type *uhost, uint8_t string_id, + uint8_t *buffer, uint16_t length); +usb_sts_type usbh_set_configuration(usbh_core_type *uhost, uint16_t config); +usb_sts_type usbh_set_address(usbh_core_type *uhost, uint8_t address); +usb_sts_type usbh_set_interface(usbh_core_type *uhost, uint8_t ept_num, uint8_t altsetting); +usb_sts_type usbh_set_feature(usbh_core_type *uhost, uint8_t feature, uint16_t index); +usb_sts_type usbh_clear_dev_feature(usbh_core_type *uhost, uint8_t feature, uint16_t index); +usb_sts_type usbh_clear_ept_feature(usbh_core_type *uhost, uint8_t ept_num, uint8_t hc_num); + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/main/drivers/at32/usb/usbh_int.c b/src/main/drivers/at32/usb/usbh_int.c new file mode 100644 index 0000000000..b63e5bbad4 --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_int.c @@ -0,0 +1,530 @@ +/** + ************************************************************************** + * @file usbh_int.c + * @brief usb host interrupt request + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +#include "platform.h" + +#include "usbh_int.h" + + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @defgroup USBH_drivers_interrupt + * @brief usb host interrupt + * @{ + */ + +/** @defgroup USBH_int_private_functions + * @{ + */ + +/** + * @brief usb host interrupt handler + * @param otgdev: to the structure of otg_core_type + * @retval none + */ +void usbh_irq_handler(otg_core_type *otgdev) +{ + otg_global_type *usbx = otgdev->usb_reg; + usbh_core_type *uhost = &otgdev->host; + uint32_t intsts = usb_global_get_all_interrupt(usbx); + + if(usbx->gintsts_bit.curmode == 1) + { + if(intsts & USB_OTG_HCH_FLAG) + { + usbh_hch_handler(uhost); + usb_global_clear_interrupt(usbx, USB_OTG_HCH_FLAG); + } + if(intsts & USB_OTG_SOF_FLAG) + { + usbh_sof_handler(uhost); + usb_global_clear_interrupt(usbx, USB_OTG_SOF_FLAG); + } + if(intsts & USB_OTG_MODEMIS_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_MODEMIS_FLAG); + } + if(intsts & USB_OTG_WKUP_FLAG) + { + usbh_wakeup_handler(uhost); + usb_global_clear_interrupt(usbx, USB_OTG_WKUP_FLAG); + } + while(usbx->gintsts & USB_OTG_RXFLVL_FLAG) + { + usbh_rx_qlvl_handler(uhost); + usb_global_clear_interrupt(usbx, USB_OTG_RXFLVL_FLAG); + } + if(intsts & USB_OTG_DISCON_FLAG) + { + usbh_disconnect_handler(uhost); + usb_global_clear_interrupt(usbx, USB_OTG_DISCON_FLAG); + } + if(intsts & USB_OTG_PRT_FLAG) + { + usbh_port_handler(uhost); + } + if(intsts & USB_OTG_INCOMPIP_INCOMPISOOUT_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_INCOMPIP_INCOMPISOOUT_FLAG); + } + if(intsts & USB_OTG_INCOMISOIN_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_INCOMISOIN_FLAG); + } + if(intsts & USB_OTG_PTXFEMP_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_PTXFEMP_FLAG); + } + if(intsts & USB_OTG_ISOOUTDROP_FLAG) + { + usb_global_clear_interrupt(usbx, USB_OTG_ISOOUTDROP_FLAG); + } + + } +} + +/** + * @brief usb host wakeup handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_wakeup_handler(usbh_core_type *uhost) +{ + uhost->global_state = USBH_WAKEUP; +} + +/** + * @brief usb host sof handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_sof_handler(usbh_core_type *uhost) +{ + uhost->timer ++; +} + +/** + * @brief usb host disconnect handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_disconnect_handler(usbh_core_type *uhost) +{ + otg_global_type *usbx = uhost->usb_reg; + + uint8_t i_index; + + usb_host_disable(usbx); + + uhost->conn_sts = 0; + + uhost->global_state = USBH_DISCONNECT; + + for(i_index = 0; i_index < USB_HOST_CHANNEL_NUM; i_index ++) + { + usbh_free_channel(uhost, i_index); + } + usbh_fsls_clksel(usbx, USB_HCFG_CLK_48M); +} + +/** + * @brief usb host in transfer request handler + * @param uhost: to the structure of usbh_core_type + * @param chn: channel number + * @retval none + */ +void usbh_hch_in_handler(usbh_core_type *uhost, uint8_t chn) +{ + otg_global_type *usbx = uhost->usb_reg; + otg_hchannel_type *usb_chh = USB_CHL(usbx, chn); + uint32_t hcint_value = usb_chh->hcint & usb_chh->hcintmsk; + + if( hcint_value & USB_OTG_HC_ACK_FLAG) + { + usb_chh->hcint = USB_OTG_HC_ACK_FLAG; + } + else if(hcint_value & USB_OTG_HC_STALL_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_chh->hcint = USB_OTG_HC_NAK_FLAG | USB_OTG_HC_STALL_FLAG; + uhost->hch[chn].state = HCH_STALL; + usb_hch_halt(usbx, chn); + } + else if(hcint_value & USB_OTG_HC_DTGLERR_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_DTGLERR_FLAG | USB_OTG_HC_NAK_FLAG; + uhost->hch[chn].state = HCH_DATATGLERR; + } + + else if(hcint_value & USB_OTG_HC_FRMOVRRUN_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_FRMOVRRUN_FLAG; + } + else if(hcint_value & USB_OTG_HC_XFERC_FLAG) + { + uhost->hch[chn].state = HCH_XFRC; + usb_chh->hcint = USB_OTG_HC_XFERC_FLAG; + + if(usb_chh->hcchar_bit.eptype == EPT_BULK_TYPE || usb_chh->hcchar_bit.eptype == EPT_CONTROL_TYPE) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_NAK_FLAG; + } + else if(usb_chh->hcchar_bit.eptype == EPT_INT_TYPE) + { + usb_chh->hcchar_bit.oddfrm = TRUE; + uhost->urb_state[chn] = URB_DONE; + } + else if(usb_chh->hcchar_bit.eptype == EPT_ISO_TYPE) + { + uhost->urb_state[chn] = URB_DONE; + } + uhost->hch[chn].toggle_in ^= 1; + } + else if(hcint_value & USB_OTG_HC_CHHLTD_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = FALSE; + if(uhost->hch[chn].state == HCH_XFRC ) + { + uhost->urb_state[chn] = URB_DONE; + } + else if(uhost->hch[chn].state == HCH_STALL) + { + uhost->urb_state[chn] = URB_STALL; + } + else if(uhost->hch[chn].state == HCH_XACTERR || + uhost->hch[chn].state == HCH_DATATGLERR) + { + uhost->err_cnt[chn] ++; + if(uhost->err_cnt[chn] > 3) + { + uhost->urb_state[chn] = URB_ERROR; + uhost->err_cnt[chn] = 0; + } + else + { + uhost->urb_state[chn] = URB_NOTREADY; + } + usb_chh->hcchar_bit.chdis = FALSE; + usb_chh->hcchar_bit.chena = TRUE; + } + else if(uhost->hch[chn].state == HCH_NAK) + { + usb_chh->hcchar_bit.chdis = FALSE; + usb_chh->hcchar_bit.chena = TRUE; + uhost->urb_state[chn] = URB_NOTREADY; + } + usb_chh->hcint = USB_OTG_HC_CHHLTD_FLAG; + } + else if(hcint_value & USB_OTG_HC_XACTERR_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + uhost->hch[chn].state = HCH_XACTERR; + usb_hch_halt(usbx, chn); + uhost->err_cnt[chn] ++; + usb_chh->hcint = USB_OTG_HC_XACTERR_FLAG; + } + else if(hcint_value & USB_OTG_HC_NAK_FLAG) + { + if(usb_chh->hcchar_bit.eptype == EPT_INT_TYPE) + { + uhost->err_cnt[chn] = 0; + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + } + else if(usb_chh->hcchar_bit.eptype == EPT_BULK_TYPE || + usb_chh->hcchar_bit.eptype == EPT_CONTROL_TYPE) + { + uhost->err_cnt[chn] = 0; + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + } + uhost->hch[chn].state = HCH_NAK; + usb_chh->hcint = USB_OTG_HC_NAK_FLAG; + } + else if(hcint_value & USB_OTG_HC_BBLERR_FLAG) + { + usb_chh->hcint = USB_OTG_HC_BBLERR_FLAG; + } +} + +/** + * @brief usb host out transfer request handler + * @param uhost: to the structure of usbh_core_type + * @param chn: channel number + * @retval none + */ +void usbh_hch_out_handler(usbh_core_type *uhost, uint8_t chn) +{ + otg_global_type *usbx = uhost->usb_reg; + otg_hchannel_type *usb_chh = USB_CHL(usbx, chn); + uint32_t hcint_value = usb_chh->hcint & usb_chh->hcintmsk; + + if( hcint_value & USB_OTG_HC_ACK_FLAG) + { + usb_chh->hcint = USB_OTG_HC_ACK_FLAG; + } + else if( hcint_value & USB_OTG_HC_FRMOVRRUN_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_FRMOVRRUN_FLAG; + } + else if( hcint_value & USB_OTG_HC_XFERC_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + uhost->hch[chn].state = HCH_XFRC; + usb_chh->hcint = USB_OTG_HC_XFERC_FLAG; + } + else if( hcint_value & USB_OTG_HC_STALL_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_chh->hcint = USB_OTG_HC_STALL_FLAG; + uhost->hch[chn].state = HCH_STALL; + usb_hch_halt(usbx, chn); + } + else if( hcint_value & USB_OTG_HC_DTGLERR_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_DTGLERR_FLAG | USB_OTG_HC_NAK_FLAG; + uhost->hch[chn].state = HCH_DATATGLERR; + } + else if( hcint_value & USB_OTG_HC_CHHLTD_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = FALSE; + if(uhost->hch[chn].state == HCH_XFRC) + { + uhost->urb_state[chn] = URB_DONE; + if(uhost->hch[chn].ept_type == EPT_BULK_TYPE || + uhost->hch[chn].ept_type == EPT_INT_TYPE) + { + uhost->hch[chn].toggle_out ^= 1; + } + } + else if(uhost->hch[chn].state == HCH_NAK) + { + uhost->urb_state[chn] = URB_NOTREADY; + } + else if(uhost->hch[chn].state == HCH_STALL) + { + uhost->hch[chn].urb_sts = URB_STALL; + } + else if(uhost->hch[chn].state == HCH_XACTERR || + uhost->hch[chn].state == HCH_DATATGLERR) + { + uhost->err_cnt[chn] ++; + if(uhost->err_cnt[chn] > 3) + { + uhost->urb_state[chn] = URB_ERROR; + uhost->err_cnt[chn] = 0; + } + else + { + uhost->urb_state[chn] = URB_NOTREADY; + } + + usb_chh->hcchar_bit.chdis = FALSE; + usb_chh->hcchar_bit.chena = TRUE; + } + usb_chh->hcint = USB_OTG_HC_CHHLTD_FLAG; + } + else if( hcint_value & USB_OTG_HC_XACTERR_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + uhost->err_cnt[chn] ++; + uhost->hch[chn].state = HCH_XACTERR; + usb_hch_halt(usbx, chn); + usb_chh->hcint = USB_OTG_HC_XACTERR_FLAG | USB_OTG_HC_NAK_FLAG; + } + else if( hcint_value & USB_OTG_HC_NAK_FLAG) + { + usb_chh->hcintmsk_bit.chhltdmsk = TRUE; + uhost->err_cnt[chn] = 0; + usb_hch_halt(usbx, chn); + uhost->hch[chn].state = HCH_NAK; + usb_chh->hcint = USB_OTG_HC_NAK_FLAG; + } +} + +/** + * @brief usb host channel request handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_hch_handler(usbh_core_type *uhost) +{ + otg_global_type *usbx = uhost->usb_reg; + otg_host_type *usb_host = OTG_HOST(usbx); + uint32_t intsts, i_index; + + intsts = usb_host->haint & 0xFFFF; + for(i_index = 0; i_index < 16; i_index ++) + { + if(intsts & (1 << i_index)) + { + if(USB_CHL(usbx, i_index)->hcchar_bit.eptdir) + { + //hc in + usbh_hch_in_handler(uhost, i_index); + } + else + { + //hc out + usbh_hch_out_handler(uhost, i_index); + } + } + } +} + +/** + * @brief usb host rx buffer not empty request handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_rx_qlvl_handler(usbh_core_type *uhost) +{ + uint8_t chn; + uint32_t pktsts; + uint32_t pktcnt; + uint32_t tmp; + otg_hchannel_type *ch; + otg_global_type *usbx = uhost->usb_reg; + + usbx->gintmsk_bit.rxflvlmsk = 0; + + tmp = usbx->grxstsp; + chn = tmp & 0xF; + pktsts = (tmp >> 17) & 0xF; + pktcnt = (tmp >> 4) & 0x7FF; + ch = USB_CHL(usbx, chn); + switch(pktsts) + { + case PKTSTS_IN_DATA_PACKET_RECV: + if(pktcnt > 0 && (uhost->hch[chn].trans_buf) != 0) + { + usb_read_packet(usbx, uhost->hch[chn].trans_buf, chn, pktcnt); + uhost->hch[chn].trans_buf += pktcnt; + uhost->hch[chn].trans_count += pktcnt; + + if(ch->hctsiz_bit.pktcnt > 0) + { + ch->hcchar_bit.chdis = FALSE; + ch->hcchar_bit.chena = TRUE; + uhost->hch[chn].toggle_in ^= 1; + } + } + break; + case PKTSTS_IN_TRANSFER_COMPLETE: + break; + case PKTSTS_DATA_BIT_ERROR: + break; + case PKTSTS_CHANNEL_STOP: + break; + default: + break; + + } + usbx->gintmsk_bit.rxflvlmsk = 1; +} + +/** + * @brief usb host port request handler + * @param uhost: to the structure of usbh_core_type + * @retval none + */ +void usbh_port_handler(usbh_core_type *uhost) +{ + otg_global_type *usbx = uhost->usb_reg; + otg_host_type *usb_host = OTG_HOST(usbx); + + uint32_t prt = 0, prt_0; + + prt = usb_host->hprt; + prt_0 = prt; + + prt_0 &= ~(USB_OTG_HPRT_PRTENA | USB_OTG_HPRT_PRTENCHNG | + USB_OTG_HPRT_PRTOVRCACT | USB_OTG_HPRT_PRTCONDET); + if(prt & USB_OTG_HPRT_PRTCONDET) + { + if(prt & USB_OTG_HPRT_PRTCONSTS) + { + /* connect callback */ + uhost->conn_sts = 1; + } + prt_0 |= USB_OTG_HPRT_PRTCONDET; + } + + if(prt & USB_OTG_HPRT_PRTENCHNG) + { + prt_0 |= USB_OTG_HPRT_PRTENCHNG; + + if(prt & USB_OTG_HPRT_PRTENA) + { + if((prt & USB_OTG_HPRT_PRTSPD) == (USB_PRTSPD_LOW_SPEED << 17)) + { + usbh_fsls_clksel(usbx, USB_HCFG_CLK_6M); + } + else + { + usbh_fsls_clksel(usbx, USB_HCFG_CLK_48M); + } + /* connect callback */ + uhost->port_enable = 1; + } + else + { + /* clean up hprt */ + uhost->port_enable = 0; + } + } + + if(prt & USB_OTG_HPRT_PRTOVRCACT) + { + prt_0 |= USB_OTG_HPRT_PRTOVRCACT; + } + + usb_host->hprt = prt_0; +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + diff --git a/src/main/drivers/at32/usb/usbh_int.h b/src/main/drivers/at32/usb/usbh_int.h new file mode 100644 index 0000000000..30ac08581a --- /dev/null +++ b/src/main/drivers/at32/usb/usbh_int.h @@ -0,0 +1,75 @@ +/** + ************************************************************************** + * @file usbh_int.h + * @brief usb header file + ************************************************************************** + * Copyright notice & Disclaimer + * + * The software Board Support Package (BSP) that is made available to + * download from Artery official website is the copyrighted work of Artery. + * Artery authorizes customers to use, copy, and distribute the BSP + * software and its related documentation for the purpose of design and + * development in conjunction with Artery microcontrollers. Use of the + * software is governed by this copyright notice and the following disclaimer. + * + * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES, + * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS, + * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR + * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS, + * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT. + * + ************************************************************************** + */ + +/* define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USBH_INT_H +#define __USBH_INT_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** @addtogroup AT32F435_437_middlewares_usbh_drivers + * @{ + */ + +/** @addtogroup USBH_drivers_int + * @{ + */ + +/** @defgroup USBH_interrupt_exported_types + * @{ + */ + +/* includes ------------------------------------------------------------------*/ +#include "usbh_core.h" +#include "usb_core.h" +void usbh_irq_handler(otg_core_type *hdev); +void usbh_hch_handler(usbh_core_type *uhost); +void usbh_port_handler(usbh_core_type *uhost); +void usbh_disconnect_handler(usbh_core_type *uhost); +void usbh_hch_in_handler(usbh_core_type *uhost, uint8_t chn); +void usbh_hch_out_handler(usbh_core_type *uhost, uint8_t chn); +void usbh_rx_qlvl_handler(usbh_core_type *uhost); +void usbh_wakeup_handler(usbh_core_type *uhost); +void usbh_sof_handler(usbh_core_type *uhost); + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/src/main/drivers/at32/usb_msc_at32f43x.c b/src/main/drivers/at32/usb_msc_at32f43x.c new file mode 100644 index 0000000000..4ac913123b --- /dev/null +++ b/src/main/drivers/at32/usb_msc_at32f43x.c @@ -0,0 +1,197 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_USB_MSC) + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "blackbox/blackbox.h" + +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#include "pg/sdcard.h" +#include "pg/usb.h" + +#include "usb_conf.h" +#include "usb_core.h" +#include "usbd_int.h" +#include "msc_class.h" +#include "msc_desc.h" + +#include "usb_io.h" + +extern otg_core_type otg_core_struct; + +void msc_usb_gpio_config(void) +{ + gpio_init_type gpio_init_struct; + + crm_periph_clock_enable(OTG_PIN_GPIO_CLOCK, TRUE); + gpio_default_para_init(&gpio_init_struct); + + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + + /* dp and dm */ + gpio_init_struct.gpio_pins = OTG_PIN_DP | OTG_PIN_DM; + gpio_init(OTG_PIN_GPIO, &gpio_init_struct); + + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_DP_SOURCE, OTG_PIN_MUX); + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_DM_SOURCE, OTG_PIN_MUX); + +#ifdef USB_SOF_OUTPUT_ENABLE + crm_periph_clock_enable(OTG_PIN_SOF_GPIO_CLOCK, TRUE); + gpio_init_struct.gpio_pins = OTG_PIN_SOF; + gpio_init(OTG_PIN_SOF_GPIO, &gpio_init_struct); + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_SOF_SOURCE, OTG_PIN_MUX); +#endif + +#ifndef USB_VBUS_IGNORE + gpio_init_struct.gpio_pins = OTG_PIN_VBUS; + gpio_init_struct.gpio_pull = GPIO_PULL_DOWN; + gpio_pin_mux_config(OTG_PIN_GPIO, OTG_PIN_VBUS_SOURCE, OTG_PIN_MUX); + gpio_init(OTG_PIN_GPIO, &gpio_init_struct); +#endif + +} + +void msc_usb_clock48m_select(usb_clk48_s clk_s) +{ + if(clk_s == USB_CLK_HICK) { + crm_usb_clock_source_select(CRM_USB_CLOCK_SOURCE_HICK); + crm_periph_clock_enable(CRM_ACC_PERIPH_CLOCK, TRUE); + + acc_write_c1(7980); + acc_write_c2(8000); + acc_write_c3(8020); +#if (USB_ID == 0) + acc_sof_select(ACC_SOF_OTG1); +#else + acc_sof_select(ACC_SOF_OTG2); +#endif + acc_calibration_mode_enable(ACC_CAL_HICKTRIM, TRUE); + } else { + switch(system_core_clock) { + /* 48MHz */ + case 48000000: + crm_usb_clock_div_set(CRM_USB_DIV_1); + break; + + /* 72MHz */ + case 72000000: + crm_usb_clock_div_set(CRM_USB_DIV_1_5); + break; + + /* 96MHz */ + case 96000000: + crm_usb_clock_div_set(CRM_USB_DIV_2); + break; + + /* 120MHz */ + case 120000000: + crm_usb_clock_div_set(CRM_USB_DIV_2_5); + break; + + /* 144MHz */ + case 144000000: + crm_usb_clock_div_set(CRM_USB_DIV_3); + break; + + /* 168MHz */ + case 168000000: + crm_usb_clock_div_set(CRM_USB_DIV_3_5); + break; + + /* 192MHz */ + case 192000000: + crm_usb_clock_div_set(CRM_USB_DIV_4); + break; + + /* 216MHz */ + case 216000000: + crm_usb_clock_div_set(CRM_USB_DIV_4_5); + break; + + /* 240MHz */ + case 240000000: + crm_usb_clock_div_set(CRM_USB_DIV_5); + break; + + /* 264MHz */ + case 264000000: + crm_usb_clock_div_set(CRM_USB_DIV_5_5); + break; + + /* 288MHz */ + case 288000000: + crm_usb_clock_div_set(CRM_USB_DIV_6); + break; + + default: + break; + } + } +} + +uint8_t mscStart(void) +{ + usbGenerateDisconnectPulse(); + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + + msc_usb_gpio_config(); + crm_periph_clock_enable(OTG_CLOCK, TRUE); + msc_usb_clock48m_select(USB_CLK_HEXT); + nvic_irq_enable(OTG_IRQ, NVIC_PRIORITY_BASE(NVIC_PRIO_USB), NVIC_PRIORITY_SUB(NVIC_PRIO_USB)); + + usbd_init(&otg_core_struct, + USB_FULL_SPEED_CORE_ID, + USB_ID, + &msc_class_handler, + &msc_desc_handler); + + nvic_irq_disable(SysTick_IRQn); + + nvic_irq_enable(SysTick_IRQn, 0, 0); + + return 0; +} + +#endif diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index e2a95179b5..68ddd85346 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -31,62 +31,8 @@ struct ioPortDef_s { rccPeriphTag_t rcc; }; -#if defined(STM32F4) -const struct ioPortDef_s ioPortDefs[] = { - { RCC_AHB1(GPIOA) }, - { RCC_AHB1(GPIOB) }, - { RCC_AHB1(GPIOC) }, - { RCC_AHB1(GPIOD) }, - { RCC_AHB1(GPIOE) }, - { RCC_AHB1(GPIOF) }, -}; -#elif defined(STM32F7) -const struct ioPortDef_s ioPortDefs[] = { - { RCC_AHB1(GPIOA) }, - { RCC_AHB1(GPIOB) }, - { RCC_AHB1(GPIOC) }, - { RCC_AHB1(GPIOD) }, - { RCC_AHB1(GPIOE) }, - { RCC_AHB1(GPIOF) }, -}; -#elif defined(STM32H7) -const struct ioPortDef_s ioPortDefs[] = { - { RCC_AHB4(GPIOA) }, - { RCC_AHB4(GPIOB) }, - { RCC_AHB4(GPIOC) }, - { RCC_AHB4(GPIOD) }, - { RCC_AHB4(GPIOE) }, - { RCC_AHB4(GPIOF) }, - { RCC_AHB4(GPIOG) }, - { RCC_AHB4(GPIOH) }, -#if !(defined(STM32H723xx) || defined(STM32H725xx) || defined(STM32H730xx)) - { RCC_AHB4(GPIOI) }, -#endif -}; -#elif defined(STM32G4) -const struct ioPortDef_s ioPortDefs[] = { - { RCC_AHB2(GPIOA) }, - { RCC_AHB2(GPIOB) }, - { RCC_AHB2(GPIOC) }, - { RCC_AHB2(GPIOD) }, - { RCC_AHB2(GPIOE) }, - { RCC_AHB2(GPIOF) }, -}; -#elif defined(AT32F4) -const struct ioPortDef_s ioPortDefs[] = { - { RCC_AHB1(GPIOA) }, - { RCC_AHB1(GPIOB) }, - { RCC_AHB1(GPIOC) }, - { RCC_AHB1(GPIOD) }, - { RCC_AHB1(GPIOE) }, - { RCC_AHB1(GPIOF) }, - { RCC_AHB1(GPIOG) }, - { RCC_AHB1(GPIOH) } -}; -#elif defined(SITL) +#if defined(SITL) const struct ioPortDef_s ioPortDefs[] = { 0 }; -#else -# error "IO PortDefs not defined for MCU" #endif ioRec_t* IO_Rec(IO_t io) @@ -106,13 +52,12 @@ uint16_t IO_Pin(IO_t io) return ioRec->pin; } -// port index, GPIOA == 0 int IO_GPIOPortIdx(IO_t io) { if (!io) { return -1; } - return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); // ports are 0x400 apart + return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); } int IO_EXTI_PortSourceGPIO(IO_t io) @@ -131,7 +76,7 @@ int IO_GPIOPinIdx(IO_t io) if (!io) { return -1; } - return 31 - __builtin_clz(IO_Pin(io)); // CLZ is a bit faster than FFS + return 31 - __builtin_clz(IO_Pin(io)); } int IO_EXTI_PinSource(IO_t io) @@ -144,132 +89,6 @@ int IO_GPIO_PinSource(IO_t io) return IO_GPIOPinIdx(io); } -// mask on stm32f103, bit index on stm32f303 -uint32_t IO_EXTI_Line(IO_t io) -{ - if (!io) { - return 0; - } -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) - return 1 << IO_GPIOPinIdx(io); -#elif defined(SIMULATOR_BUILD) - return 0; -#else -# error "Unknown target type" -#endif -} - -bool IORead(IO_t io) -{ - if (!io) { - return false; - } -#if defined(USE_FULL_LL_DRIVER) - return (LL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io)); -#elif defined(USE_HAL_DRIVER) - return !! HAL_GPIO_ReadPin(IO_GPIO(io), IO_Pin(io)); -#elif defined(USE_ATBSP_DRIVER) - return (IO_GPIO(io)->idt & IO_Pin(io)); -#else - return (IO_GPIO(io)->IDR & IO_Pin(io)); -#endif -} - -void IOWrite(IO_t io, bool hi) -{ - if (!io) { - return; - } -#if defined(USE_FULL_LL_DRIVER) - LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16)); -#elif defined(USE_HAL_DRIVER) - HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), hi ? GPIO_PIN_SET : GPIO_PIN_RESET); -#elif defined(STM32F4) - if (hi) { - IO_GPIO(io)->BSRRL = IO_Pin(io); - } else { - IO_GPIO(io)->BSRRH = IO_Pin(io); - } -#elif defined(USE_ATBSP_DRIVER) - IO_GPIO(io)->scr = IO_Pin(io) << (hi ? 0 : 16); -#else - IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16); -#endif -} - -void IOHi(IO_t io) -{ - if (!io) { - return; - } -#if defined(USE_FULL_LL_DRIVER) - LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io)); -#elif defined(USE_HAL_DRIVER) - HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_SET); -#elif defined(STM32F4) - IO_GPIO(io)->BSRRL = IO_Pin(io); -#elif defined(USE_ATBSP_DRIVER) - IO_GPIO(io)->scr = IO_Pin(io); -#else - IO_GPIO(io)->BSRR = IO_Pin(io); -#endif -} - -void IOLo(IO_t io) -{ - if (!io) { - return; - } -#if defined(USE_FULL_LL_DRIVER) - LL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io)); -#elif defined(USE_HAL_DRIVER) - HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_RESET); -#elif defined(STM32F4) - IO_GPIO(io)->BSRRH = IO_Pin(io); -#elif defined(USE_ATBSP_DRIVER) - IO_GPIO(io)->clr = IO_Pin(io); -#else - IO_GPIO(io)->BRR = IO_Pin(io); -#endif -} - -void IOToggle(IO_t io) -{ - if (!io) { - return; - } - - uint32_t mask = IO_Pin(io); - // Read pin state from ODR but write to BSRR because it only changes the pins - // high in the mask value rather than all pins. XORing ODR directly risks - // setting other pins incorrectly because it change all pins' state. -#if defined(USE_FULL_LL_DRIVER) - if (LL_GPIO_ReadOutputPort(IO_GPIO(io)) & mask) { - mask <<= 16; // bit is set, shift mask to reset half - } - LL_GPIO_SetOutputPin(IO_GPIO(io), mask); -#elif defined(USE_HAL_DRIVER) - UNUSED(mask); - HAL_GPIO_TogglePin(IO_GPIO(io), IO_Pin(io)); -#elif defined(STM32F4) - if (IO_GPIO(io)->ODR & mask) { - IO_GPIO(io)->BSRRH = mask; - } else { - IO_GPIO(io)->BSRRL = mask; - } -#elif defined(USE_ATBSP_DRIVER) - if (IO_GPIO(io)->odt & mask) { - mask <<= 16; // bit is set, shift mask to reset half - } - IO_GPIO(io)->scr = mask; -#else - if (IO_GPIO(io)->ODR & mask) { - mask <<= 16; // bit is set, shift mask to reset half - } - IO_GPIO(io)->BSRR = mask; -#endif -} - // claim IO pin, set owner and resources void IOInit(IO_t io, resourceOwner_e owner, uint8_t index) { @@ -310,146 +129,6 @@ bool IOIsFreeOrPreinit(IO_t io) return false; } -#if defined(STM32H7) || defined(STM32G4) - -void IOConfigGPIO(IO_t io, ioConfig_t cfg) -{ - IOConfigGPIOAF(io, cfg, 0); -} - -void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) -{ - if (!io) { - return; - } - - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - GPIO_InitTypeDef init = { - .Pin = IO_Pin(io), - .Mode = (cfg >> 0) & 0x13, - .Speed = (cfg >> 2) & 0x03, - .Pull = (cfg >> 5) & 0x03, - .Alternate = af - }; - - HAL_GPIO_Init(IO_GPIO(io), &init); -} - -#elif defined(STM32F7) - -void IOConfigGPIO(IO_t io, ioConfig_t cfg) -{ - IOConfigGPIOAF(io, cfg, 0); -} - -void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) -{ - if (!io) { - return; - } - - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - LL_GPIO_InitTypeDef init = { - .Pin = IO_Pin(io), - .Mode = (cfg >> 0) & 0x03, - .Speed = (cfg >> 2) & 0x03, - .OutputType = (cfg >> 4) & 0x01, - .Pull = (cfg >> 5) & 0x03, - .Alternate = af - }; - - LL_GPIO_Init(IO_GPIO(io), &init); -} - -#elif defined(STM32F4) - -void IOConfigGPIO(IO_t io, ioConfig_t cfg) -{ - if (!io) { - return; - } - - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - GPIO_InitTypeDef init = { - .GPIO_Pin = IO_Pin(io), - .GPIO_Mode = (cfg >> 0) & 0x03, - .GPIO_Speed = (cfg >> 2) & 0x03, - .GPIO_OType = (cfg >> 4) & 0x01, - .GPIO_PuPd = (cfg >> 5) & 0x03, - }; - GPIO_Init(IO_GPIO(io), &init); -} - -void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) -{ - if (!io) { - return; - } - - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af); - - GPIO_InitTypeDef init = { - .GPIO_Pin = IO_Pin(io), - .GPIO_Mode = (cfg >> 0) & 0x03, - .GPIO_Speed = (cfg >> 2) & 0x03, - .GPIO_OType = (cfg >> 4) & 0x01, - .GPIO_PuPd = (cfg >> 5) & 0x03, - }; - GPIO_Init(IO_GPIO(io), &init); -} - -#elif defined(AT32F4) - -//TODO: move to specific files (applies to STM32 also) -void IOConfigGPIO(IO_t io, ioConfig_t cfg) -{ - if (!io) { - return; - } - - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - gpio_init_type init = { - .gpio_pins = IO_Pin(io), - .gpio_mode = (cfg >> 0) & 0x03, - .gpio_drive_strength = (cfg >> 2) & 0x03, - .gpio_out_type = (cfg >> 4) & 0x01, - .gpio_pull = (cfg >> 5) & 0x03, - }; - gpio_init(IO_GPIO(io), &init); -} - -void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) -{ - if (!io) { - return; - } - - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - gpio_init_type init = { - .gpio_pins = IO_Pin(io), - .gpio_mode = (cfg >> 0) & 0x03, - .gpio_drive_strength = (cfg >> 2) & 0x03, - .gpio_out_type = (cfg >> 4) & 0x01, - .gpio_pull = (cfg >> 5) & 0x03, - }; - gpio_init(IO_GPIO(io), &init); - gpio_pin_mux_config(IO_GPIO(io), IO_GPIO_PinSource(io), af); -} - -#endif - #if DEFIO_PORT_USED_COUNT > 0 static const uint16_t ioDefUsedMask[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_USED_LIST }; static const uint8_t ioDefUsedOffset[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_OFFSET_LIST }; diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/stm32/dshot_bitbang.c similarity index 100% rename from src/main/drivers/dshot_bitbang.c rename to src/main/drivers/stm32/dshot_bitbang.c diff --git a/src/main/drivers/exti.c b/src/main/drivers/stm32/exti.c similarity index 76% rename from src/main/drivers/exti.c rename to src/main/drivers/stm32/exti.c index 4ab20a1782..1ddf71b1aa 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/stm32/exti.c @@ -24,10 +24,10 @@ #include "platform.h" -#if !defined(SIMULATOR_BUILD) +#ifdef USE_EXTI #include "drivers/nvic.h" -#include "io_impl.h" +#include "drivers/io_impl.h" #include "drivers/exti.h" typedef struct { @@ -52,33 +52,19 @@ static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { EXTI9_5_IRQn, EXTI15_10_IRQn }; -#elif defined(USE_ATBSP_DRIVER) -static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { - EXINT0_IRQn, - EXINT1_IRQn, - EXINT2_IRQn, - EXINT3_IRQn, - EXINT4_IRQn, - EXINT9_5_IRQn, - EXINT15_10_IRQn -}; #else # warning "Unknown CPU" #endif static uint32_t triggerLookupTable[] = { #if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) - [BETAFLIGHT_EXTI_TRIGGER_RISING] = GPIO_MODE_IT_RISING, - [BETAFLIGHT_EXTI_TRIGGER_FALLING] = GPIO_MODE_IT_FALLING, - [BETAFLIGHT_EXTI_TRIGGER_BOTH] = GPIO_MODE_IT_RISING_FALLING + [BETAFLIGHT_EXTI_TRIGGER_RISING] = GPIO_MODE_IT_RISING, + [BETAFLIGHT_EXTI_TRIGGER_FALLING] = GPIO_MODE_IT_FALLING, + [BETAFLIGHT_EXTI_TRIGGER_BOTH] = GPIO_MODE_IT_RISING_FALLING #elif defined(STM32F4) - [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXTI_Trigger_Rising, - [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXTI_Trigger_Falling, - [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXTI_Trigger_Rising_Falling -#elif defined(AT32F4) - [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXINT_TRIGGER_RISING_EDGE, - [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXINT_TRIGGER_FALLING_EDGE, - [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXINT_TRIGGER_BOTH_EDGE + [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXTI_Trigger_Rising, + [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXTI_Trigger_Falling, + [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXTI_Trigger_Rising_Falling #else # warning "Unknown CPU" #endif @@ -92,9 +78,6 @@ static uint32_t triggerLookupTable[] = { #elif defined(STM32G4) #define EXTI_REG_IMR (EXTI->IMR1) #define EXTI_REG_PR (EXTI->PR1) -#elif defined(USE_ATBSP_DRIVER) -#define EXTI_REG_IMR (EXINT->inten) -#define EXTI_REG_PR (EXINT->intsts) #else #define EXTI_REG_IMR (EXTI->IMR) #define EXTI_REG_PR (EXTI->PR) @@ -111,10 +94,7 @@ void EXTIInit(void) #ifdef REMAP_TIM17_DMA SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, ENABLE); #endif -#elif defined(USE_ATBSP_DRIVER) - crm_periph_clock_enable(CRM_SCFG_PERIPH_CLOCK, TRUE); #endif - memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); } @@ -178,33 +158,6 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); } -#elif defined(USE_ATBSP_DRIVER) - scfg_exint_line_config(IO_GPIO_PortSource(io), IO_GPIO_PinSource(io)); - - uint32_t extiLine = IO_EXTI_Line(io); - - exint_flag_clear(extiLine); - - exint_init_type exint_init_struct; - exint_default_para_init(&exint_init_struct); - exint_init_struct.line_enable = TRUE; - exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT; - exint_init_struct.line_select = extiLine; - exint_init_struct.line_polarity = triggerLookupTable[trigger]; - exint_init(&exint_init_struct); - - if (extiGroupPriority[group] > irqPriority) { - extiGroupPriority[group] = irqPriority; - - nvic_priority_group_config(NVIC_PRIORITY_GROUPING); - nvic_irq_enable( - extiGroupIRQn[group], - NVIC_PRIORITY_BASE(irqPriority), - NVIC_PRIORITY_SUB(irqPriority) - ); - } -#else -# warning "Unknown CPU" #endif #endif @@ -227,7 +180,7 @@ void EXTIRelease(IO_t io) void EXTIEnable(IO_t io) { -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) uint32_t extiLine = IO_EXTI_Line(io); if (!extiLine) { @@ -243,7 +196,7 @@ void EXTIEnable(IO_t io) void EXTIDisable(IO_t io) { -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) uint32_t extiLine = IO_EXTI_Line(io); if (!extiLine) { @@ -257,7 +210,6 @@ void EXTIDisable(IO_t io) #endif } - #define EXTI_EVENT_MASK 0xFFFF // first 16 bits only, see also definition of extiChannelRecs. void EXTI_IRQHandler(uint32_t mask) @@ -284,7 +236,7 @@ void EXTI_IRQHandler(uint32_t mask) _EXTI_IRQ_HANDLER(EXTI0_IRQHandler, 0x0001); _EXTI_IRQ_HANDLER(EXTI1_IRQHandler, 0x0002); -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) _EXTI_IRQ_HANDLER(EXTI2_IRQHandler, 0x0004); #else # warning "Unknown CPU" diff --git a/src/main/drivers/stm32/io_stm32.c b/src/main/drivers/stm32/io_stm32.c new file mode 100644 index 0000000000..2557f09764 --- /dev/null +++ b/src/main/drivers/stm32/io_stm32.c @@ -0,0 +1,290 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" + +#include "common/utils.h" + +// io ports defs are stored in array by index now +struct ioPortDef_s { + rccPeriphTag_t rcc; +}; + +#if defined(STM32F4) +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB1(GPIOA) }, + { RCC_AHB1(GPIOB) }, + { RCC_AHB1(GPIOC) }, + { RCC_AHB1(GPIOD) }, + { RCC_AHB1(GPIOE) }, + { RCC_AHB1(GPIOF) }, +}; +#elif defined(STM32F7) +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB1(GPIOA) }, + { RCC_AHB1(GPIOB) }, + { RCC_AHB1(GPIOC) }, + { RCC_AHB1(GPIOD) }, + { RCC_AHB1(GPIOE) }, + { RCC_AHB1(GPIOF) }, +}; +#elif defined(STM32H7) +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB4(GPIOA) }, + { RCC_AHB4(GPIOB) }, + { RCC_AHB4(GPIOC) }, + { RCC_AHB4(GPIOD) }, + { RCC_AHB4(GPIOE) }, + { RCC_AHB4(GPIOF) }, + { RCC_AHB4(GPIOG) }, + { RCC_AHB4(GPIOH) }, +#if !(defined(STM32H723xx) || defined(STM32H725xx) || defined(STM32H730xx)) + { RCC_AHB4(GPIOI) }, +#endif +}; +#elif defined(STM32G4) +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB2(GPIOA) }, + { RCC_AHB2(GPIOB) }, + { RCC_AHB2(GPIOC) }, + { RCC_AHB2(GPIOD) }, + { RCC_AHB2(GPIOE) }, + { RCC_AHB2(GPIOF) }, +}; +#else +# error "IO PortDefs not defined for MCU" +#endif + +// mask on stm32f103, bit index on stm32f303 +uint32_t IO_EXTI_Line(IO_t io) +{ + if (!io) { + return 0; + } +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) + return 1 << IO_GPIOPinIdx(io); +#elif defined(SIMULATOR_BUILD) + return 0; +#else +# error "Unknown target type" +#endif +} + +bool IORead(IO_t io) +{ + if (!io) { + return false; + } +#if defined(USE_FULL_LL_DRIVER) + return (LL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io)); +#elif defined(USE_HAL_DRIVER) + return !! HAL_GPIO_ReadPin(IO_GPIO(io), IO_Pin(io)); +#else + return (IO_GPIO(io)->IDR & IO_Pin(io)); +#endif +} + +void IOWrite(IO_t io, bool hi) +{ + if (!io) { + return; + } +#if defined(USE_FULL_LL_DRIVER) + LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16)); +#elif defined(USE_HAL_DRIVER) + HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), hi ? GPIO_PIN_SET : GPIO_PIN_RESET); +#elif defined(STM32F4) + if (hi) { + IO_GPIO(io)->BSRRL = IO_Pin(io); + } else { + IO_GPIO(io)->BSRRH = IO_Pin(io); + } +#else + IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16); +#endif +} + +void IOHi(IO_t io) +{ + if (!io) { + return; + } +#if defined(USE_FULL_LL_DRIVER) + LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io)); +#elif defined(USE_HAL_DRIVER) + HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_SET); +#elif defined(STM32F4) + IO_GPIO(io)->BSRRL = IO_Pin(io); +#else + IO_GPIO(io)->BSRR = IO_Pin(io); +#endif +} + +void IOLo(IO_t io) +{ + if (!io) { + return; + } +#if defined(USE_FULL_LL_DRIVER) + LL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io)); +#elif defined(USE_HAL_DRIVER) + HAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_RESET); +#elif defined(STM32F4) + IO_GPIO(io)->BSRRH = IO_Pin(io); +#else + IO_GPIO(io)->BRR = IO_Pin(io); +#endif +} + +void IOToggle(IO_t io) +{ + if (!io) { + return; + } + + uint32_t mask = IO_Pin(io); + // Read pin state from ODR but write to BSRR because it only changes the pins + // high in the mask value rather than all pins. XORing ODR directly risks + // setting other pins incorrectly because it change all pins' state. +#if defined(USE_FULL_LL_DRIVER) + if (LL_GPIO_ReadOutputPort(IO_GPIO(io)) & mask) { + mask <<= 16; // bit is set, shift mask to reset half + } + LL_GPIO_SetOutputPin(IO_GPIO(io), mask); +#elif defined(USE_HAL_DRIVER) + UNUSED(mask); + HAL_GPIO_TogglePin(IO_GPIO(io), IO_Pin(io)); +#elif defined(STM32F4) + if (IO_GPIO(io)->ODR & mask) { + IO_GPIO(io)->BSRRH = mask; + } else { + IO_GPIO(io)->BSRRL = mask; + } +#else + if (IO_GPIO(io)->ODR & mask) { + mask <<= 16; // bit is set, shift mask to reset half + } + IO_GPIO(io)->BSRR = mask; +#endif +} + +#if defined(STM32H7) || defined(STM32G4) + +void IOConfigGPIO(IO_t io, ioConfig_t cfg) +{ + IOConfigGPIOAF(io, cfg, 0); +} + +void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) +{ + if (!io) { + return; + } + + rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + GPIO_InitTypeDef init = { + .Pin = IO_Pin(io), + .Mode = (cfg >> 0) & 0x13, + .Speed = (cfg >> 2) & 0x03, + .Pull = (cfg >> 5) & 0x03, + .Alternate = af + }; + + HAL_GPIO_Init(IO_GPIO(io), &init); +} + +#elif defined(STM32F7) + +void IOConfigGPIO(IO_t io, ioConfig_t cfg) +{ + IOConfigGPIOAF(io, cfg, 0); +} + +void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + LL_GPIO_InitTypeDef init = { + .Pin = IO_Pin(io), + .Mode = (cfg >> 0) & 0x03, + .Speed = (cfg >> 2) & 0x03, + .OutputType = (cfg >> 4) & 0x01, + .Pull = (cfg >> 5) & 0x03, + .Alternate = af + }; + + LL_GPIO_Init(IO_GPIO(io), &init); +} + +#elif defined(STM32F4) + +void IOConfigGPIO(IO_t io, ioConfig_t cfg) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + GPIO_InitTypeDef init = { + .GPIO_Pin = IO_Pin(io), + .GPIO_Mode = (cfg >> 0) & 0x03, + .GPIO_Speed = (cfg >> 2) & 0x03, + .GPIO_OType = (cfg >> 4) & 0x01, + .GPIO_PuPd = (cfg >> 5) & 0x03, + }; + GPIO_Init(IO_GPIO(io), &init); +} + +void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af); + + GPIO_InitTypeDef init = { + .GPIO_Pin = IO_Pin(io), + .GPIO_Mode = (cfg >> 0) & 0x03, + .GPIO_Speed = (cfg >> 2) & 0x03, + .GPIO_OType = (cfg >> 4) & 0x01, + .GPIO_PuPd = (cfg >> 5) & 0x03, + }; + GPIO_Init(IO_GPIO(io), &init); +} + +#else +# warning MCU not set +#endif diff --git a/src/main/drivers/persistent.c b/src/main/drivers/stm32/persistent.c similarity index 100% rename from src/main/drivers/persistent.c rename to src/main/drivers/stm32/persistent.c diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/stm32/pwm_output.c similarity index 100% rename from src/main/drivers/pwm_output.c rename to src/main/drivers/stm32/pwm_output.c diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/stm32/pwm_output_dshot.c similarity index 99% rename from src/main/drivers/pwm_output_dshot.c rename to src/main/drivers/stm32/pwm_output_dshot.c index ded7bd2714..d162d5859d 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/stm32/pwm_output_dshot.c @@ -40,12 +40,11 @@ #include "stm32f4xx.h" #endif -#include "pwm_output.h" +#include "drivers/pwm_output.h" #include "drivers/dshot.h" #include "drivers/dshot_dpwm.h" #include "drivers/dshot_command.h" - -#include "pwm_output_dshot_shared.h" +#include "drivers/pwm_output_dshot_shared.h" #ifdef USE_DSHOT_TELEMETRY diff --git a/src/main/drivers/stm32/pwm_output_dshot_hal_hal.c b/src/main/drivers/stm32/pwm_output_dshot_hal_hal.c deleted file mode 100644 index b4a293274b..0000000000 --- a/src/main/drivers/stm32/pwm_output_dshot_hal_hal.c +++ /dev/null @@ -1,480 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#ifdef USE_DSHOT - -#include "drivers/dma_reqmap.h" -#include "drivers/io.h" -#include "timer.h" -#include "pwm_output.h" -#include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" -#include "drivers/dshot_command.h" -#include "drivers/nvic.h" -#include "dma.h" -#include "rcc.h" -#include "pg/timerup.h" - -static HAL_StatusTypeDef result; - -static uint8_t dmaMotorTimerCount = 0; -static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; -static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; - -motorDmaOutput_t *getMotorDmaOutput(uint8_t index) -{ - return &dmaMotors[index]; -} - -uint8_t getTimerIndex(TIM_TypeDef *timer) -{ - for (int i = 0; i < dmaMotorTimerCount; i++) { - if (dmaMotorTimers[i].timer == timer) { - return i; - } - } - dmaMotorTimers[dmaMotorTimerCount++].timer = timer; - return dmaMotorTimerCount - 1; -} - -// TIM_CHANNEL_x TIM_CHANNEL_x/4 TIM_DMA_ID_CCx TIM_DMA_CCx -// 0x0 0 1 0x0200 -// 0x4 1 2 0x0400 -// 0x8 2 3 0x0800 -// 0xC 3 4 0x1000 -// -// TIM_CHANNEL_TO_TIM_DMA_ID_CC = (TIM_CHANNEL_x / 4) + 1 -// TIM_CHANNEL_TO_TIM_DMA_CC = 0x200 << (TIM_CHANNEL_x / 4) - -// pwmChannelDMAStart -// A variant of HAL_TIM_PWM_Start_DMA/HAL_TIMEx_PWMN_Start_DMA that only disables DMA on a timer channel: -// 1. Configure and enable DMA Stream -// 2. Enable DMA request on a timer channel - -void pwmChannelDMAStart(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length) -{ - switch (Channel) { - case TIM_CHANNEL_1: - HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC1], (uint32_t)pData, (uint32_t)&htim->Instance->CCR1, Length); - __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC1); - break; - case TIM_CHANNEL_2: - HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC2], (uint32_t)pData, (uint32_t)&htim->Instance->CCR2, Length); - __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC2); - break; - case TIM_CHANNEL_3: - HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC3], (uint32_t)pData, (uint32_t)&htim->Instance->CCR3,Length); - __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC3); - break; - case TIM_CHANNEL_4: - HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC4], (uint32_t)pData, (uint32_t)&htim->Instance->CCR4, Length); - __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC4); - break; - } -} - -// pwmChannelDMAStop -// A variant of HAL_TIM_PWM_Stop_DMA/HAL_TIMEx_PWMN_Stop_DMA that only disables DMA on a timer channel -// 1. Disable the TIM Capture/Compare 1 DMA request - -void pwmChannelDMAStop(TIM_HandleTypeDef *htim, uint32_t Channel) -{ - switch (Channel) { - case TIM_CHANNEL_1: - __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC1); - break; - case TIM_CHANNEL_2: - __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC2); - break; - case TIM_CHANNEL_3: - __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC3); - break; - case TIM_CHANNEL_4: - __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC4); - break; - } -} - -// pwmBurstDMAStart -// A variant of HAL_TIM_DMABurst_WriteStart that can handle multiple bursts. -// (HAL_TIM_DMABurst_WriteStart can only handle single burst) - -void pwmBurstDMAStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32_t BurstRequestSrc, uint32_t BurstUnit, uint32_t* BurstBuffer, uint32_t BurstLength) -{ - // Setup DMA stream - HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_UPDATE], (uint32_t)BurstBuffer, (uint32_t)&htim->Instance->DMAR, BurstLength); - - // Configure burst mode DMA */ - htim->Instance->DCR = BurstBaseAddress | BurstUnit; - - // Enable burst mode DMA - __HAL_TIM_ENABLE_DMA(htim, BurstRequestSrc); -} - -FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) -{ - motorDmaOutput_t *const motor = &dmaMotors[index]; - - if (!motor->configured) { - return; - } - - /*If there is a command ready to go overwrite the value and send that instead*/ - if (dshotCommandIsProcessing()) { - value = dshotCommandGetCurrent(index); - if (value) { - motor->protocolControl.requestTelemetry = true; - } - } - - if (!motor->timerHardware -#ifndef USE_DMA_SPEC - // When USE_DMA_SPEC is in effect, motor->timerHardware remains NULL if valid DMA is not assigned. - || !motor->timerHardware->dmaRef -#endif - ) - { - return; - } - - motor->protocolControl.value = value; - - uint16_t packet = prepareDshotPacket(&motor->protocolControl); - uint8_t bufferSize; - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet); - motor->timer->dmaBurstLength = bufferSize * 4; - } else -#endif - { - bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet); - - pwmChannelDMAStart(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize); - } -} - -void pwmCompleteDshotMotorUpdate(void) -{ - // If there is a dshot command loaded up, time it correctly with motor update - - if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) { - return; - } - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - for (int i = 0; i < dmaMotorTimerCount; i++) { - motorDmaTimer_t *burstDmaTimer = &dmaMotorTimers[i]; - - // Transfer CCR1 through CCR4 for each burst - pwmBurstDMAStart(&burstDmaTimer->timHandle, - TIM_DMABASE_CCR1, TIM_DMA_UPDATE, TIM_DMABURSTLENGTH_4TRANSFERS, - (uint32_t*)burstDmaTimer->dmaBurstBuffer, burstDmaTimer->dmaBurstLength); - } - } else -#endif - { - // XXX Empty for non-burst? - } -} - -FAST_IRQ_HANDLER static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - motorDmaTimer_t *burstDmaTimer = &dmaMotorTimers[descriptor->userParam]; - - HAL_TIM_DMABurst_WriteStop(&burstDmaTimer->timHandle, TIM_DMA_UPDATE); - HAL_DMA_IRQHandler(&burstDmaTimer->hdma_tim); - } else -#endif - { - motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; - - pwmChannelDMAStop(&motor->TimHandle,motor->timerHardware->channel); - HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaIndex]); - } - - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -} - -bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) -{ - dmaResource_t *dmaRef = NULL; - uint32_t dmaChannel; - -#ifdef USE_DMA_SPEC - const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware); - - if (dmaSpec) { - dmaRef = dmaSpec->ref; - dmaChannel = dmaSpec->channel; - } -#else - dmaRef = timerHardware->dmaRef; - dmaChannel = timerHardware->dmaChannel; -#endif - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { -#ifdef USE_DMA_SPEC - uint8_t timnum = timerGetTIMNumber(timerHardware->tim); - dmaoptValue_t dmaopt = timerUpConfig(timnum - 1)->dmaopt; - const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_TIMUP, timnum - 1, dmaopt); - dmaRef = dmaChannelSpec->ref; - dmaChannel = dmaChannelSpec->channel; -#else - dmaRef = timerHardware->dmaTimUPRef; - dmaChannel = timerHardware->dmaTimUPChannel; -#endif - } -#endif - - if (dmaRef == NULL) { - return false; - } - - dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef); - - bool dmaIsConfigured = false; -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier); - if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) { - dmaIsConfigured = true; - } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) { - return false; - } - } else -#endif - { - if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) { - return false; - } - } - - motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; - motor->timerHardware = timerHardware; - - TIM_TypeDef *timer = timerHardware->tim; // "timer" is confusing; "tim"? - const IO_t motorIO = IOGetByTag(timerHardware->tag); - uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP; -#ifdef USE_DSHOT_TELEMETRY - if (useDshotTelemetry) { - output ^= TIMER_OUTPUT_INVERTED; - } -#endif - - motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, pupMode); - const uint8_t timerIndex = getTimerIndex(timer); - const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1); - - IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction); - - // Configure time base - - if (configureTimer) { - RCC_ClockCmd(timerRCC(timer), ENABLE); - - motor->TimHandle.Instance = timerHardware->tim; // timer - motor->TimHandle.Init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; - motor->TimHandle.Init.RepetitionCounter = 0; - motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - motor->TimHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - - result = HAL_TIM_PWM_Init(&motor->TimHandle); - - if (result != HAL_OK) { - /* Initialization Error */ - return false; - } - } - -/* -From LL version -chan oinv IDLE NIDLE POL NPOL -N I - Low - Low -N - - Low - High -P I High - Low - -P - High - High - -*/ - - /* PWM mode 1 configuration */ - - TIM_OC_InitTypeDef TIM_OCInitStructure; - TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - - if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; - TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; - } else { - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; - TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET; - TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; - } - TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - TIM_OCInitStructure.Pulse = 0; - - result = HAL_TIM_PWM_ConfigChannel(&motor->TimHandle, &TIM_OCInitStructure, motor->timerHardware->channel); - - if (result != HAL_OK) { - /* Configuration Error */ - return false; - } - - // DMA setup - - motor->timer = &dmaMotorTimers[timerIndex]; - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - motor->timer->dmaBurstRef = dmaRef; - - if (!configureTimer) { - motor->configured = true; - return false; - } - } else -#endif - { - motor->timerDmaSource = timerDmaSource(timerHardware->channel); - motor->timer->timerDmaSources |= motor->timerDmaSource; - motor->timerDmaIndex = timerDmaIndex(timerHardware->channel); - } - - if (!dmaIsConfigured) { - dmaEnable(dmaIdentifier); -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex); - } else -#endif - { - dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex); - } - } - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - motor->timer->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; - motor->timer->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; - motor->timer->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; - motor->timer->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; - motor->timer->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; - motor->timer->hdma_tim.Init.Mode = DMA_NORMAL; - motor->timer->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; -#if !defined(STM32G4) - motor->timer->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - motor->timer->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; - motor->timer->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; - motor->timer->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; -#endif - - motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0]; - motor->timer->timHandle = motor->TimHandle; - memset(motor->timer->dmaBurstBuffer, 0, DSHOT_DMA_BUFFER_SIZE * 4 * sizeof(uint32_t)); - - /* Set hdma_tim instance */ - motor->timer->hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef; - motor->timer->hdma_tim.Init.Request = dmaChannel; - - /* Link hdma_tim to hdma[TIM_DMA_ID_UPDATE] (update) */ - __HAL_LINKDMA(&motor->timer->timHandle, hdma[TIM_DMA_ID_UPDATE], motor->timer->hdma_tim); - - if (!dmaIsConfigured) { - /* Initialize TIMx DMA handle */ - result = HAL_DMA_Init(motor->timer->timHandle.hdma[TIM_DMA_ID_UPDATE]); - } else { - result = HAL_OK; - } - } else -#endif - { - motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; - motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; - motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; - motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; - motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; - motor->hdma_tim.Init.Mode = DMA_NORMAL; - motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; -#if !defined(STM32G4) - motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - motor->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; - motor->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; - motor->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; -#endif - - motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0]; - motor->dmaBuffer[DSHOT_DMA_BUFFER_SIZE-2] = 0; // XXX Is this necessary? -> probably. - motor->dmaBuffer[DSHOT_DMA_BUFFER_SIZE-1] = 0; // XXX Is this necessary? - - motor->hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef; - motor->hdma_tim.Init.Request = dmaChannel; - - /* Link hdma_tim to hdma[x] (channelx) */ - __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaIndex], motor->hdma_tim); - - /* Initialize TIMx DMA handle */ - result = HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaIndex]); - - } - - if (result != HAL_OK) { - /* Initialization Error */ - return false; - } - - // Start the timer channel now. - // Enabling/disabling DMA request can restart a new cycle without PWM start/stop. - - if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - result = HAL_TIMEx_PWMN_Start(&motor->TimHandle, motor->timerHardware->channel); - } else { - result = HAL_TIM_PWM_Start(&motor->TimHandle, motor->timerHardware->channel); - } - - if (result != HAL_OK) { - /* Starting PWM generation Error */ - return false; - } - - motor->configured = true; - - return true; -} -#endif diff --git a/src/main/drivers/rcc.c b/src/main/drivers/stm32/rcc_stm32.c similarity index 99% rename from src/main/drivers/rcc.c rename to src/main/drivers/stm32/rcc_stm32.c index 130d563247..f8afd03f36 100644 --- a/src/main/drivers/rcc.c +++ b/src/main/drivers/stm32/rcc_stm32.c @@ -19,7 +19,7 @@ */ #include "platform.h" -#include "rcc.h" +#include "drivers/rcc.h" void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) { diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/stm32/serial_usb_vcp.c similarity index 97% rename from src/main/drivers/serial_usb_vcp.c rename to src/main/drivers/stm32/serial_usb_vcp.c index 0449dcb656..808433d21e 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/stm32/serial_usb_vcp.c @@ -39,10 +39,10 @@ #ifdef USE_USB_CDC_HID #include "usbd_hid_cdc_wrapper.h" #endif -#include "usb_io.h" +#include "drivers/usb_io.h" #elif defined(STM32F7) || defined(STM32H7) || defined(STM32G4) -#include "vcp_hal/usbd_cdc_interface.h" -#include "usb_io.h" +#include "drivers/stm32/vcp_hal/usbd_cdc_interface.h" +#include "drivers/usb_io.h" #ifdef USE_USB_CDC_HID #include "usbd_ioreq.h" @@ -57,8 +57,8 @@ USBD_HandleTypeDef USBD_Device; #include "drivers/time.h" -#include "serial.h" -#include "serial_usb_vcp.h" +#include "drivers/serial.h" +#include "drivers/serial_usb_vcp.h" #define USB_TIMEOUT 50 diff --git a/src/main/drivers/timer.c b/src/main/drivers/stm32/timer_stdperiph.c similarity index 99% rename from src/main/drivers/timer.c rename to src/main/drivers/stm32/timer_stdperiph.c index a2e0eec56f..ec0e008c2d 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/stm32/timer_stdperiph.c @@ -34,11 +34,11 @@ #include "drivers/nvic.h" #include "drivers/io.h" -#include "rcc.h" +#include "drivers/rcc.h" #include "drivers/system.h" -#include "timer.h" -#include "timer_impl.h" +#include "drivers/timer.h" +#include "drivers/timer_impl.h" #define TIM_N(n) (1 << (n)) diff --git a/src/main/vcp/hw_config.c b/src/main/drivers/stm32/vcp/hw_config.c similarity index 100% rename from src/main/vcp/hw_config.c rename to src/main/drivers/stm32/vcp/hw_config.c diff --git a/src/main/vcp/hw_config.h b/src/main/drivers/stm32/vcp/hw_config.h similarity index 100% rename from src/main/vcp/hw_config.h rename to src/main/drivers/stm32/vcp/hw_config.h diff --git a/src/main/vcp/platform_config.h b/src/main/drivers/stm32/vcp/platform_config.h similarity index 100% rename from src/main/vcp/platform_config.h rename to src/main/drivers/stm32/vcp/platform_config.h diff --git a/src/main/vcp/stm32_it.c b/src/main/drivers/stm32/vcp/stm32_it.c similarity index 100% rename from src/main/vcp/stm32_it.c rename to src/main/drivers/stm32/vcp/stm32_it.c diff --git a/src/main/vcp/stm32_it.h b/src/main/drivers/stm32/vcp/stm32_it.h similarity index 100% rename from src/main/vcp/stm32_it.h rename to src/main/drivers/stm32/vcp/stm32_it.h diff --git a/src/main/vcp/usb_conf.h b/src/main/drivers/stm32/vcp/usb_conf.h similarity index 100% rename from src/main/vcp/usb_conf.h rename to src/main/drivers/stm32/vcp/usb_conf.h diff --git a/src/main/vcp/usb_desc.c b/src/main/drivers/stm32/vcp/usb_desc.c similarity index 100% rename from src/main/vcp/usb_desc.c rename to src/main/drivers/stm32/vcp/usb_desc.c diff --git a/src/main/vcp/usb_desc.h b/src/main/drivers/stm32/vcp/usb_desc.h similarity index 100% rename from src/main/vcp/usb_desc.h rename to src/main/drivers/stm32/vcp/usb_desc.h diff --git a/src/main/vcp/usb_endp.c b/src/main/drivers/stm32/vcp/usb_endp.c similarity index 100% rename from src/main/vcp/usb_endp.c rename to src/main/drivers/stm32/vcp/usb_endp.c diff --git a/src/main/vcp/usb_istr.c b/src/main/drivers/stm32/vcp/usb_istr.c similarity index 100% rename from src/main/vcp/usb_istr.c rename to src/main/drivers/stm32/vcp/usb_istr.c diff --git a/src/main/vcp/usb_istr.h b/src/main/drivers/stm32/vcp/usb_istr.h similarity index 100% rename from src/main/vcp/usb_istr.h rename to src/main/drivers/stm32/vcp/usb_istr.h diff --git a/src/main/vcp/usb_prop.c b/src/main/drivers/stm32/vcp/usb_prop.c similarity index 100% rename from src/main/vcp/usb_prop.c rename to src/main/drivers/stm32/vcp/usb_prop.c diff --git a/src/main/vcp/usb_prop.h b/src/main/drivers/stm32/vcp/usb_prop.h similarity index 100% rename from src/main/vcp/usb_prop.h rename to src/main/drivers/stm32/vcp/usb_prop.h diff --git a/src/main/vcp/usb_pwr.c b/src/main/drivers/stm32/vcp/usb_pwr.c similarity index 100% rename from src/main/vcp/usb_pwr.c rename to src/main/drivers/stm32/vcp/usb_pwr.c diff --git a/src/main/vcp/usb_pwr.h b/src/main/drivers/stm32/vcp/usb_pwr.h similarity index 100% rename from src/main/vcp/usb_pwr.h rename to src/main/drivers/stm32/vcp/usb_pwr.h diff --git a/src/main/drivers/stm32/vcp_hal/usbd_cdc_hid.c b/src/main/drivers/stm32/vcp_hal/usbd_cdc_hid.c new file mode 100644 index 0000000000..fc45b10067 --- /dev/null +++ b/src/main/drivers/stm32/vcp_hal/usbd_cdc_hid.c @@ -0,0 +1,344 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + * + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include "platform.h" + +#ifdef USE_USB_CDC_HID + +#include "usbd_conf.h" +#include "usbd_desc.h" +#include "usbd_ctlreq.h" +#include "usbd_def.h" + +#include "usbd_cdc.h" +#include "usbd_hid.h" + +#include "drivers/serial_usb_vcp.h" +#include "usbd_hid.h" +#include "drivers/stm32/vcp_hal/usbd_cdc_interface.h" + +#define USB_HID_CDC_CONFIG_DESC_SIZ (USB_HID_CONFIG_DESC_SIZ - 9 + USB_CDC_CONFIG_DESC_SIZ + 8) + +#define HID_INTERFACE 0x0 +#define HID_POOLING_INTERVAL 0x0A // 10ms - 100Hz update rate + +#define CDC_COM_INTERFACE 0x1 + +#define USBD_VID 0x0483 +#define USBD_PID 0x3256 + +__ALIGN_BEGIN uint8_t USBD_HID_CDC_DeviceDescriptor[USB_LEN_DEV_DESC] __ALIGN_END = +{ + 0x12, /*bLength */ + USB_DESC_TYPE_DEVICE, /*bDescriptorType*/ + 0x00, 0x02, /*bcdUSB */ + 0xEF, /*bDeviceClass*/ + 0x02, /*bDeviceSubClass*/ + 0x01, /*bDeviceProtocol*/ + USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID), + HIBYTE(USBD_PID), /*idProduct*/ + 0x00, 0x02, /*bcdDevice rel. 2.00*/ + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_MAX_NUM_CONFIGURATION /*bNumConfigurations*/ +}; + +__ALIGN_BEGIN static uint8_t USBD_HID_CDC_CfgDesc[USB_HID_CDC_CONFIG_DESC_SIZ] __ALIGN_END = +{ + 0x09, /* bLength: Configuration Descriptor size */ + USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */ + USB_HID_CDC_CONFIG_DESC_SIZ, + /* wTotalLength: Bytes returned */ + 0x00, + 0x03, /*bNumInterfaces: 2 interfaces (1 for CDC, 1 for HID)*/ + 0x01, /*bConfigurationValue: Configuration value*/ + 0x00, /*iConfiguration: Index of string descriptor describing + the configuration*/ + 0xC0, /*bmAttributes: bus powered and Support Remote Wake-up */ + 0x32, /*MaxPower 100 mA: this current is used for detecting Vbus*/ + + /************** Descriptor of Joystick Mouse interface ****************/ + /* 09 */ + 0x09, /*bLength: Interface Descriptor size*/ + USB_DESC_TYPE_INTERFACE, /*bDescriptorType: Interface descriptor type*/ + HID_INTERFACE, /*bInterfaceNumber: Number of Interface*/ + 0x00, /*bAlternateSetting: Alternate setting*/ + 0x01, /*bNumEndpoints*/ + 0x03, /*bInterfaceClass: HID*/ + 0x00, /*bInterfaceSubClass : 1=BOOT, 0=no boot*/ + 0x00, /*nInterfaceProtocol : 0=none, 1=keyboard, 2=mouse*/ + 0, /*iInterface: Index of string descriptor*/ + /******************** Descriptor of Joystick Mouse HID ********************/ + /* 18 */ + 0x09, /*bLength: HID Descriptor size*/ + HID_DESCRIPTOR_TYPE, /*bDescriptorType: HID*/ + 0x11, /*bcdHID: HID Class Spec release number*/ + 0x01, + 0x00, /*bCountryCode: Hardware target country*/ + 0x01, /*bNumDescriptors: Number of HID class descriptors to follow*/ + 0x22, /*bDescriptorType*/ + HID_MOUSE_REPORT_DESC_SIZE, /*wItemLength: Total length of Report descriptor*/ + 0x00, + /******************** Descriptor of Mouse endpoint ********************/ + /* 27 */ + 0x07, /*bLength: Endpoint Descriptor size*/ + USB_DESC_TYPE_ENDPOINT, /*bDescriptorType:*/ + + HID_EPIN_ADDR, /*bEndpointAddress: Endpoint Address (IN)*/ + 0x03, /*bmAttributes: Interrupt endpoint*/ + HID_EPIN_SIZE, /*wMaxPacketSize: 8 Byte max */ + 0x00, + HID_POOLING_INTERVAL, /*bInterval: Polling Interval (10 ms)*/ + /* 34 */ + + /******** /IAD should be positioned just before the CDC interfaces ****** + IAD to associate the two CDC interfaces */ + + 0x08, /* bLength */ + 0x0B, /* bDescriptorType */ + 0x01, /* bFirstInterface */ + 0x02, /* bInterfaceCount */ + 0x02, /* bFunctionClass */ + 0x02, /* bFunctionSubClass */ + 0x01, /* bFunctionProtocol */ + 0x00, /* iFunction (Index of string descriptor describing this function) */ + + /*Interface Descriptor */ + 0x09, /* bLength: Interface Descriptor size */ + USB_DESC_TYPE_INTERFACE, /* bDescriptorType: Interface */ + /* Interface descriptor type */ + CDC_COM_INTERFACE, /* bInterfaceNumber: Number of Interface */ + 0x00, /* bAlternateSetting: Alternate setting */ + 0x01, /* bNumEndpoints: One endpoints used */ + 0x02, /* bInterfaceClass: Communication Interface Class */ + 0x02, /* bInterfaceSubClass: Abstract Control Model */ + 0x01, /* bInterfaceProtocol: Common AT commands */ + 0x00, /* iInterface: */ + + /*Header Functional Descriptor*/ + 0x05, /* bLength: Endpoint Descriptor size */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x00, /* bDescriptorSubtype: Header Func Desc */ + 0x10, /* bcdCDC: spec release number */ + 0x01, + + /*Call Management Functional Descriptor*/ + 0x05, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x01, /* bDescriptorSubtype: Call Management Func Desc */ + 0x00, /* bmCapabilities: D0+D1 */ + 0x02, /* bDataInterface: 2 */ + + /*ACM Functional Descriptor*/ + 0x04, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x02, /* bDescriptorSubtype: Abstract Control Management desc */ + 0x02, /* bmCapabilities */ + + /*Union Functional Descriptor*/ + 0x05, /* bFunctionLength */ + 0x24, /* bDescriptorType: CS_INTERFACE */ + 0x06, /* bDescriptorSubtype: Union func desc */ + 0x01, /* bMasterInterface: Communication class interface */ + 0x02, /* bSlaveInterface0: Data Class Interface */ + + /*Endpoint 2 Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_CMD_EP, /* bEndpointAddress */ + 0x03, /* bmAttributes: Interrupt */ + LOBYTE(CDC_CMD_PACKET_SIZE), /* wMaxPacketSize: */ + HIBYTE(CDC_CMD_PACKET_SIZE), + 0xFF, /* bInterval: */ + + /*---------------------------------------------------------------------------*/ + + /*Data class interface descriptor*/ + 0x09, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_INTERFACE, /* bDescriptorType: */ + 0x02, /* bInterfaceNumber: Number of Interface */ + 0x00, /* bAlternateSetting: Alternate setting */ + 0x02, /* bNumEndpoints: Two endpoints used */ + 0x0A, /* bInterfaceClass: CDC */ + 0x00, /* bInterfaceSubClass: */ + 0x00, /* bInterfaceProtocol: */ + 0x00, /* iInterface: */ + + /*Endpoint OUT Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_OUT_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes: Bulk */ + LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize: */ + HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), + 0x00, /* bInterval: ignore for Bulk transfer */ + + /*Endpoint IN Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ + CDC_IN_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes: Bulk */ + LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize: */ + HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), + 0x00, /* bInterval */ +}; + +/* USB Standard Device Descriptor */ +__ALIGN_BEGIN static uint8_t USBD_HID_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = +{ + USB_LEN_DEV_QUALIFIER_DESC, + USB_DESC_TYPE_DEVICE_QUALIFIER, + 0x00, + 0x02, + 0x00, + 0x00, + 0x00, + 0x40, + 0x01, + 0x00, +}; + +/* Wrapper related callbacks */ +static uint8_t USBD_HID_CDC_Init (USBD_HandleTypeDef *pdev, uint8_t cfgidx); +static uint8_t USBD_HID_CDC_DeInit (USBD_HandleTypeDef *pdev, uint8_t cfgidx); + +/* Control Endpoints*/ +static uint8_t USBD_HID_CDC_Setup (USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); +static uint8_t USBD_HID_CDC_EP0_RxReady (USBD_HandleTypeDef *pdev); + +/* Class Specific Endpoints*/ +static uint8_t USBD_HID_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum); +static uint8_t USBD_HID_CDC_DataOut (USBD_HandleTypeDef *pdev, uint8_t epnum); + +static uint8_t *USBD_HID_CDC_GetFSCfgDesc (uint16_t *length); +uint8_t *USBD_HID_CDC_GetDeviceQualifierDescriptor (uint16_t *length); //Will be NULL Callback because it's unused + +/* CDC interface class callbacks structure */ +USBD_ClassTypeDef USBD_HID_CDC = +{ + USBD_HID_CDC_Init, + USBD_HID_CDC_DeInit, + USBD_HID_CDC_Setup, + NULL, /* EP0_TxSent, */ + USBD_HID_CDC_EP0_RxReady, + USBD_HID_CDC_DataIn, + USBD_HID_CDC_DataOut, + NULL, + NULL, + NULL, + NULL, + USBD_HID_CDC_GetFSCfgDesc, + NULL, + USBD_HID_CDC_GetDeviceQualifierDescriptor, +}; + +static uint8_t USBD_HID_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + //Init CDC + USBD_CDC.Init(pdev, cfgidx); + + //Init HID + USBD_HID.Init(pdev, cfgidx); + + return USBD_OK; +} + +static uint8_t USBD_HID_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx) +{ + //DeInit CDC + USBD_CDC.DeInit(pdev, cfgidx); + + //DeInit HID + USBD_HID.DeInit(pdev, cfgidx); + + return USBD_OK; +} + +static uint8_t USBD_HID_CDC_Setup(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) +{ + switch(req->bmRequest & USB_REQ_RECIPIENT_MASK) { + case USB_REQ_RECIPIENT_INTERFACE: + if (req->wIndex == HID_INTERFACE) { + return USBD_HID.Setup(pdev, req); + } else { + return USBD_CDC.Setup(pdev, req); + } + break; + case USB_REQ_RECIPIENT_ENDPOINT: + if (req->wIndex == HID_EPIN_ADDR) { + return USBD_HID.Setup(pdev, req); + } else { + return USBD_CDC.Setup(pdev, req); + } + break; + } + + return USBD_OK; +} + +static uint8_t USBD_HID_CDC_EP0_RxReady (USBD_HandleTypeDef *pdev) +{ + return (USBD_CDC.EP0_RxReady(pdev)); +} + +static uint8_t USBD_HID_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum) +{ + if (epnum == (CDC_IN_EP &~ 0x80)) { + return USBD_CDC.DataIn(pdev, epnum); + } else { + return USBD_HID.DataIn(pdev, epnum); + } + + return USBD_OK; +} + +static uint8_t USBD_HID_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum) +{ + if (epnum == (CDC_OUT_EP &~ 0x80)) { + return (USBD_CDC.DataOut(pdev, epnum)); + } + + return USBD_OK; +} + +static uint8_t *USBD_HID_CDC_GetFSCfgDesc (uint16_t *length) +{ + *length = sizeof(USBD_HID_CDC_CfgDesc); + return USBD_HID_CDC_CfgDesc; +} + +uint8_t *USBD_HID_CDC_GetDeviceQualifierDescriptor (uint16_t *length) +{ + *length = sizeof(USBD_HID_CDC_DeviceQualifierDesc); + return USBD_HID_CDC_DeviceQualifierDesc; +} + +void sendReport(uint8_t *report, uint8_t len) +{ + USBD_HID_SendReport(&USBD_Device, report, len); +} + +#endif diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/drivers/stm32/vcp_hal/usbd_cdc_interface.c similarity index 100% rename from src/main/vcp_hal/usbd_cdc_interface.c rename to src/main/drivers/stm32/vcp_hal/usbd_cdc_interface.c diff --git a/src/main/vcp_hal/usbd_cdc_interface.h b/src/main/drivers/stm32/vcp_hal/usbd_cdc_interface.h similarity index 100% rename from src/main/vcp_hal/usbd_cdc_interface.h rename to src/main/drivers/stm32/vcp_hal/usbd_cdc_interface.h diff --git a/src/main/vcp_hal/usbd_conf.h b/src/main/drivers/stm32/vcp_hal/usbd_conf.h similarity index 100% rename from src/main/vcp_hal/usbd_conf.h rename to src/main/drivers/stm32/vcp_hal/usbd_conf.h diff --git a/src/main/vcp_hal/usbd_conf_stm32f7xx.c b/src/main/drivers/stm32/vcp_hal/usbd_conf_stm32f7xx.c similarity index 100% rename from src/main/vcp_hal/usbd_conf_stm32f7xx.c rename to src/main/drivers/stm32/vcp_hal/usbd_conf_stm32f7xx.c diff --git a/src/main/vcp_hal/usbd_conf_stm32g4xx.c b/src/main/drivers/stm32/vcp_hal/usbd_conf_stm32g4xx.c similarity index 100% rename from src/main/vcp_hal/usbd_conf_stm32g4xx.c rename to src/main/drivers/stm32/vcp_hal/usbd_conf_stm32g4xx.c diff --git a/src/main/vcp_hal/usbd_conf_stm32h7xx.c b/src/main/drivers/stm32/vcp_hal/usbd_conf_stm32h7xx.c similarity index 100% rename from src/main/vcp_hal/usbd_conf_stm32h7xx.c rename to src/main/drivers/stm32/vcp_hal/usbd_conf_stm32h7xx.c diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/drivers/stm32/vcp_hal/usbd_desc.c similarity index 100% rename from src/main/vcp_hal/usbd_desc.c rename to src/main/drivers/stm32/vcp_hal/usbd_desc.c diff --git a/src/main/vcp_hal/usbd_desc.h b/src/main/drivers/stm32/vcp_hal/usbd_desc.h similarity index 100% rename from src/main/vcp_hal/usbd_desc.h rename to src/main/drivers/stm32/vcp_hal/usbd_desc.h diff --git a/src/main/vcpf4/stm32f4xx_it.c b/src/main/drivers/stm32/vcpf4/stm32f4xx_it.c similarity index 100% rename from src/main/vcpf4/stm32f4xx_it.c rename to src/main/drivers/stm32/vcpf4/stm32f4xx_it.c diff --git a/src/main/vcpf4/stm32f4xx_it.h b/src/main/drivers/stm32/vcpf4/stm32f4xx_it.h similarity index 100% rename from src/main/vcpf4/stm32f4xx_it.h rename to src/main/drivers/stm32/vcpf4/stm32f4xx_it.h diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/drivers/stm32/vcpf4/usb_bsp.c similarity index 100% rename from src/main/vcpf4/usb_bsp.c rename to src/main/drivers/stm32/vcpf4/usb_bsp.c diff --git a/src/main/drivers/stm32/vcpf4/usb_cdc_hid.c b/src/main/drivers/stm32/vcpf4/usb_cdc_hid.c new file mode 100644 index 0000000000..c0dc35f98d --- /dev/null +++ b/src/main/drivers/stm32/vcpf4/usb_cdc_hid.c @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + + +#include + +#include "platform.h" + +#ifdef USE_USB_CDC_HID + +#include "drivers/stm32/vcpf4/usbd_cdc_vcp.h" +#include "usbd_hid_core.h" + +void sendReport(uint8_t *report, uint8_t len) +{ + USBD_HID_SendReport(&USB_OTG_dev, report, len); +} + +#endif diff --git a/src/main/vcpf4/usb_conf.h b/src/main/drivers/stm32/vcpf4/usb_conf.h similarity index 100% rename from src/main/vcpf4/usb_conf.h rename to src/main/drivers/stm32/vcpf4/usb_conf.h diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/drivers/stm32/vcpf4/usbd_cdc_vcp.c similarity index 100% rename from src/main/vcpf4/usbd_cdc_vcp.c rename to src/main/drivers/stm32/vcpf4/usbd_cdc_vcp.c diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/drivers/stm32/vcpf4/usbd_cdc_vcp.h similarity index 100% rename from src/main/vcpf4/usbd_cdc_vcp.h rename to src/main/drivers/stm32/vcpf4/usbd_cdc_vcp.h diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/drivers/stm32/vcpf4/usbd_conf.h similarity index 100% rename from src/main/vcpf4/usbd_conf.h rename to src/main/drivers/stm32/vcpf4/usbd_conf.h diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/drivers/stm32/vcpf4/usbd_desc.c similarity index 100% rename from src/main/vcpf4/usbd_desc.c rename to src/main/drivers/stm32/vcpf4/usbd_desc.c diff --git a/src/main/vcpf4/usbd_desc.h b/src/main/drivers/stm32/vcpf4/usbd_desc.h similarity index 100% rename from src/main/vcpf4/usbd_desc.h rename to src/main/drivers/stm32/vcpf4/usbd_desc.h diff --git a/src/main/vcpf4/usbd_usr.c b/src/main/drivers/stm32/vcpf4/usbd_usr.c similarity index 100% rename from src/main/vcpf4/usbd_usr.c rename to src/main/drivers/stm32/vcpf4/usbd_usr.c diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 7da8fb7421..7fcd6c7538 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -103,14 +103,7 @@ #endif #ifdef USE_USB_CDC_HID -//TODO: Make it platform independent in the future -#ifdef STM32F4 -#include "vcpf4/usbd_cdc_vcp.h" -#include "usbd_hid_core.h" -#elif defined(STM32F7) -#include "usbd_cdc_interface.h" -#include "usbd_hid.h" -#endif +#include "io/usb_cdc_hid.h" #endif #include "tasks.h" diff --git a/src/main/io/usb_cdc_hid.c b/src/main/io/usb_cdc_hid.c index ae6942fdaa..0af76b6b9f 100644 --- a/src/main/io/usb_cdc_hid.c +++ b/src/main/io/usb_cdc_hid.c @@ -26,24 +26,11 @@ #ifdef USE_USB_CDC_HID #include "common/maths.h" - #include "fc/rc_controls.h" - #include "rx/rx.h" - #include "pg/usb.h" - #include "sensors/battery.h" - -//TODO: Make it platform independent in the future -#if defined(STM32F4) -#include "vcpf4/usbd_cdc_vcp.h" -#include "usbd_hid_core.h" -#elif defined(STM32F7) || defined(STM32H7) || defined(STM32G4) -#include "drivers/serial_usb_vcp.h" -#include "usbd_hid.h" -#include "vcp_hal/usbd_cdc_interface.h" -#endif +#include "usb_cdc_hid.h" #define USB_CDC_HID_NUM_AXES 8 #define USB_CDC_HID_NUM_BUTTONS 8 @@ -94,13 +81,8 @@ void sendRcDataToHid(void) report[8] |= (1 << i); } } -#if defined(STM32F4) - USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report)); -#elif defined(STM32F7) || defined(STM32H7) || defined(STM32G4) - USBD_HID_SendReport(&USBD_Device, (uint8_t*)report, sizeof(report)); -#else -# error "MCU does not support USB HID." -#endif + + sendReport((uint8_t*)report, sizeof(report)); } bool cdcDeviceIsMayBeActive(void) diff --git a/src/main/io/usb_cdc_hid.h b/src/main/io/usb_cdc_hid.h index 38261701a0..ca65b8cffc 100644 --- a/src/main/io/usb_cdc_hid.h +++ b/src/main/io/usb_cdc_hid.h @@ -22,3 +22,5 @@ void sendRcDataToHid(void); bool cdcDeviceIsMayBeActive(); +void sendReport(uint8_t *report, uint8_t len); +uint8_t usbIsConnected(void); diff --git a/src/main/target/AT32F435/target.h b/src/main/target/AT32F435/target.h index 014b67795e..230d26101d 100644 --- a/src/main/target/AT32F435/target.h +++ b/src/main/target/AT32F435/target.h @@ -59,12 +59,12 @@ #define SPI_FULL_RECONFIGURABILITY //#define USE_USB_DETECT -//#define USE_VCP +#define USE_VCP //#define USE_SOFTSERIAL1 //#define USE_SOFTSERIAL2 -#define UNIFIED_SERIAL_PORT_COUNT 0 +#define UNIFIED_SERIAL_PORT_COUNT 1 //#define USE_ADC @@ -103,3 +103,5 @@ // 2K page sectors #define FLASH_PAGE_SIZE ((uint32_t)0x0800) #endif + +#define USE_EXTI diff --git a/src/main/target/AT32F435/target.mk b/src/main/target/AT32F435/target.mk index d8c917d59c..f207c7a4ac 100644 --- a/src/main/target/AT32F435/target.mk +++ b/src/main/target/AT32F435/target.mk @@ -5,7 +5,10 @@ DEVICE_FLAGS = -DAT32F435ZMT7 TARGET_MCU_FAMILY := AT32F4 STDPERIPH_DIR = $(ROOT)/lib/main/AT32F43x/drivers -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) +STDPERIPH_SRC = \ + $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) \ + $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) \ + EXCLUDES = at32f435_437_dvp.c \ at32f435_437_can.c \ at32f435_437_xmc.c \ @@ -27,15 +30,14 @@ DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) LD_SCRIPT = $(LINKER_DIR)/at32_flash_f43xM.ld ARCH_FLAGS = -std=c99 -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion -DEVICE_FLAGS += -DUSE_ATBSP_DRIVER -DAT32F43x -DHSE_VALUE=$(HSE_VALUE) -DAT32 +DEVICE_FLAGS += -DUSE_ATBSP_DRIVER -DAT32F43x -DHSE_VALUE=$(HSE_VALUE) -DAT32 -DUSE_OTG_HOST_MODE MCU_COMMON_SRC = \ $(addprefix startup/at32/,$(notdir $(wildcard $(SRC_DIR)/startup/at32/*.c))) \ $(addprefix drivers/at32/,$(notdir $(wildcard $(SRC_DIR)/drivers/at32/*.c))) -MCU_EXCLUDES = \ - drivers/bus_i2c.c \ - drivers/timer.c \ - drivers/persistent.c \ - drivers/pwm_output.c +MCU_EXCLUDES = +VCP_SRC = \ + $(addprefix drivers/at32/usb/,$(notdir $(wildcard $(SRC_DIR)/drivers/at32/usb/*.c))) \ + drivers/usb_io.c diff --git a/src/main/target/STM32F405/target.h b/src/main/target/STM32F405/target.h index 684c5da42b..6eaa5dde07 100644 --- a/src/main/target/STM32F405/target.h +++ b/src/main/target/STM32F405/target.h @@ -76,4 +76,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS - +#define USE_EXTI diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h index 06f7467cac..0f3e152245 100644 --- a/src/main/target/STM32F411/target.h +++ b/src/main/target/STM32F411/target.h @@ -73,3 +73,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS +#define USE_EXTI diff --git a/src/main/target/STM32F745/target.h b/src/main/target/STM32F745/target.h index 12a4cef7e6..7c74c7f4f6 100644 --- a/src/main/target/STM32F745/target.h +++ b/src/main/target/STM32F745/target.h @@ -83,3 +83,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS +#define USE_EXTI diff --git a/src/main/target/STM32F7X2/target.h b/src/main/target/STM32F7X2/target.h index e271d589a6..d451818d0e 100644 --- a/src/main/target/STM32F7X2/target.h +++ b/src/main/target/STM32F7X2/target.h @@ -75,3 +75,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS +#define USE_EXTI diff --git a/src/main/target/STM32G47X/target.h b/src/main/target/STM32G47X/target.h index d0787af54c..8f539115fd 100644 --- a/src/main/target/STM32G47X/target.h +++ b/src/main/target/STM32G47X/target.h @@ -73,3 +73,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS +#define USE_EXTI diff --git a/src/main/target/STM32H723/target.h b/src/main/target/STM32H723/target.h index 3283a66969..2d62ff86b5 100644 --- a/src/main/target/STM32H723/target.h +++ b/src/main/target/STM32H723/target.h @@ -95,3 +95,5 @@ #if !defined(USE_EXST) #define USE_CUSTOM_DEFAULTS #endif + +#define USE_EXTI diff --git a/src/main/target/STM32H730/target.h b/src/main/target/STM32H730/target.h index 23a1a74b0c..c2f8738260 100644 --- a/src/main/target/STM32H730/target.h +++ b/src/main/target/STM32H730/target.h @@ -111,3 +111,5 @@ #ifdef USE_EXST #define USE_CUSTOM_DEFAULTS #endif + +#define USE_EXTI diff --git a/src/main/target/STM32H743/target.h b/src/main/target/STM32H743/target.h index ee6d3c0ed3..da8c343f07 100644 --- a/src/main/target/STM32H743/target.h +++ b/src/main/target/STM32H743/target.h @@ -83,3 +83,4 @@ #define USE_ADC #define USE_CUSTOM_DEFAULTS +#define USE_EXTI diff --git a/src/main/target/STM32H750/target.h b/src/main/target/STM32H750/target.h index 2b40d8a37a..d53ce23870 100644 --- a/src/main/target/STM32H750/target.h +++ b/src/main/target/STM32H750/target.h @@ -119,3 +119,4 @@ #define USE_CUSTOM_DEFAULTS #endif +#define USE_EXTI diff --git a/src/main/vcp_hal/usbd_cdc_hid.c b/src/main/vcp_hal/usbd_cdc_hid.c deleted file mode 100644 index 875e0ab9de..0000000000 --- a/src/main/vcp_hal/usbd_cdc_hid.c +++ /dev/null @@ -1,336 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - * - * Author: Chris Hockuba (https://github.com/conkerkh) - * - */ - -#include "platform.h" - -#ifdef USE_USB_CDC_HID - -#include "usbd_conf.h" -#include "usbd_desc.h" -#include "usbd_ctlreq.h" -#include "usbd_def.h" - -#include "usbd_cdc.h" -#include "usbd_hid.h" - -#define USB_HID_CDC_CONFIG_DESC_SIZ (USB_HID_CONFIG_DESC_SIZ - 9 + USB_CDC_CONFIG_DESC_SIZ + 8) - -#define HID_INTERFACE 0x0 -#define HID_POOLING_INTERVAL 0x0A // 10ms - 100Hz update rate - -#define CDC_COM_INTERFACE 0x1 - -#define USBD_VID 0x0483 -#define USBD_PID 0x3256 - -__ALIGN_BEGIN uint8_t USBD_HID_CDC_DeviceDescriptor[USB_LEN_DEV_DESC] __ALIGN_END = -{ - 0x12, /*bLength */ - USB_DESC_TYPE_DEVICE, /*bDescriptorType*/ - 0x00, 0x02, /*bcdUSB */ - 0xEF, /*bDeviceClass*/ - 0x02, /*bDeviceSubClass*/ - 0x01, /*bDeviceProtocol*/ - USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ - LOBYTE(USBD_VID), HIBYTE(USBD_VID), /*idVendor*/ - LOBYTE(USBD_PID), - HIBYTE(USBD_PID), /*idProduct*/ - 0x00, 0x02, /*bcdDevice rel. 2.00*/ - USBD_IDX_MFC_STR, /*Index of manufacturer string*/ - USBD_IDX_PRODUCT_STR, /*Index of product string*/ - USBD_IDX_SERIAL_STR, /*Index of serial number string*/ - USBD_MAX_NUM_CONFIGURATION /*bNumConfigurations*/ -}; - -__ALIGN_BEGIN static uint8_t USBD_HID_CDC_CfgDesc[USB_HID_CDC_CONFIG_DESC_SIZ] __ALIGN_END = -{ - 0x09, /* bLength: Configuration Descriptor size */ - USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */ - USB_HID_CDC_CONFIG_DESC_SIZ, - /* wTotalLength: Bytes returned */ - 0x00, - 0x03, /*bNumInterfaces: 2 interfaces (1 for CDC, 1 for HID)*/ - 0x01, /*bConfigurationValue: Configuration value*/ - 0x00, /*iConfiguration: Index of string descriptor describing - the configuration*/ - 0xC0, /*bmAttributes: bus powered and Support Remote Wake-up */ - 0x32, /*MaxPower 100 mA: this current is used for detecting Vbus*/ - - /************** Descriptor of Joystick Mouse interface ****************/ - /* 09 */ - 0x09, /*bLength: Interface Descriptor size*/ - USB_DESC_TYPE_INTERFACE, /*bDescriptorType: Interface descriptor type*/ - HID_INTERFACE, /*bInterfaceNumber: Number of Interface*/ - 0x00, /*bAlternateSetting: Alternate setting*/ - 0x01, /*bNumEndpoints*/ - 0x03, /*bInterfaceClass: HID*/ - 0x00, /*bInterfaceSubClass : 1=BOOT, 0=no boot*/ - 0x00, /*nInterfaceProtocol : 0=none, 1=keyboard, 2=mouse*/ - 0, /*iInterface: Index of string descriptor*/ - /******************** Descriptor of Joystick Mouse HID ********************/ - /* 18 */ - 0x09, /*bLength: HID Descriptor size*/ - HID_DESCRIPTOR_TYPE, /*bDescriptorType: HID*/ - 0x11, /*bcdHID: HID Class Spec release number*/ - 0x01, - 0x00, /*bCountryCode: Hardware target country*/ - 0x01, /*bNumDescriptors: Number of HID class descriptors to follow*/ - 0x22, /*bDescriptorType*/ - HID_MOUSE_REPORT_DESC_SIZE, /*wItemLength: Total length of Report descriptor*/ - 0x00, - /******************** Descriptor of Mouse endpoint ********************/ - /* 27 */ - 0x07, /*bLength: Endpoint Descriptor size*/ - USB_DESC_TYPE_ENDPOINT, /*bDescriptorType:*/ - - HID_EPIN_ADDR, /*bEndpointAddress: Endpoint Address (IN)*/ - 0x03, /*bmAttributes: Interrupt endpoint*/ - HID_EPIN_SIZE, /*wMaxPacketSize: 8 Byte max */ - 0x00, - HID_POOLING_INTERVAL, /*bInterval: Polling Interval (10 ms)*/ - /* 34 */ - - /******** /IAD should be positioned just before the CDC interfaces ****** - IAD to associate the two CDC interfaces */ - - 0x08, /* bLength */ - 0x0B, /* bDescriptorType */ - 0x01, /* bFirstInterface */ - 0x02, /* bInterfaceCount */ - 0x02, /* bFunctionClass */ - 0x02, /* bFunctionSubClass */ - 0x01, /* bFunctionProtocol */ - 0x00, /* iFunction (Index of string descriptor describing this function) */ - - /*Interface Descriptor */ - 0x09, /* bLength: Interface Descriptor size */ - USB_DESC_TYPE_INTERFACE, /* bDescriptorType: Interface */ - /* Interface descriptor type */ - CDC_COM_INTERFACE, /* bInterfaceNumber: Number of Interface */ - 0x00, /* bAlternateSetting: Alternate setting */ - 0x01, /* bNumEndpoints: One endpoints used */ - 0x02, /* bInterfaceClass: Communication Interface Class */ - 0x02, /* bInterfaceSubClass: Abstract Control Model */ - 0x01, /* bInterfaceProtocol: Common AT commands */ - 0x00, /* iInterface: */ - - /*Header Functional Descriptor*/ - 0x05, /* bLength: Endpoint Descriptor size */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x00, /* bDescriptorSubtype: Header Func Desc */ - 0x10, /* bcdCDC: spec release number */ - 0x01, - - /*Call Management Functional Descriptor*/ - 0x05, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x01, /* bDescriptorSubtype: Call Management Func Desc */ - 0x00, /* bmCapabilities: D0+D1 */ - 0x02, /* bDataInterface: 2 */ - - /*ACM Functional Descriptor*/ - 0x04, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x02, /* bDescriptorSubtype: Abstract Control Management desc */ - 0x02, /* bmCapabilities */ - - /*Union Functional Descriptor*/ - 0x05, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x06, /* bDescriptorSubtype: Union func desc */ - 0x01, /* bMasterInterface: Communication class interface */ - 0x02, /* bSlaveInterface0: Data Class Interface */ - - /*Endpoint 2 Descriptor*/ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_CMD_EP, /* bEndpointAddress */ - 0x03, /* bmAttributes: Interrupt */ - LOBYTE(CDC_CMD_PACKET_SIZE), /* wMaxPacketSize: */ - HIBYTE(CDC_CMD_PACKET_SIZE), - 0xFF, /* bInterval: */ - - /*---------------------------------------------------------------------------*/ - - /*Data class interface descriptor*/ - 0x09, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_INTERFACE, /* bDescriptorType: */ - 0x02, /* bInterfaceNumber: Number of Interface */ - 0x00, /* bAlternateSetting: Alternate setting */ - 0x02, /* bNumEndpoints: Two endpoints used */ - 0x0A, /* bInterfaceClass: CDC */ - 0x00, /* bInterfaceSubClass: */ - 0x00, /* bInterfaceProtocol: */ - 0x00, /* iInterface: */ - - /*Endpoint OUT Descriptor*/ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_OUT_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes: Bulk */ - LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize: */ - HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), - 0x00, /* bInterval: ignore for Bulk transfer */ - - /*Endpoint IN Descriptor*/ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_IN_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes: Bulk */ - LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize: */ - HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), - 0x00, /* bInterval */ -}; - -/* USB Standard Device Descriptor */ -__ALIGN_BEGIN static uint8_t USBD_HID_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = -{ - USB_LEN_DEV_QUALIFIER_DESC, - USB_DESC_TYPE_DEVICE_QUALIFIER, - 0x00, - 0x02, - 0x00, - 0x00, - 0x00, - 0x40, - 0x01, - 0x00, -}; - -/* Wrapper related callbacks */ -static uint8_t USBD_HID_CDC_Init (USBD_HandleTypeDef *pdev, uint8_t cfgidx); -static uint8_t USBD_HID_CDC_DeInit (USBD_HandleTypeDef *pdev, uint8_t cfgidx); - -/* Control Endpoints*/ -static uint8_t USBD_HID_CDC_Setup (USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static uint8_t USBD_HID_CDC_EP0_RxReady (USBD_HandleTypeDef *pdev); - -/* Class Specific Endpoints*/ -static uint8_t USBD_HID_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum); -static uint8_t USBD_HID_CDC_DataOut (USBD_HandleTypeDef *pdev, uint8_t epnum); - -static uint8_t *USBD_HID_CDC_GetFSCfgDesc (uint16_t *length); -uint8_t *USBD_HID_CDC_GetDeviceQualifierDescriptor (uint16_t *length); //Will be NULL Callback because it's unused - -/* CDC interface class callbacks structure */ -USBD_ClassTypeDef USBD_HID_CDC = -{ - USBD_HID_CDC_Init, - USBD_HID_CDC_DeInit, - USBD_HID_CDC_Setup, - NULL, /* EP0_TxSent, */ - USBD_HID_CDC_EP0_RxReady, - USBD_HID_CDC_DataIn, - USBD_HID_CDC_DataOut, - NULL, - NULL, - NULL, - NULL, - USBD_HID_CDC_GetFSCfgDesc, - NULL, - USBD_HID_CDC_GetDeviceQualifierDescriptor, -}; - -static uint8_t USBD_HID_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - //Init CDC - USBD_CDC.Init(pdev, cfgidx); - - //Init HID - USBD_HID.Init(pdev, cfgidx); - - return USBD_OK; -} - -static uint8_t USBD_HID_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - //DeInit CDC - USBD_CDC.DeInit(pdev, cfgidx); - - //DeInit HID - USBD_HID.DeInit(pdev, cfgidx); - - return USBD_OK; -} - -static uint8_t USBD_HID_CDC_Setup(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - switch(req->bmRequest & USB_REQ_RECIPIENT_MASK) { - case USB_REQ_RECIPIENT_INTERFACE: - if (req->wIndex == HID_INTERFACE) { - return USBD_HID.Setup(pdev, req); - } - else { - return USBD_CDC.Setup(pdev, req); - } - break; - case USB_REQ_RECIPIENT_ENDPOINT: - if (req->wIndex == HID_EPIN_ADDR) { - return USBD_HID.Setup(pdev, req); - } else { - return USBD_CDC.Setup(pdev, req); - } - break; - } - - return USBD_OK; -} - -static uint8_t USBD_HID_CDC_EP0_RxReady (USBD_HandleTypeDef *pdev) -{ - return (USBD_CDC.EP0_RxReady(pdev)); -} - -static uint8_t USBD_HID_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum) -{ - if (epnum == (CDC_IN_EP &~ 0x80)) { - return USBD_CDC.DataIn(pdev, epnum); - } - else { - return USBD_HID.DataIn(pdev, epnum); - } - - return USBD_OK; -} - -static uint8_t USBD_HID_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum) -{ - if (epnum == (CDC_OUT_EP &~ 0x80)) { - return (USBD_CDC.DataOut(pdev, epnum)); - } - - return USBD_OK; -} - -static uint8_t *USBD_HID_CDC_GetFSCfgDesc (uint16_t *length) -{ - *length = sizeof(USBD_HID_CDC_CfgDesc); - return USBD_HID_CDC_CfgDesc; -} - -uint8_t *USBD_HID_CDC_GetDeviceQualifierDescriptor (uint16_t *length) -{ - *length = sizeof(USBD_HID_CDC_DeviceQualifierDesc); - return USBD_HID_CDC_DeviceQualifierDesc; -} -#endif